Question

我正在研究一个自动平衡机器人项目，我正在使用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)
  }
}

Answer 1

我认为这可能是字节格式的问题。比编译器真正使用buff [2]中的字节，字节（8bit）将被移位8到nirwana。结果将为0，这将与buff [3]结合或组合。编译器向下一步迈出了一步。

原：

byte buff [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];

ADXL345加速度计整数溢出与Arduino Due

1 个答案: