This topic is about gyros and accelerometers used in balancing robots. I'm fairly new here so if i posted this in the wrong place, please move as required.
Was inspired by a couple of articles on first and second order complimentary filters. I was impressed with the effectiveness of single order filters setup to be an extremely simple bandpass. Along with that, adding a Hanning filter that takes a 3 reading average but the middle one has twice the weight as its neighbours on either side. While simple, its a second order giving great response.
If the accel and gyros aren't dead solid, responsive, and practically immune to noise it will always struggle to balance. Also, there is no loss of resolution using this method, there is no reason to have actual degrees, good responsive resolution is more important as a final off center measurement.
Hanning: http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf
Filters: Kalman filter vs Complementary filter « Robottini
Simple code of the function that would replace the Kalman filter:
float tau=0.075; //http://robottini.altervista.org/kalman-filter-vs-complementary-filter
float a=0.0;
float han[3] = {0,0,0};
float hanAvg = 0;
float Complementary(float newAngle, float newRate,int looptime)
{
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC) ;
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
//Hanning filter //http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf
han[2] = han[1];
han[1] = han[0];
han[0] = x_angleC;
hanAvg = han[0] + 2*han[1] + han[2];
return hanAvg;
I'll be the first to admit, i don't understand the math of the Kalman filter. Cutting and pasting code without understanding it for me has never been a good way to program. The math in this is very simple-ish, easy to adjust and for a balancing robot very good bandwidth yet removing noise.
Any comments would be great. Thx Kas and many others for all your work, the credit goes to you for helping so many of us understand this stuff.
The code below is just the Module 5 from the old Balancing Robot for Dummies thread modified.
http://forum.arduino.cc/index.php?topic=8871.0
Uncomment the analogRef if using external Vref. I use Due so not required.
// Main module K_bot angle angles in Quids, 10 bit ADC -----------------------------
// 5 - angle and rate calculation display ACC_Angle and GYRO_rate
//http://forum.arduino.cc/index.php?topic=8871.60
#include <math.h>
#define GYR_Y 0 // Gyro Y (IMU pin #4)
#define ACC_Z 1 // Acc Z (IMU pin #7)
#define ACC_X 2 // Acc X (IMU pin #9)
int STD_LOOP_TIME = 9;
int sensorValue[3] = { 0, 0, 0};
int sensorZero[3] = { 0, 0, 0 };
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int ACC_angle;
int GYRO_rate;
float accZ=0;
float accY=0;
float x1=0;
float why1=0;
float x2=0;
float y2=0;
float x_angle=0;
float x_angleC=0;
float x_angleRG=0;
float x_angleRA=0;
float x_angle2C=0;
void setup() {
// analogReference(EXTERNAL); // Aref 3.3V
Serial.begin(115200);
delay(100);
calibrateSensors();
}
void loop() {
// ********************* Sensor aquisition & filtering *******************
updateSensors();
ACC_angle = getAccAngle(); // in Quids
GYRO_rate = getGyroRate(); // in Quids per seconds
x_angle = Complementary(ACC_angle, GYRO_rate, lastLoopTime);
// ********************* print Debug info *************************************
serialOut_sensor();
// *********************** loop timing control **************************
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
//**************************** Comp Filter ******************************************
//
float tau=0.075; //http://robottini.altervista.org/kalman-filter-vs-complementary-filter
float a=0.0;
float han[3] = {0,0,0};
float hanAvg = 0;
float Complementary(float newAngle, float newRate,int looptime)
{
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC) ;
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
//Hanning filter //http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf
han[2] = han[1];
han[1] = han[0];
han[0] = x_angleC;
hanAvg = han[0] + 2*han[1] + han[2];
return hanAvg;
}
void serialOut_sensor() {
static int skip=0;
if(skip++==5) {
skip = 0;
Serial.print(lastLoopTime); Serial.print("\t");
Serial.print(sensorValue[2]); Serial.print("\t");
Serial.print(ACC_angle); Serial.print("\t");
Serial.print(sensorValue[0]); Serial.print("\t");
Serial.print(GYRO_rate); Serial.print("\t");
int x = int(x_angle);
Serial.println(x);
}
}
// Sensors Module -----------------------------------------------------------------------------------
void calibrateSensors() { // Set zero sensor values
long v;
for(int n=0; n<3; n++) {
v = 0;
for(int i=0; i<50; i++) v += readSensor(n);
sensorZero[n] = v/50;
} //(618 - 413)/2 = 102.5 330/3.3 = x/1024
sensorZero[ACC_Z] -= 100; //102; // Sensor: horizontal, upward
}
void updateSensors() { // data acquisition
long v;
for(int n=0; n<3; n++) {
v = 0;
for(int i=0; i<5; i++) v += readSensor(n);
sensorValue[n] = v/5 - sensorZero[n];
}
}
int readSensor(int channel){
return (analogRead(channel));
}
int getGyroRate() { // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
return int(sensorValue[GYR_Y] * 4.583333333); // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}
int getAccAngle() {
return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256; // in Quid: 1024/(2*PI))
}
int arctan2(int y, int x) { // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
int coeff_1 = 128; // angle in Quids (1024 Quids=360°)
int coeff_2 = 3*coeff_1;
float abs_y = abs(y)+1e-10;
float r, angle;
if (x >= 0) {
r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
if (y < 0) return int(-angle);
else return int(angle);
}
You are exactly right!