0

我正在开发一个自平衡机器人项目，并使用 ADXL345 加速度计和 ITG-3200 陀螺仪来感应机器人的倾斜度。

我之前遇到了带符号整数显示不正确的问题，事实证明，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)
  }
}
4

1 回答 1

0

我认为这可能是字节格式的问题。比编译器真正使用来自 buff[2] 的字节（8 位）将被移动 8 到 nirwana。结果将为 0，这将与 buff[3] 进行或组合。编译器一步一步地工作。

原来的：

字节缓冲区[6]；

accel_x = (( buff[2]) << 8) | buff[3];  // X axis (internal sensor y axis)

解决方案：

accel_x = (( (int16_t)buff[2]) << 8) | buff[3];  // X axis (internal sensor y axis)

或分步

accel_x= buff[2];//accel_x has 16-bit-format
accel_x<<= 8;
accel_x|= buff[3];
于 2014-12-28T20:30:13.667 回答