本帖最后由 董董soul 于 2018-10-24 14:38 编辑
小弟,修改了一下arduino 方面的代码 使用的是l3G4200D + ADXL345 来完成楼主的试验。其中读取gyro部分的代码来自版主 黑马的代码,感谢版主黑马。感谢弘毅。感谢xx。
- #include <Wire.h> // 调用I2C库
- // 加速度传感器 ADXL345
- #define ACC (0x1D) //定义ADXL345地址 0x3B >> 1 = 0x1D
- #define A_TO_READ (6) //读取每次占用的字节数 (每个坐标轴占两个字节)
- //int ADXAddress = 0x3B >> 1; // ADXL345的I2C地址
- //int L3GAddress = 0x69; // L3G4200D的I2C地址
- //int HMCAddress = 0x1E; // HMC5883L的I2C地址
- byte vL, vH; // 存放低位、高位值
- int xGyro, yGyro, zGyro,tGyro; // 存放角速度值
- // 陀螺仪 L3G4200D
- #define L3GAddress 0x69 // 请注意此处 SDO已接上拉电阻 SDO默认高电平 IIC地址为0x69 若接地则IIC地址为0x68 具体参看 L3G4200D 手册P22
- #define G_TO_READ 8 // x,y,z 每个轴2 bytes and temprtures.
- // 陀螺仪误差修正的偏移量
- /*int g_offx = 67;
- int g_offy = 5;
- int g_offz = 41; */
- int g_offx = 0;
- int g_offy = 0;
- int g_offz = 0;
- // 加速度传感器误差修正的偏移量
- int a_offx = -14;
- int a_offy = -14;
- int a_offz = 0;
- char str[512];
- void initAcc() {
- //调用 ADXL345
- writeTo(ACC, 0x2D, 0);
- writeTo(ACC, 0x2D, 16);
- writeTo(ACC, 0x2D, 8); //设定在 +-2g 时的默认读数
- }
- void getAccelerometerData(int * result) {
- int regAddress = 0x32; //加速度传感器ADXL345第一轴的数据的设定
- byte buff[A_TO_READ];
- readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据
- //每个轴的读数有10位分辨率,即2个字节.
- //我们要转换两个bytes为一个int变量
- result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;
- result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
- result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
- }
- //初始化陀螺仪
- void initGyro()
- {
- writeTo(L3GAddress, 0x20, 0b00001111); // 设置睡眠模式、x, y, z轴使能
- writeTo(L3GAddress, 0x21, 0b00000000); // 选择高通滤波模式和高通截止频率
- writeTo(L3GAddress, 0x22, 0b00000000); // 设置中断模式
- writeTo(L3GAddress, 0x23, 0b00110000); // 设置量程(2000dps)、自检状态、SPI模式
- writeTo(L3GAddress, 0x24, 0b00000000); // FIFO & 高通滤波 // 配置L3G4200D(2000 deg/sec)
- }
- /*void getGyroscopeData(int * result)
- {
- /**************************************
- * 陀螺仪L3G4200D的I2C
- * 寄存器:
- * temp MSB = 0x26, Status_REG = 0x27
- * x axis MSB = 0x29, x axis LSB = 0x28
- * y axis MSB = 0x2B, y axis LSB = 0x2A
- * z axis MSB = 0x2D, z axis LSB = 0x2C
-
- int regAddress = 0x26;
- //int temp, x, y, z;
- byte buff[G_TO_READ];
- readFrom(L3GAddress, regAddress, G_TO_READ, buff); //读取陀螺仪L3G4200D的数据
- result[0] = ((buff[3] << 8) | buff[2]);// + g_offx;
- result[1] = ((buff[5] << 8) | buff[4]);// + g_offy;
- result[2] = ((buff[7] << 8) | buff[6]);// + g_offz;
- //result[3] = (buff[0] << 8) | buff[1]; // 温度
- result[3] = buff[0]; // 温度
- }*/
- void setup()
- {
- Serial.begin(9600);
- Wire.begin();
- initAcc();
- initGyro();
- }
- void loop()
- {
- int acc[3];
- int gyro[4];
- getAccelerometerData(acc);
- delay(200);
- //getGyroscopeData(gyro);
- getGyroValues();
-
-
- //sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], gyro[3]);
- sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], xGyro, yGyro,zGyro,tGyro);
- Serial.print(str);
- Serial.print(10, BYTE);
- delay(50); //延时50毫秒
- }
- void getGyroValues() { // 角速度值读取
- vL = readRegister(L3GAddress, 0x28);
- vH = readRegister(L3GAddress, 0x29);
- xGyro = (vH << 8) | vL;
- vL = readRegister(L3GAddress, 0x2A);
- vH = readRegister(L3GAddress, 0x2B);
- yGyro = (vH << 8) | vL;
- vL = readRegister(L3GAddress, 0x2C);
- vH = readRegister(L3GAddress, 0x2D);
- zGyro = (vH << 8) | vL;
- tGyro = readRegister(L3GAddress,0x26);
- }
- //---------------- 功能
- //将val写入到加速度传感器的地址寄存器中
- void writeTo(int DEVICE, byte address, byte val) {
- Wire.beginTransmission(DEVICE); //传送到加速度传感器
- Wire.send(address); // 发送寄存器地址
- Wire.send(val); // 发送要写入的值
- Wire.endTransmission(); //结束传输
- }
- //加速度传感器在地址寄存器的缓冲区阵列中读取读数
- void readFrom(int DEVICE, byte address, int num, byte buff[]) {
- Wire.beginTransmission(DEVICE); //开始传送至加速度传感器
- Wire.send(address); //发送读取的地址
- Wire.endTransmission(); //结束传输
- Wire.beginTransmission(DEVICE); //开始传送到ACC
- Wire.requestFrom(DEVICE, num); // 要求从加速度传感器中发送6个字节的数据
- int i = 0;
- while(Wire.available()) //当加速度传感器返回的数据小于要求值时(异常情况)
- {
- buff[i] = Wire.receive(); // 接收数据
- i++;
- }
- Wire.endTransmission(); //结束传输
- }
- int readRegister(int deviceAddress, byte address) {
- // 读寄存器
- int v;
- Wire.beginTransmission(deviceAddress);
- Wire.send(address);
- Wire.endTransmission();
- Wire.requestFrom(deviceAddress, 1);
- while(!Wire.available()) {}
- v = Wire.receive();
- return v;
- }
复制代码
|