0
我正在研究自動平衡機器人項目，並使用ADGL345加速計與ITG-3200陀螺儀一起檢測機器人的傾斜度。ADXL345加速度計與Arduino的整數溢出
我之前遇到的問題是有符號整數顯示不正確，因爲Arduino Due將標準整數存儲爲32位數，導致16位有符號數據全部爲正數，超過65000爲負數。我做了一些研究，並在整個過程中實施了int16_t以維持一個有符號的16位系統，但是我遇到了一個新問題。
似乎在機器人向任何方向傾斜超過35度左右後，整數開始溢出，超過32,767的任何數字開始再次減少。這在確定機器人的角度時顯然不適合。有什麼方法可以縮小這些值或以其他方式接收整個範圍內的恆定有效數據流？
見下面的代碼：
#include <stdint.h>
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
void setup()
{
// Init serial output
Serial.begin(57600);
// Init sensors
delay(50); // Give sensors enough time to start
I2C_Init();
Accel_Init();
Gyro_Init();
}
void loop()
{
Read_Accel();
Serial.print("#A:");
Serial.print(accel_x); Serial.print(",");
Serial.print(accel_y); Serial.print(",");
Serial.print(accel_z); Serial.println();
Read_Gyro();
Serial.print("#G:");
Serial.print(gyro_x); Serial.print(",");
Serial.print(gyro_y); Serial.print(",");
Serial.print(gyro_z); Serial.println();
}
// *******************I2C code to read the sensors************************
#include <Wire.h>
// Sensor I2C addresses
#define ACCEL_ADDRESS ((int) 0x53) // 0x53 = 0xA6/2
#define GYRO_ADDRESS ((int) 0x68) // 0x68 = 0xD0/2
void I2C_Init()
{
Wire.begin();
}
void Accel_Init()
{
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x2D); // Power register
Wire.write(0x08); // Measurement mode
Wire.endTransmission();
delay(5);
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x31); // Data format register
Wire.write(0x08); // Set to full resolution
Wire.endTransmission();
delay(5);
// Adjust the output data rate to 100Hz (50Hz bandwidth)
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x2C); // Rate
Wire.write(0x0A); // Set to 100Hz, normal operation
Wire.endTransmission();
delay(5);
}
// Reads x, y and z accelerometer registers
void Read_Accel()
{
int i = 0;
byte buff[6];
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x32); // Send address to read from
Wire.endTransmission();
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.requestFrom(ACCEL_ADDRESS, 6); // Request 6 bytes
while(Wire.available()) // ((Wire.available())&&(i<6))
{
buff[i] = Wire.read(); // Read one byte
i++;
}
Wire.endTransmission();
{
accel_x = ((buff[2]) << 8) | buff[3]; // X axis (internal sensor y axis)
accel_y = ((buff[0]) << 8) | buff[1]; // Y axis (internal sensor x axis)
accel_z = ((buff[4]) << 8) | buff[5]; // Z axis (internal sensor z axis)
}
}
void Gyro_Init()
{
// Power up reset defaults
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x3E);
Wire.write(0x80);
Wire.endTransmission();
delay(5);
// Select full-scale range of the gyro sensors
// Set LP filter bandwidth to 42Hz
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x16);
Wire.write(0x1B); // DLPF_CFG = 3, FS_SEL = 3
Wire.endTransmission();
delay(5);
// Set sample rato to 100Hz
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x15);
Wire.write(0x09); // SMPLRT_DIV = 9 (100Hz)
Wire.endTransmission();
delay(5);
// Set clock to PLL with z gyro reference
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x3E);
Wire.write(0x00);
Wire.endTransmission();
delay(5);
}
// Reads x, y and z gyroscope registers
void Read_Gyro()
{
int i = 0;
byte buff[6];
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x1D); // Sends address to read from
Wire.endTransmission();
Wire.beginTransmission(GYRO_ADDRESS);
Wire.requestFrom(GYRO_ADDRESS, 6); // Request 6 bytes
while(Wire.available()) // ((Wire.available())&&(i<6))
{
buff[i] = Wire.read(); // Read one byte
i++;
}
Wire.endTransmission();
{
gyro_x = (((buff[2]) << 8) | buff[3]); // X axis (internal sensor -y axis)
gyro_y = (((buff[0]) << 8) | buff[1]); // Y axis (internal sensor -x axis)
gyro_z = (((buff[4]) << 8) | buff[5]); // Z axis (internal sensor -z axis)
}
}