RoBoard魔人的機器人日誌

2011/10/17

[教學] 如何控制RM-G146陀螺儀

RM-G146

 

 
控制G146必須要用I2C去控制
因為我這邊使用的範例為RB110

 
所以沒有特別再去弄線材
首先必須知道的資訊
陀螺儀的位址 在0xD0
以及陀螺儀的XYZ軸的位址
AddrHex   AddrDecimal     Register Name        
1D             29                       GYRO_XOUT_H
1E             30                       GYRO_XOUT_L

 
1F             31                       GYRO_YOUT_H
20             32                       GYRO_YOUT_L

 
21             33                       GYRO_ZOUT_H
22             34                       GYRO_ZOUT_L
這樣就可以用一些基本的指令去讀取陀螺儀得值了

 
而I2C的讀取方法為
| start | slave address | R/W | A | data | A/A |stop |

 
所以基本上會用到的指令有
i2c0master_StartN(slave address,R/W,byte)           告訴某位置要讀或寫N的byte
i2c0master_WriteN(byte)           寫入的值
i2c0master_ReadN()          會回傳一個值

 
在W/R之後會值接回傳STOP

 
所以無須再傳stop

 

 

 


 

 

 

 

 

 

 

 
範例code    用G146控制AX-12馬達:

#include "stdio.h"
#include "conio.h"
#include "windows.h"
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char gyro_addr = 0xD0>>1;

int main()
{
 unsigned int gyro_X, gyro_Y, d;
 unsigned char cmd1[11] = {0xff, 0xff, 0x01, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char cmd2[11] = {0xff, 0xff, 0x02, 0x07, 0x03, 0x1e, 0x00, 0x02, 0x00, 0x02, 0x00};
 unsigned char buf[6] = {0};
 unsigned int i = 0;

 roboio_SetRBVer(RB_110);

 com3_Init(COM_HDUPLEX);
 com3_SetFormat(COM_BYTESIZE8, COM_STOPBIT1, COM_NOPARITY);
 com3_SetBaud(COMBAUD_115200BPS);

    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_AUTO, 400000L);
 i2c0master_StartN(gyro_addr, I2C_WRITE,2);
 i2c0master_WriteN(0x16);
 i2c0master_WriteN(0x18);
 while(i++ < 60){
  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1d);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  gyro_X = i2c0master_ReadN();
  gyro_X = gyro_X << 8;
  
  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1e);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  d = i2c0master_ReadN();
  gyro_X = gyro_X + d;

  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x1f);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  gyro_Y = i2c0master_ReadN();

  gyro_Y = (gyro_Y + 1) &(~256);
  gyro_Y = gyro_Y << 8;

  i2c0master_StartN(gyro_addr, I2C_WRITE,1);
  i2c0master_WriteN(0x20);
  i2c0master_StartN(gyro_addr, I2C_READ,1);
  d = i2c0master_ReadN();
  gyro_Y = gyro_Y + d;
  
  gyro_X /= 0x40;
  if(gyro_X > 0x1ff){
   gyro_X -= 0x200;
   if(cmd1[6] + (gyro_X % 0x100) >= 0x100){
    cmd1[6] = cmd1[6] + (gyro_X % 0x100) - 0x100;
    cmd1[7] = cmd1[7] + 0x01;
   }else{
    cmd1[6] = cmd1[6] + (gyro_X % 0x100);
    cmd1[7] = cmd1[7] + (gyro_X >> 8);
   }
  }else{
   cmd1[6] = cmd1[6] + (gyro_X % 0x100) - 0x1ff; 
   cmd1[7] = cmd1[7] + (gyro_X >> 8) - 0x1ff;
  }

  gyro_Y /= 0x40;
  if(gyro_Y > 0x1ff){
   gyro_Y -= 0x200;
   if(cmd2[6] + (gyro_Y % 0x100) >= 0x100){
    cmd2[6] = cmd2[6] + (gyro_Y % 0x100) - 0x100;
    cmd2[7] = cmd2[7] + 0x01;
   }else{
    cmd2[6] = cmd2[6] + (gyro_Y % 0x100);
    cmd2[7] = cmd2[7] + (gyro_Y >> 8);
   }
  }else{
   cmd2[6] = cmd2[6] + (gyro_Y % 0x100) - 0x1ff; 
   cmd2[7] = cmd2[7] + (gyro_Y >> 8) - 0x1ff;
  }

  cmd1[7] = cmd1[7] >= 0x03 ? 0x03 : cmd1[2];
  cmd1[7] = cmd1[7] < 0x00 ? 0x00 : cmd1[2];
  cmd2[7] = cmd2[7] >= 0x03 ? 0x03 : cmd2[2];
  cmd2[7] = cmd2[7] < 0x00 ? 0x00 : cmd2[2];

  cmd1[10] = ~(cmd1[2] + cmd1[3] + cmd1[4] + cmd1[5] + cmd1[6] + cmd1[7] + cmd1[8] + cmd1[9]);
  cmd2[10] = ~(cmd2[2] + cmd2[3] + cmd2[4] + cmd2[5] + cmd2[6] + cmd2[7] + cmd2[8] + cmd2[9]);
  com3_ServoTRX(cmd1, 11, buf, 6);
  com3_ServoTRX(cmd2, 11, buf, 6);
  _sleep(250);
 }
 i2c_Close();
 com3_Close();
 return 0;
}
Share:

