Hello All,
I have been working on my balancing robot project on and off since the new year and have made it as far as filtering the signals to provide a clean angle to work with when balancing the robot. A thorough explanation of this technique can be found here: http://web.mit.edu/scolton/www/filter.pdf. In the filter I have implemented (Based on the filter found here: pieter-jan.com - This website is for sale! - pieter jan Resources and Information.) I can read back an accurate accelerometer angle (pitchAcc) however when I go to read the combined angle (*pitch) I read only angles of 0.00 degrees. What is happening here?
#include <stdint.h>
#define GYROSCOPE_SENSITIVITY 14.375
#define M_PI 3.14159265359
#define dt 0.01 // 10 ms sample rate!
float *pitch;
float pitchAcc;
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int STD_LOOP_TIME = 9; //Length of main loop in milliseconds
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
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();
ComplementaryFilter();
Serial.print("Angle: ");
Serial.print(*pitch); Serial.println();
// *********************** loop timing control ***************************
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
// *******************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 = ((((int) buff[3]) << 8) | buff[2])-5; // X axis (internal sensor y axis)
accel_y = ((((int) buff[1]) << 8) | buff[0])-3; // Y axis (internal sensor x axis)
accel_z = ((((int) buff[5]) << 8) | buff[4])-1; // 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 = ((((int) buff[2]) << 8) | buff[3])-11; // X axis (internal sensor -y axis)
gyro_y = ((((int) buff[0]) << 8) | buff[1])+34; // Y axis (internal sensor -x axis)
gyro_z = ((((int) buff[4]) << 8) | buff[5])+12; // Z axis (internal sensor -z axis)
}
}
// ***********************Complementary Filter****************************
void ComplementaryFilter()
{
// Integrate the gyroscope data -> int(angularSpeed) = angle
*pitch = ((float)gyro_y / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
// Compensate for drift with accelerometer data if wrong
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 513 && 0.5G = 128
int16_t forceMagnitudeApprox = abs(accel_x) + abs(accel_y) + abs(accel_z);
if (forceMagnitudeApprox > 128 && forceMagnitudeApprox < 513)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accel_x, (float)accel_z) * 180 / M_PI;
*pitch = *pitch * 0.98 + pitchAcc * 0.02;
}
}
Any help would be appreciated
Andrew