Robofun 機器人論壇

 找回密碼
 申請會員
搜索
熱搜: 活動 交友 discuz
查看: 3772|回復: 4

[請教]I2C 裝置使用 RoboIO LIB 的寫法..(Wii Motion Plus)

[複製鏈接]
發表於 2010-9-9 14:06:01 | 顯示全部樓層 |閱讀模式
本帖最後由 tommylin 於 2010-9-9 17:31 編輯

99.9.9 我又來麻煩老師了~因為是我第一次寫 I2C 的程式,
如果問的太笨, 希望老師不要介意 @@

請問老師~
我要讀取 Wii Motion Plus (任天堂 動態強化感測器,以下簡稱 WMP )

我已經在 Arduino 板上成功讀取 yaw, pitch, roll~
參考網址:
原始出處網站按這裏
依照 Arduino的程式,改成 RoboIO 版程式如下:

  1. unsigned long i2c_clock = 400000L;
  2. unsigned int i2c_address = 0x53; // WMP 的位址

  3. if (i2c_Initialize(I2CIRQ_DISABLE) == false) // 起始設定 I2C
  4. {
  5.         printf("FALSE!!  %s\n", roboio_GetErrMsg());
  6.         return -1;
  7. }
  8. i2c0_SetSpeed(I2CMODE_FAST, i2c_clock); //設定速率

  9. //===== 啟動 WMP 裝置 =====
  10. i2c0master_StartN(i2c_address >> 1,I2C_WRITE,2);//write 2 byte
  11. i2c0master_WriteN(0xfe);
  12. i2c0master_WriteN(0x04); // 根據 RoboIO 手冊說明, 寫入後會自動發送"結束"命令.

  13. //===== 讀取裝置傳回數值byte data[6]; //six data bytes
  14. i2c_address = 0x52; // 根據 Arduino程式指出, 啟動裝置後讀取位址換成 0x52
  15. i2c0master_StartN(i2c_address >> 1,I2C_WRITE,1);// ???
  16. i2c0master_SetRestartN(I2C_READ, 6); // WMP 回傳6個bytes資料.
  17. for ( int i=0; i < 6; i++ ) data [i] = i2c0master_ReadN();
複製代碼


請問這樣寫對嗎?.. 因為讀到的數值都是零..我不確定第18行寫法是否正確?

下列附上 Arduino 版的程式:
  1. #include <Wire.h>
  2. byte data[6]; //six data bytes
  3. int yaw, pitch, roll; //three axes
  4. int yaw0, pitch0, roll0; //calibration zeroes

  5. void wmpOn(){
  6. Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53
  7. Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
  8. Wire.send(0x04);
  9. Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
  10. }

  11. void wmpSendZero(){
  12. Wire.beginTransmission(0x52); //now at address 0x52
  13. Wire.send(0x00); //send zero to signal we want info
  14. Wire.endTransmission();
  15. }

  16. void calibrateZeroes(){
  17. for (int i=0;i<10;i++){
  18. wmpSendZero();
  19. Wire.requestFrom(0x52,6);
  20. for (int i=0;i<6;i++){
  21. data[i]=Wire.receive();
  22. }
  23. yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings
  24. pitch0+=(((data[4]>>2)<<8)+data[1])/10;
  25. roll0+=(((data[5]>>2)<<8)+data[2])/10;
  26. }
  27. Serial.print("Yaw0:");
  28. Serial.print(yaw0);
  29. Serial.print(" Pitch0:");
  30. Serial.print(pitch0);
  31. Serial.print(" Roll0:");
  32. Serial.println(roll0);
  33. }

  34. void receiveData(){
  35. wmpSendZero(); //send zero before each request (same as nunchuck)
  36. Wire.requestFrom(0x52,6); //request the six bytes from the WM+
  37. for (int i=0;i<6;i++){
  38. data[i]=Wire.receive();
  39. }
  40. yaw=((data[3]>>2)<<8)+data[0]-yaw0;
  41. pitch=((data[4]>>2)<<8)+data[1]-pitch0;
  42. roll=((data[5]>>2)<<8)+data[2]-roll0;
  43. }
  44. //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  45. //for info on what each byte represents
  46. void setup(){
  47. Serial.begin(115200);
  48. Serial.println("WM+ tester");
  49. Wire.begin();
  50. wmpOn(); //turn WM+ on
  51. calibrateZeroes(); //calibrate zeroes
  52. delay(1000);
  53. }

  54. void loop(){
  55. receiveData(); //receive data and calculate yaw pitch and roll
  56. Serial.print("yaw:");//see diagram on randomhacksofboredom.blogspot.com
  57. Serial.print(yaw); //for info on which axis is which
  58. Serial.print(" pitch:");
  59. Serial.print(pitch);
  60. Serial.print(" roll:");
  61. Serial.println(roll);
  62. delay(100);
  63. }
複製代碼
發表於 2010-9-9 22:07:48 | 顯示全部樓層
HI

看到這篇我又拿了以前測過的 Motion Plus 起來玩 XD
Wii 的 Moition Plus 的位址 0x53 與 0x52 都是 7 bit, 不用將它右移 1 位 :)
另外, 要讀 WMP 的值時, 要先寫 0x00 給它(此時位址是 0x52) 再讀 6 筆 data 出來, 必須注
意的是, 寫與讀中間不用 restart, 所以是先寫 0x00 後 stop, 再 start 讀 6 筆, 參考以下:

i2c0master_StartN(0x52, I2C_WRITE, 1);
i2c0master_WriteN(0x00);
wait_ms(10); // delay 10 ms
i2c0master_StartN(0x52, I2C_READ, 6);

for(i=0; i< 6; i++) data[i] = i2c0master_ReadN();

試試看吧 :)
 樓主| 發表於 2010-9-9 23:54:33 | 顯示全部樓層
本帖最後由 tommylin 於 2010-9-10 00:23 編輯

老師~ 還是讀不到...~"~ 啟動裝置的部分這樣寫對嗎?... 先說聲謝謝~又來麻煩您了..
我的程式如下:

roboio_SetRBVer(RB_100); // RB-100

if (i2c_Initialize(I2CIRQ_DISABLE) == false)
{
    showmsg ("FALSE!!  %s\n", roboio_GetErrMsg());
    return ;
}
i2c0_SetSpeed(I2CMODE_FAST, 400000L);

//========= 啟動 Wii Motion Plus (WMP)
        //WMP On
        unsigned char i2c_address = 0x53;
        if ( i2c0master_StartN(i2c_address,I2C_WRITE, 1 ) != true )
                showmsg ("FALSE Step 1 - ON device");
        if ( i2c0master_WriteN(0xfe) != true )
                showmsg ("FALSE Step 2 - 0xfe");
        if ( i2c0master_WriteN(0x04) != true )
                showmsg ("FALSE Step 3 - 0x04");

//========= 讀取數據
while(1)
{
        if ( i2c0master_StartN( 0x52, I2C_WRITE, 1) != true )
                showmsg ("FALSE receive_WMP_I2c_Data() Step 1 ");        
        if ( i2c0master_WriteN(0x00) != true ) //Read from  (Address : 0x52)
                showmsg ("FALSE receive_WMP_I2c_Data() Step 2 ");
        wait_ms(10); // 延遲 10 ms

        if ( i2c0master_StartN( 0x52, I2C_READ, 6 ) != true )
                showmsg ("FALSE receive_WMP_I2c_Data() Step 3 ");
        for (int i=0;i<6;i++)
                data= i2c0master_ReadN();//Wire.receive();
}
發表於 2010-9-10 09:41:07 | 顯示全部樓層
HI

啟動的程序是沒錯的, 只是 start 的部分應該改為
    i2c0master_StartN(i2c_address,I2C_WRITE, 2)
因為寫了 2 筆 data (0xfe, 0x04)
 樓主| 發表於 2010-9-10 12:04:54 | 顯示全部樓層
HI

啟動的程序是沒錯的, 只是 start 的部分應該改為
    i2c0master_StartN(i2c_address,I2C_WRITE, 2) ...
acen2008 發表於 2010-9-10 09:41



阿~ 抱歉...真是低級的錯誤..晚上馬上試試...
您需要登錄後才可以回帖 登錄 | 申請會員

本版積分規則

小黑屋|手機版|Archiver|機器人論壇 from 2005.07

GMT+8, 2022-8-11 22:53 , Processed in 0.179996 second(s), 7 queries , Apc On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

快速回復 返回頂部 返回列表