22 則留言:

  1. 赞!
    有没有c#程序把RM-G146的姿态3D显示在电脑屏幕上?

    回覆刪除
  2. 您是說http://www.youtube.com/watch?v=QCdS_JRBxpQ&feature=player_embedded
    像這影片的東西?

    回覆刪除
  3. 你好~我看你讀取角度訊號只讀一個Byte~想請教一下就是陀螺儀本身是16位元的AD訊號~那該怎樣去處理這1Byte的資料才能符合16位元的ad值呢??

    回覆刪除
  4. 這顆陀螺儀是用I²C通訊來傳輸的喔

    回覆刪除
  5. 所以陀螺儀端的I²C是怎麼將16位元的AD值轉成1Byte的資料做發送呢??如果我想讀取16位元這樣高解析的角度計資料~那是不是我要命令主控端I²C連續讀取2個Byte的陀螺儀資料??感謝~

    回覆刪除
  6. I²C傳輸時是用兩條線來做控制SDA和SCL
    而這兩條傳輸出的訊號有高低電位
    而這些電位會讓I²C上面的裝置得知資料內容以及存取位置

    這裡有G146的文件 也有sample code可以參考看看
    http://www.roboard.com/G146.html

    基本上如果你只是想要控制的話 你知道它的address 然後傳輸指令過去你就可以read回需要的資料了

    回覆刪除
  7. 你好~我已經有先由傳輸VCU MASTER端發送了S d0 16 18 p 去設定陀螺儀~接著開始讀取s d0 1d s d1 00 p 但不管我怎麼擺動我的G146感知器資料都是傳00回來~請問有了什麼步驟嗎?

    回覆刪除
  8. 請問你使用的也是RoBoard控制板嘛?
    然後關於控制的部分
    AddrHex AddrDecimal Register Name
    1D 29 GYRO_XOUT_H
    1E 30 GYRO_XOUT_L

    這裡有提到一個是高位元一個是低位元喔
    你必須兩個都取回來才能算出正確的資料內容

    回覆刪除
  9. 我不是使用RoBoard控制板耶~我是用其他的vcu去控的~vcu端要自己先建i2c功能然後再和g146溝通,目前就是照你的範例先傳16和18兩個位元的資料去開啟陀螺儀功能~由於目前我還看不懂暫存器map的設定,所以就先用你的範例去試,於是先抓了1d和1e的x軸資料,但卻都是ffxx(g146朝上平放),低位元的xx值跳動很大,而高位元幾乎都維持在ff,偶爾跳動,這樣的的值好奇怪,是不是在陀螺儀的初使化中少設定了其他暫存器的功能

    回覆刪除
  10. 這樣應該是正確的沒錯吧
    因為高低位元組合起來變成一個16bit的值
    所以低位元變動超過8bit後才會進位到高位元阿
    所以才會有你說的一遍跳動大一邊跳動小的問題

    回覆刪除
  11. 那我想再請教一下~G146晶片TOP面向上然後平放不動的值是大概是多少呢??因為我平放不動G146時,X軸的deg/s的高位元值都是ff有這麼大嗎??然後我旋轉x軸方向,訊號也沒跟著我同步變化

    回覆刪除
  12. 補充一下~我的控制板I2C是用100K傳輸模式和G146溝通的~這樣有問題嗎?

    回覆刪除
  13. 平放時的職我也不太確定 要實際測測看才知道

    至於傳輸速度基本上小於3.3 Mbps應該就沒什麼問題了


    這裡有官方的授課影片供你參考
    http://www.youtube.com/watch?v=KSLEDZP4e_g

    回覆刪除
  14. 更正一下
    3.3 Mbps是RoBoard控制板的

    I2C最大支援是到400K

    然後另外建議去取其它的值試試看

    回覆刪除
  15. 非常感謝影片!!
    我後來在看了一下暫存器MAP,得知默認寫法都為00
    所以我單純只讀取X軸也該會有值的變化對吧~另外想再請教一下魔人大大是否有寫過G146加速度規??
    我最近有寫加速度規~首先傳了默認值S 30 20 07 P啟動XYZ軸的命令~接著就是S 30 29 S 31 1Byte P來接收X軸的資料~結果不管我怎麼沿X軸揮動G146但資料回傳值都是00h...糗

    回覆刪除
  16. 魔人你好 想問問代碼中:

    gyro_Y = (gyro_Y + 1) &(~256);
    gyro_Y = gyro_Y << 8;

    代碼中的第一句是有甚麼用途?

    還有:
    const unsigned char gyro_addr = 0xD0>>1;

    為甚麼地址碼還需要右移一位呢?

    小白一個 煩請魔人解答疑惑 XD

    回覆刪除
  17. 志東你好

    因為G146讀回後會有兩個值
    在datasheet裡面有寫到
    一個是高位元一個是低位元
    所以gyro_Y這裡將兩個組合起來才會得到真正的數值
    至於計算方法 你將他轉為二進位去算看看
    大概就知道他在做什麼了

    接著就是gyro_addr為什麼要右移
    其實也可以不右移的 只是妳的數值要自己調對
    datasheet裡面也有他的address可以去對照看看

    回覆刪除
  18. 謝謝魔人回答,但始終有些問題不太懂:

    第一問 其實gyro_?上的運算我的理解是將高低位的寄存器的值讀取出來,高位的左移8位再加低8位,但問題是y的值對比x,z多了上述一步預處理 又不像是取補碼... 所以煩請魔人再回答

    第二問 我想應該明白了

    感謝魔人解答

    回覆刪除
  19. 志東你好
    你的問題也讓我想了很久 我也查了datasheet
    看起來的確是沒有必要這樣做的
    我想這是我之前在撰寫程式時做測試用的
    但忘記將它移除
    真是抱歉

    接著這裡提醒您一下 在gyro值提出時是unsigned int
    但datasheet裡面有寫到取回的值為二補數
    所以您在使用時必須做些轉換才能拿到正負號

    感謝您的提問

    回覆刪除
  20. 作者已經移除這則留言。

    回覆刪除
  21. 魔人:
    NOTED WITH MANY THANKS...
    大致都明白了 我再試試看
    同時想問問魔人提示在datasheet的哪一版?
    期待你新的教程~!

    回覆刪除
  22. 看Sensor的型號
    像G-146有LSM303DLH和MPU-3050
    google查一下就有囉^^

    回覆刪除

技術提供:Blogger.

追蹤者