极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 154299|回复: 197

arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验

  [复制链接]
发表于 2011-11-25 17:54:25 | 显示全部楼层 |阅读模式
姿态识别应用范围很广,像自平衡车呀,飞行器呀,双足机器人呀之类。本次我们使用Arduino+ITG3205+ADXL345做姿态检测,使用Processing作为输出,实时显示姿态。

本次实验使用的ITG3205与ADXL345都是成品模块,都可以使用I2C接口进行连接。

先看硬件连接,模拟5号口连接I2C模块的SCL,模拟4号口连接I2C模块的SDA口。VCC与GND正常连接,主要不要接错电压,要使用3.3V。I2C模块之间并联。




把下面的代码编译后下载进入arduino控制板中。
  1. #include <Wire.h>  // 调用I2C库

  2. // 加速度传感器 ADXL345
  3. #define ACC (0x53)    //定义ADXL345地址
  4. #define A_TO_READ (6)        //读取每次占用的字节数 (每个坐标轴占两个字节)


  5. // 陀螺仪 ITG3200
  6. #define GYRO 0x68 // 定义传感器地址,将AD0连接到GND口,传感器地址为二进制数11101000 (请参考你接口板的原理图)
  7. #define G_SMPLRT_DIV 0x15
  8. #define G_DLPF_FS 0x16
  9. #define G_INT_CFG 0x17
  10. #define G_PWR_MGM 0x3E

  11. #define G_TO_READ 8 // x,y,z 每个轴2 bytes

  12. // 陀螺仪误差修正的偏移量
  13. int g_offx = 67;
  14. int g_offy = 5;
  15. int g_offz = 41;

  16. // 加速度传感器误差修正的偏移量
  17. int a_offx = -30;
  18. int a_offy = -8;
  19. int a_offz = 0;

  20. char str[512];

  21. void initAcc() {
  22.   //调用 ADXL345
  23.   writeTo(ACC, 0x2D, 0);      
  24.   writeTo(ACC, 0x2D, 16);
  25.   writeTo(ACC, 0x2D, 8);
  26.   //设定在 +-2g 时的默认读数
  27. }

  28. void getAccelerometerData(int * result) {
  29.   int regAddress = 0x32;    //加速度传感器ADXL345第一轴的数据的设定
  30.   byte buff[A_TO_READ];

  31.   readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据

  32.   //每个轴的读数有10位分辨率,即2个字节.  
  33.   //我们要转换两个bytes为一个int变量
  34.   result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;   
  35.   result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
  36.   result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
  37. }

  38. //初始化陀螺仪
  39. void initGyro()
  40. {
  41.   /*****************************************
  42.    * ITG 3200
  43.    * 电源管理设定:
  44.    * 时钟选择 =内部振荡器
  45.    * 无复位, 无睡眠模式
  46.    * 无待机模式
  47.    * 采样率 = 125Hz
  48.    * 参数为+ / - 2000度/秒
  49.    * 低通滤波器=5HZ
  50.    * 没有中断
  51.    ******************************************/
  52.   writeTo(GYRO, G_PWR_MGM, 0x00);
  53.   writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
  54.   writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
  55.   writeTo(GYRO, G_INT_CFG, 0x00);
  56. }


  57. void getGyroscopeData(int * result)
  58. {
  59.   /**************************************
  60.    * 陀螺仪ITG- 3200的I2C
  61.    * 寄存器:
  62.    * temp MSB = 1B, temp LSB = 1C
  63.    * x axis MSB = 1D, x axis LSB = 1E
  64.    * y axis MSB = 1F, y axis LSB = 20
  65.    * z axis MSB = 21, z axis LSB = 22
  66.    *************************************/

  67.   int regAddress = 0x1B;
  68.   int temp, x, y, z;
  69.   byte buff[G_TO_READ];

  70.   readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据

  71.   result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
  72.   result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
  73.   result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
  74.   result[3] = (buff[0] << 8) | buff[1]; // 温度

  75. }


  76. void setup()
  77. {
  78.   Serial.begin(9600);
  79.   Wire.begin();
  80.   initAcc();
  81.   initGyro();
  82. }


  83. void loop()
  84. {
  85.   int acc[3];
  86.   int gyro[4];
  87.   getAccelerometerData(acc);
  88.   getGyroscopeData(gyro);

  89.   sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], gyro[3]);  
  90.   Serial.print(str);
  91.   Serial.print(10, BYTE);

  92.   //延时50毫秒
  93. }


  94. //---------------- 功能
  95. //将val写入到加速度传感器的地址寄存器中
  96. void writeTo(int DEVICE, byte address, byte val) {
  97.   Wire.beginTransmission(DEVICE); //传送到加速度传感器
  98.   Wire.send(address);        // 发送寄存器地址
  99.   Wire.send(val);        // 发送要写入的值
  100.   Wire.endTransmission(); //结束传输
  101. }


  102. //加速度传感器在地址寄存器的缓冲区阵列中读取读数
  103. void readFrom(int DEVICE, byte address, int num, byte buff[]) {
  104.   Wire.beginTransmission(DEVICE); //开始传送至加速度传感器
  105.   Wire.send(address);        //发送读取的地址
  106.   Wire.endTransmission(); //结束传输

  107.   Wire.beginTransmission(DEVICE); //开始传送到ACC
  108.   Wire.requestFrom(DEVICE, num);    // 要求从加速度传感器中发送6个字节的数据

  109.   int i = 0;
  110.   while(Wire.available())    //当加速度传感器返回的数据小于要求值时(异常情况)
  111.   {
  112.     buff[i] = Wire.receive(); // 接收数据
  113.     i++;
  114.   }
  115.   Wire.endTransmission(); //结束传输
  116. }
复制代码
先介绍一下processing的基本使用方法,先从http://processing.org/download/下载回来processing的IDE。

然后把下面代码拷贝进入进入processing,查看连接arduino的com口是第几个。根据具体情况调整com口连接代码。
  1. import processing.serial.*;

  2. Serial myPort;  // 创建串口对象myPort

  3. boolean firstSample = true;

  4. float [] RwAcc = new float[3];         // 通过加速度传感器把重力加速度投影在x/y/z三轴上
  5. float [] Gyro = new float[3];          // 陀螺仪读取
  6. float [] RwGyro = new float[3];        // 重新读取陀螺仪
  7. float [] Awz = new float[2];           // XZ/ YZ平面和Z轴(度)R的投影之间的角度
  8. float [] RwEst = new float[3];


  9. int lastTime = 0;
  10. int interval = 0;
  11. float wGyro = 10.0;

  12. int lf = 10; // 10在ASCII表中表示'\n'
  13. byte[] inBuffer = new byte[100];

  14. PFont font;
  15. final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;


  16. void setup()
  17. {
  18.   size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  19.   myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口,这个要根据你电脑情况进行设置。

  20.   //myPort = new Serial(this, "/dev/ttyUSB0", 9600);  

  21.   // 加载字体,字体必须在代码文件同目录下的data文件夹中
  22.   font = loadFont("CourierNew36.vlw");
  23. }


  24. void readSensors() {
  25.   if (myPort.available() > 0) {
  26.     if (myPort.readBytesUntil(lf, inBuffer) > 0) {
  27.       String inputString = new String(inBuffer);
  28.       String [] inputStringArr = split(inputString, ',');

  29.       // 把原始数据转换为G
  30.       RwAcc[0] = float(inputStringArr[0]) / 256.0;
  31.       RwAcc[1] = float(inputStringArr[1])/ 256.0;
  32.       RwAcc[2] = float(inputStringArr[2])/ 256.0;

  33.       // 把原始数据转换为"度/秒"
  34.       Gyro[0] = float(inputStringArr[3]) / 14.375;
  35.       Gyro[1] = float(inputStringArr[4]) / 14.375;
  36.       Gyro[2] = float(inputStringArr[5]) / 14.375;
  37.     }
  38.   }
  39. }


  40. void normalize3DVec(float [] vector) {
  41.   float R;
  42.   R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  43.   vector[0] /= R;
  44.   vector[1] /= R;  
  45.   vector[2] /= R;
  46. }


  47. float squared(float x) {
  48.   return x*x;
  49. }


  50. void buildBoxShape() {
  51.   //box(60, 10, 40);
  52.   noStroke();
  53.   beginShape(QUADS);

  54.   //Z+ (绘图区域)
  55.   fill(#00ff00);
  56.   vertex(-30, -5, 20);
  57.   vertex(30, -5, 20);
  58.   vertex(30, 5, 20);
  59.   vertex(-30, 5, 20);

  60.   //Z-
  61.   fill(#0000ff);
  62.   vertex(-30, -5, -20);
  63.   vertex(30, -5, -20);
  64.   vertex(30, 5, -20);
  65.   vertex(-30, 5, -20);

  66.   //X-
  67.   fill(#ff0000);
  68.   vertex(-30, -5, -20);
  69.   vertex(-30, -5, 20);
  70.   vertex(-30, 5, 20);
  71.   vertex(-30, 5, -20);

  72.   //X+
  73.   fill(#ffff00);
  74.   vertex(30, -5, -20);
  75.   vertex(30, -5, 20);
  76.   vertex(30, 5, 20);
  77.   vertex(30, 5, -20);

  78.   //Y-
  79.   fill(#ff00ff);
  80.   vertex(-30, -5, -20);
  81.   vertex(30, -5, -20);
  82.   vertex(30, -5, 20);
  83.   vertex(-30, -5, 20);

  84.   //Y+
  85.   fill(#00ffff);
  86.   vertex(-30, 5, -20);
  87.   vertex(30, 5, -20);
  88.   vertex(30, 5, 20);
  89.   vertex(-30, 5, 20);

  90.   endShape();
  91. }


  92. void drawCube() {  
  93.   pushMatrix();
  94.   translate(300, 450, 0);
  95.   scale(4, 4, 4);

  96.   rotateX(HALF_PI * -RwEst[0]);
  97.   rotateZ(HALF_PI * RwEst[1]);

  98.   buildBoxShape();

  99.   popMatrix();
  100. }


  101. void getInclination() {
  102.   int w = 0;
  103.   float tmpf = 0.0;
  104.   int currentTime, signRzGyro;


  105.   readSensors();
  106.   normalize3DVec(RwAcc);

  107.   currentTime = millis();
  108.   interval = currentTime - lastTime;
  109.   lastTime = currentTime;

  110.   if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
  111.     for (w=0;w<=2;w++) {
  112.       RwEst[w] = RwAcc[w];    // 初始化加速度传感器读数
  113.     }
  114.   }
  115.   else {
  116.     // 对RwGyro进行评估
  117.     if (abs(RwEst[2]) < 0.1) {
  118.       // Rz值非常的小,它的作用是作为Axz与Ayz的计算参照值,防止放大的波动产生错误的结果。
  119.       // 这种情况下就跳过当前的陀螺仪数据,使用以前的。
  120.       for (w=0;w<=2;w++) {
  121.         RwGyro[w] = RwEst[w];
  122.       }
  123.     }
  124.     else {
  125.       // ZX/ZY平面和Z轴R的投影之间的角度,基于最近一次的RwEst值
  126.       for (w=0;w<=1;w++) {
  127.         tmpf = Gyro[w];                        // 获取当前陀螺仪的deg/s
  128.         tmpf *= interval / 1000.0f;                     // 得到角度变化值
  129.         Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI;   // 得到角度并转换为度
  130.         Awz[w] += tmpf;             // 根据陀螺仪的运动得到更新后的角度
  131.       }

  132.       // 判断RzGyro是多少,主要看Axz的弧度是多少
  133.       // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
  134.       signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;

  135.       // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
  136.       for (w=0;w<=1;w++) {
  137.         RwGyro[0] = sin(Awz[0] * PI / 180);
  138.         RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
  139.         RwGyro[1] = sin(Awz[1] * PI / 180);
  140.         RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
  141.       }
  142.       RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
  143.     }

  144.     // 把陀螺仪与加速度传感器的值进行结合
  145.     for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

  146.     normalize3DVec(RwEst);
  147.   }

  148.   firstSample = false;
  149. }


  150. void draw() {  
  151.   getInclination();

  152.   background(#000000);
  153.   fill(#ffffff);

  154.   textFont(font, 20);
  155.   //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  156.   //text("temp:\n" + temp_decoded + " C", 350, 250);
  157.   text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
  158.   text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
  159.   text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
  160.   text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
  161.   text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);

  162.   // display axes显示轴
  163.   pushMatrix();
  164.   translate(450, 250, 0);
  165.   stroke(#ffffff);
  166.   scale(100, 100, 100);
  167.   line(0, 0, 0, 1, 0, 0);
  168.   line(0, 0, 0, 0, -1, 0);
  169.   line(0, 0, 0, 0, 0, 1);
  170.   line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
  171.   popMatrix();

  172.   drawCube();
  173. }
复制代码
然后点击运行



实验效果:


arduino上的代码主要作用就是采集两个传感器的数据,然后通过串口发送出去。关键的算法在processing上面,代码做了注释,如果想研究可以仔细看看代码。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

发表于 2014-7-18 21:18:40 | 显示全部楼层
我现在想用MSP430读取MPU6050,六个传感器数据发串口,processing读数并处理数据,然后画方块显示姿态。由于我的MSP430是用CCS软件编的,所以和arduino的平台不太一样,看了一下代码不太明白,想请教一下,这六个轴的数据是以“ax,ay,az,gx,gy,gz\n”这样的格式发送的吗?而且是字符型数据?我按这样的格式发送,processing不画方块,不明白为什么。请高手指点一二。

另外processing报了一排红字“Display 0 does not exist, using the default display instead.”
不知道是不是不显示方块的原因。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 2 反对 0

使用道具 举报

发表于 2011-11-25 22:54:42 | 显示全部楼层
  观摩  观摩
回复 支持 反对

使用道具 举报

发表于 2012-2-6 17:17:41 | 显示全部楼层
请问楼主的陀螺仪模块howmuch?
几轴的?
能否用其他的代替?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-2-6 17:48:06 | 显示全部楼层
Malc 发表于 2012-2-6 17:17
请问楼主的陀螺仪模块howmuch?
几轴的?
能否用其他的代替?

当时买的芯片自己焊的,100多点,3三轴数字陀螺仪~~

其他数字陀螺仪都可以替代
回复 支持 反对

使用道具 举报

发表于 2012-2-6 18:03:37 | 显示全部楼层
有了加速度传感器为什么还要陀螺仪?
是为了减少加速度传感器的噪声吗?
还是有别的作用
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-2-6 18:13:38 | 显示全部楼层
Malc 发表于 2012-2-6 18:03
有了加速度传感器为什么还要陀螺仪?
是为了减少加速度传感器的噪声吗?
还是有别的作用


陀螺仪感应姿态,加速度传感器对陀螺仪的累积误差进行校准,一般是这样子用。

对于快速倾斜的场合比如飞控上,加速度传感器的反应速度跟不上。这种需要快速检测倾斜角度的地方,就需要用陀螺仪。

但是陀螺仪数据久了,就会产生误差,可以理解为陀螺仪转晕了不知道哪里是正上方了。

这种情况下,因为加速度传感器是感应重力加速度的,不管怎么转,都不会晕。。。所以要用加速度传感器对陀螺仪的误差进行纠正。
回复 支持 反对

使用道具 举报

发表于 2012-2-28 11:27:59 | 显示全部楼层
细看了你的代码,主要用了加速度来感应姿态,中间对数据做了一些修正和防抖,好像没用到陀螺仪的数据来参与修正,是吗?
如果是的话,这仅仅是静态物体的姿态检测。

不管怎么样,还是要谢谢大哥的分享,大家都学习了。
回复 支持 反对

使用道具 举报

发表于 2012-3-17 20:52:47 | 显示全部楼层
问下弘毅,这个字体去哪里找?

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-18 20:02:09 | 显示全部楼层
moon 发表于 2012-3-17 20:52
问下弘毅,这个字体去哪里找?

在processing的目录里面搜索。。就能找到这个字体。
回复 支持 反对

使用道具 举报

发表于 2012-3-18 22:27:46 | 显示全部楼层
谢了啊,找到了~另外你这个arduino发送数组和processing接收数组这部分实在没看懂啊
回复 支持 反对

使用道具 举报

发表于 2012-3-19 19:45:33 | 显示全部楼层
这个刚好用上,慢慢研究……
回复 支持 反对

使用道具 举报

发表于 2012-3-20 12:03:12 | 显示全部楼层
请教一下,为什么我的ADXL345读取频率最高才5Hz不到呢?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-20 12:04:45 | 显示全部楼层
黑马 发表于 2012-3-20 12:03
请教一下,为什么我的ADXL345读取频率最高才5Hz不到呢?


其实。。这玩意我也不是很会用,能读出来数值。。我就没有深入研究了,具体多少采样率,我估计得配置寄存器吧=.=英文datasheet看的头疼
回复 支持 反对

使用道具 举报

发表于 2012-3-20 17:11:00 | 显示全部楼层
重新测了一下,不是传感器读取的慢,是oled没弄好
回复 支持 反对

使用道具 举报

发表于 2012-3-25 15:24:30 | 显示全部楼层
  你好  新手  读出现 Serial.print(10, BYTE);   不明
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2022-5-17 12:53 , Processed in 0.171121 second(s), 27 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表