In continuing the work started by ardvark and others here I'm trying to figure out how best to filter accelerometer and gyro data for a segway-style robot I'm working on.
I've tried to take the code down to the bare minimum from what they had gotten to; rather than read all three axes, I'm only reading and processing one axis on both the accelerometer (Z-axis) and gyro (pitch). I also modified the ranges for the accelerometer to fit those read by my nunchuck.
Right now the calculations are done in radians, so there are a couple of floating point calculations and the output is in radians. I think if I can reduce the number of FP calculations and do everything in degrees, I can get better accuracy while reducing time spent calculation. I'm not exactly sure what data is output by the sensors, so I'm not sure how to advance.
Does that sound like the right thing to do? or am I going about this the wrong way?
/*
Heavily modified code from this page: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1253202298, modifications by Joey Casabar
Intended for use with segway-style robots, so only one axis on both accelerometer and gyro are being measured.
For ease of mounting pitch axis of gyro and Z-axis of accelerometer are being used so WMP and NK boards can be mounted together in vertical position
Calculations are done in radians because it seemed to execute faster, although I'm figuring out how to do them in degrees for more accuracy/less floating-point math
More information about the project can be found at http://makerandomstuff.blogspot.com/2011/12/introduction-fail-way-crappy-balancing.html
*/
#include <Wire.h>
unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;
boolean calibrating = false;
uint8_t data[6]; // array to store arduino output
int cnt = 0;
int ledPin = 13;
int xID;
static const int wmpSlowToDegreePerSec = 16; //when gyro speed 1, 16 units = 1°/s
static const int wmpFastToDegreePerSec = 4; //when gyro speed 0, 4 units = 0,25°/s
// Hardware identifiers. Polling data[5]&0x03 for present device
static const int NC = 0x00;
static const int MP = 0x02;
int currentDevice;
// WM+ state variables - if true, then in slow (high res) mode
boolean slowPitch = 0;
int pitchScale;
// Calibrated gyro values (- yaw0 etc)
int pitch = 0;
float pitchf = 0;
float pitch_int = 0;
float pitch_int_rad = 0;
float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)
float pitchR = 0;
float pitchCF = 0;
static const double DegreeToRadian = 0.0174532925; // PI/180
// Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
// Tweak tau to alter the performance of the complimentary filter.
// Increase tau: influence of the gyro input increases (less translation artifacts).
// Reduce tau: influence of the accelerometer input increases (faster drift-correction).
// This filtering is temporal: translations that occur over a time scale shorter than tau will be suppressed,
// and gyro changes that occur over a time scale longer than tau will have their drift corrected.
// NB: changes in the read rate of the gyro (dtMP) will change the filter's performance (change tau to compensate).
static const float tau = 0.95; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
float a = tau/(tau + dtMP);
float b = 1 - a;
return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}
// accelerometer values
int az_m = 0;
// Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
static const int azMIN = 186;
static const int azMAX = 529;
static const int azMID = (azMAX - azMIN)/2 + azMIN;
static const int azRange = (azMID - azMIN)*2;
// raw 3 axis gyro readings MP+ //gyro readings => yaw/yawscale
int readingsX;
// Nunchuck variables //NunChuck X angle NOT readings
float accelAngleX=0; //NunChuck X angle
float theta=0;
// calibration zeroes
int pitch0 = 0; //gyros
int pitchAcc0 = 0; //accelerometers
double z;
//Nunchuk accelerometer value to radian conversion.
float angleInRadians(int range, int measured) {
float x = range;
return (float)(measured/x) * PI;
}
void
setup ()
{
Serial.begin(19200);
Wire.begin();
initializeSensors();
calibrateZeroes();
}
void send_zero ()
{
Wire.beginTransmission(0x52);
Wire.send(0x00);
Wire.endTransmission();
}
void loop ()
{
Now = millis();
dt = Now - lastread; //compute the time delta since last iteration, in msec.
if (dt >= 10)
{
readData();
lastread = Now;
Serial.print("pitchCF: ");
Serial.println(pitchCF);
}
else
{
//code here
}
}
void readData()
{
long startReading = millis();
send_zero (); // send the request for next bytes
// now follows conversion command instructing extension contpitcher to get
// all sensor data and put them into 6 byte register within the extension contpitcher
Wire.requestFrom (0x52,6);
data[0] = Wire.receive();
data[1] = Wire.receive();
data[2] = Wire.receive();
data[3] = Wire.receive();
data[4] = Wire.receive();
data[5] = Wire.receive();
parseData();
}
void parseData ()
{
// check if nunchuk is really connected
if ((data[4]&0x01)==0x01)
{
//printLine("Ext con: ");
currentDevice = data[5]&0x03;
}
if (currentDevice == NC)
{
az_m = (data[4] << 2) + ((data[5] >> 5) & 6); // Dimitris version of accl readings
az_m -= azMID; //Zero the values on a_MID.
// Convert to radians by mapping 180 deg of rotation to PI
z = angleInRadians(azRange, az_m);
}
else
if (currentDevice == MP)
{
dtMP = (Now - lastreadMP)/1000.0; //compute the time delta since last MP read, in sec.
lastreadMP = Now;
pitch = -(((data[5]&0xFC)<<6) + data[0]);
if(calibrating == false) //Dont compensate if we are doing the calibration measurements.
{
slowPitch = (data[4]&0x02)>>1;
pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; // if speed == 1, then wmpSlowToDegreePerSec, else wmpFastToDegreePerSec
pitch -= pitch0; //Calibrating
pitch = pitch/pitchScale; // deg/s = mV/(mV/deg/s)
pitchCF = compFilter(pitchCF, pitch, z, dtMP); // Complimentary filter inspired by filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
pitchR = pitch * DegreeToRadian; //Convert deg/s to rad/s
}
}
}
void initializeSensors()
{
digitalWrite(13,HIGH); // first flash the LED to show activity
//delay(100);
// now make Wii Motion plus the active extension
// nunchuk mode = 05, wm+ only = 04, classic contpitcher = 07
Wire.beginTransmission(0x53);
Wire.send(0xFE);
Wire.send(0x05);// nunchuk
Wire.endTransmission();
//delay (100);
// now innitiate Wii Motion plus
Wire.beginTransmission(0x53);
Wire.send(0xF0);
Wire.send(0x55);
Wire.endTransmission();
//delay (100);
Wire.beginTransmission(0x52);
Wire.send(0xFA);
Wire.endTransmission();
//delay (100);
Wire.requestFrom (0x52,6);
data[0] = Wire.receive();//Serial.print(outbuf[0],HEX);Serial.print(" ");
data[1] = Wire.receive();//Serial.print(outbuf[1],HEX);Serial.print(" ");
data[2] = Wire.receive();//Serial.print(outbuf[2],HEX);Serial.print(" ");
data[3] = Wire.receive();//Serial.print(outbuf[3],HEX);Serial.print(" ");
data[4] = Wire.receive();//Serial.print(outbuf[4],HEX);Serial.print(" ");
data[5] = Wire.receive();//Serial.print(outbuf[5],HEX);Serial.print(" ");
xID= data[0] + data[1] + data[2] + data[3] + data[4] + data[5];
//delay (100);
// Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
Wire.beginTransmission(0x52);
Wire.send(0x08);
Wire.endTransmission();
digitalWrite(13,LOW); // Led off
}
/*
* Find the steady-state of the WM+ gyros. This function reads the gyro values 100 times and averages them.
* Those values are used as the baseline steady-state for all subsequent reads. As a result it is very
* important that the device remain relatively still during startup.
*/
void calibrateZeroes(){ // wiibrew: While the Wiimote is still, the values will be about 0x1F7F (8,063)
calibrating = true;
long r0 = 0; //Gyros
//Accelerometers for application calibration, not for zeroing the starting points.
long ra0 = 0;
const int avg = 200; //100 reads of the gyros and 100 of the accelerometers
digitalWrite(13,HIGH); // first flash the LED to show activity
for (int i=0;i<avg; i++)
{
delay(10); // bypasses the normal global delays
readData();
if(currentDevice == MP)
{
r0+=pitch;
}
else
{
ra0 += az_m;
}
}
pitch0 = r0/(avg/2);
pitchAcc0 = ra0/(avg/2);
calibrating = false;
digitalWrite(13,LOW); // Led off
}