圣源电子制作

 找回密码
 立即注册
查看: 8831|回复: 0
打印 上一主题 下一主题

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

[复制链接]
跳转到指定楼层
楼主
发表于 2012-4-28 22:58:02 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
转载自互联网!!!仅供学习
姿态识别应用范围很广,像自平衡车呀,飞行器呀,双足机器人呀之类。本次我们使用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上面,代码做了注释,如果想研究可以仔细看看代码。
回复

使用道具 举报

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

本版积分规则

QQ|手机版|联系我们|闽公网安备 35012102000020号|闽ICP备11020110号-1|圣源电子

GMT+8, 2024-12-4 01:43 , Processed in 0.042088 second(s), 19 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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