# IMU and Gyro balance bot code

i ve been working on this for the last few days. im using a L3G4200 gyro and a LSM303 Acc in an attempt to balance two wheeled robot. it first reads the gyro and acc, then combines them using Khalman equations(to find a final angle of the IMU) and then PID to adjust the servo output to stabilize the bot according to the angle.

i wasnt really expecting the PID to work because i have to change Kp Ki and Kd for fine stabilization. but for some reason the Khalman equations didnt combine the GYRO and ACC correctly and the values were way off. if i hold the imu in one place, the value will stay at once place. but if i move the imu, the value jumps, and then stabilizes to value around what it should be, ish.

heres the code:

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(9600);

//LM303 acc setup
Wire.begin();
compass.init();

LServoMtr.attach(LServoMtrPin); //attach then set servos to center
LServoMtr.write(90);
RServoMtr.attach(RServoMtrPin);
RServoMtr.write(90);

IMUCenter(); //center IMU at current position

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

PlatformAngle = IMUCalc(); //final current angle of the bot

Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly

Serial.println(PlatformAngle);

LServoMtr.write(Output + offset);
RServoMtr.write(Output + offset);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
//IMU------------------------------------------------------------
AccAngle = arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle
GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
CalibSensor[1] = (int)gyro.g.y; //store values for centering
CalibSensor[2] = (int)compass.a.x;
CalibSensor[3] = (int)compass.a.z;

}

int PIDCalc()
{
//PID-------------------------------------------------------------
Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
Proportion = Error; //"makes a change to the output that is proportional to the current error value"
Integral += Error; //“if you aren’t quite getting to target, keep increasing over a period of time until you get there”
Derivative = (Error - PrevError); //"how fast a change in angle occurs"

//calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
//the program and the resulting values from Proportion, Integral, and Derivative calculations
Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
PrevError = Error; //puts current error in another value for next loop to use.
return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
int coeff_1 = 128;
int coeff_2 = 3*coeff_1;
float abs_z = abs(z)+1e-10;
float r, angle;

if (x >= 0)
{
r = (x - abs_z) / (x + abs_z);
angle = coeff_1 - coeff_1 * r;
}
else
{
r = (x + abs_z) / (abs_z - x);
angle = coeff_2 - coeff_1 * r;
}
if (z < 0)
{
return int(-angle);
}
else
{
return int(angle);
}
}

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
dt = float(SetDelay)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}
``````

Are you saying that this is a working code? If so, good job!

I am interested in the code because I bought the same IMU and have had some issues compiling the code examples.

In my case it get an error saying LSM303 compass; L3G4200D gyro; “does not define a type”. I guess I have a problem with the library installation.

Have you had any problems getting the code to work?

Thanks for sharing!

you have to get the LSM303 and L3G4200D libraries from here: https://github.com/pololu/L3G4200D and https://github.com/pololu/LSM303

yes it reads the sensors correctly, but thats the easy part; its not combining the Gyro and ACC together correctly. <-harder part. i was going to implement the I2C my self, but im already learning enough new stuff in this project; i think ill wait till after the code works as it is, then go back and do I2C without the libraries(for gyro and ACC).

as of now i think the arctan and khalman functions are NOT functioning improperly. im sure someone out there in the wonderful world of arduino (lol) knows what im doing wrong. could you help?

I can't personally help much but, maybe this link will help. P.S. it is spelled Kalman or Kalman Filter, I had to look it up myself.

http://arduino.cc/forum/index.php/topic,58048.0.html

Hello, I am working with the MinIMU-9. It has got the same sensors.

On http://www.pololu.com/catalog/product/1264 you can get the librarys and a sample code + AHRS software.

I only invested some hours because I had not enough time for it, but my first results were looking promising.

best regards

cyclegadget: I can't personally help much but, maybe this link will help. P.S. it is spelled Kalman or Kalman Filter, I had to look it up myself.

http://arduino.cc/forum/index.php/topic,58048.0.html

hmm yes i saw that. but i have question. do i need to do the sensitivity stuff since im using a digital sensor?

thanks. it helped

If we could assume for a moment that your code is working, there is a few things to try. Typically, when you get excessive response from a PID loop that is called overshoot. It can be solved in your code with these values:

``````//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative
``````

Try starting out with large changes in the values. I don’t know what is allowed, but I would put a 30 in for Kp and then try a 60. If you see a change in performance it will give you a direction to go in.

Here is a description of each tuning parameter.:

``````Proportion = Error; //"makes a change to the output that is proportional to the current error value"
Integral += Error; //“if you aren’t quite getting to target, keep increasing over a period of time until you get there”
Derivative = (Error - PrevError); //"how fast a change in angle occurs"
``````

You might also want to google “how to tune a PID loop”.

thanks for the explanation on PID tuning, but that doesn't matter right now as the the correct values arent getting to the PID loop.

thanks for the tuning tips though! :) i believe ive made some progress with the ACC angle calculations though. anyone feel free to comment :)

also, i may have had the pid loop incorrectly set at the time i noticed this, but what does it mean when the PID output lowers each time PID is ran even when the input value is the same?

`int GyroRate = 0;`

I believe Gyro's return a figure based on rate of rotation. It is based on degrees rotation per second or some other time angle base. The result is when your device is not rotating the Gyro Rate is going to go to 0. Your PID uses both the GYRO and ACC to figure correction factor.

To put very simply it would be similar to saying PID = rate of rotation "Gyro" + angle of board based on ACC.

sirbow2: i wasnt really expecting the PID to work because i have to change Kp Ki and Kd for fine stabilization. but for some reason the Khalman equations didnt combine the GYRO and ACC correctly and the values were way off. if i hold the imu in one place, the value will stay at once place. but if i move the imu, the value jumps, and then stabilizes to value around what it should be, ish.

Does Kahlman assume the system is a linear system? I imagine that there are highly non-linear things going on inside a balance bot.

cyclegadget: `int GyroRate = 0;`

I believe Gyro's return a figure based on rate of rotation. It is based on degrees rotation per second or some other time angle base. The result is when your device is not rotating the Gyro Rate is going to go to 0. Your PID uses both the GYRO and ACC to figure correction factor.

To put very simply it would be similar to saying PID = rate of rotation "Gyro" + angle of board based on ACC.

yeah um but its kalman that combines gyro+acc. PID finds the difference between what the value should be and what is and the last value to smooth output.

Sorry, I messed up. I meant to say... To put very simply it would be similar to saying error = rate of rotation "Gyro" + angle of board based on ACC. As the error decreases so does the PID correction.

I am going to load the code and try to test it with my board. I will see if I can figure this out.

I would like someone else to join in, as I am not a code expert. Most of my experience has been with Yokogawa, and Rosemont sensors, transmitters, and controllers. They do not require me to understand code at least for the things that I do.

this is so weird! i copied the code for getting the y angle from here: http://arduino.cc/forum/index.php/topic,58048.0.html

``````R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
``````

since im using digital acc, it is already in raw g(gravity) output from the sensor, so if i put the x,y, and z values into the code above, it should give me a halfway(drift etc) accurate reading of the y angle!

here is the full code(calculates y angle and thats it, no servo PID or kalman code in it):

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int Gyro_Y_Rate = 0;
int Acc_Y_Angle = 0;
int CalibSensor[4] = {
0,0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU
int yAngle =0; //used in y axis degree calculations

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(9600);

//LM303 acc setup
Wire.begin();
compass.init();

IMUCenter(); //center IMU at current position

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

IMUCalc(); //final current angle of the bot

Serial.println(Acc_Y_Angle);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

void IMUCenter()
{

//store values for centering
CalibSensor[0] = (int)gyro.g.y;
CalibSensor[1] = (int)compass.a.x;
CalibSensor[2] = (int)compass.a.y;
CalibSensor[3] = (int)compass.a.z;
}

int IMUCalc()
{
//IMU------------------------------------------------------------
Acc_Y_Angle = YAngleCalc(((int)compass.a.x - CalibSensor[1]), ((int)compass.a.y - CalibSensor[2]), ((int)compass.a.z - CalibSensor[3]));  //calculates acc angle
Gyro_Y_Rate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
}

int YAngleCalc(int x, int y, int z) //finds Acc angle
{

yAngle = acos(y/(sqrt(pow(x,2)+pow(y,2)+pow(z,2))))*RAD_TO_DEG-90; //force vector + degree format

if(z < 0)//360 degrees
{
if(yAngle < 0)
{
yAngle = -180-yAngle;
}
else
{
yAngle = 180-yAngle;
}
}
return yAngle;
}
``````

I ran your sketch without the IR library and I found some confusing output. I added some Serial.print to show both platform angle "PA" and Output "OP". I looks kind of like a roll over problems using INT, I am not sure. Look at how the output jumps form - to +. I am holding the board steady the whole time. This is after a long count up in PA and OP.

``````PA 337
OP -31504

PA 333
OP -31819

PA 328
OP -32117

PA 325
OP -32411

PA 321
OP -32709

PA 318
OP 32535

PA 312
OP 32250

PA 307
OP 31983

PA 302
``````

Here is your code with the IR part commented out and some Serial.prints added to see PA an OP. I also raised the baud rate up to 56700.

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
//#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(57600);
delay(1000);

//LM303 acc setup
Wire.begin();
compass.init();
delay(500);
LServoMtr.attach(LServoMtrPin); //attach then set servos to center
LServoMtr.write(90);
RServoMtr.attach(RServoMtrPin);
RServoMtr.write(90);

IMUCenter(); //center IMU at current position
delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

PlatformAngle = IMUCalc(); //final current angle of the bot

Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly
Serial.println("  ");
Serial.print("PA ");
Serial.println(PlatformAngle);
Serial.print("OP ");
Serial.println(Output);

LServoMtr.write(Output + offset);
RServoMtr.write(Output + offset);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
//IMU------------------------------------------------------------
AccAngle = arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle
GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
CalibSensor[1] = (int)gyro.g.y; //store values for centering
CalibSensor[2] = (int)compass.a.x;
CalibSensor[3] = (int)compass.a.z;
}

int PIDCalc()
{
//PID-------------------------------------------------------------
Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
Proportion = Error; //"makes a change to the output that is proportional to the current error value"
Integral += Error; //“if you aren’t quite getting to target, keep increasing over a period of time until you get there”
Derivative = (Error - PrevError); //"how fast a change in angle occurs"

//calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
//the program and the resulting values from Proportion, Integral, and Derivative calculations
Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
PrevError = Error; //puts current error in another value for next loop to use.
return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
int coeff_1 = 128;
int coeff_2 = 3*coeff_1;
float abs_z = abs(z)+1e-10;
float r, angle;

if (x >= 0)
{
r = (x - abs_z) / (x + abs_z);
angle = coeff_1 - coeff_1 * r;
}
else
{
r = (x + abs_z) / (abs_z - x);
angle = coeff_2 - coeff_1 * r;
}
if (z < 0)
{
return int(-angle);
}
else
{
return int(angle);
}
}

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
dt = float(SetDelay)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}
``````

thanks.

stil doesnt matter if the y angle isnt right…

i may have something: from the pololu 9DOF AHRS

``````// LSM303 accelerometer: 8 g sensitivity
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer
``````

YESHHH that -256 made a world of difference.

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int AccYAngle = 0;
int CalibSensor[4] = {
0,0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X Yand Z
int SensorValues[4] ={
0,0,0,0}; //stores gyro y, ACC x y z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(9600);

//LM303 acc setup
Wire.begin();
compass.init();

IMUCenter(); //center IMU at current position
Serial.print("Calibration values: ");
for(int x=0; x<=3; x++)
{
Serial.print(x);
Serial.print(":");
Serial.print(CalibSensor[x]);
Serial.print(" ");
}

delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

//calculates acc angle
//gyro rate is already correct in SensorValues[0]
/*
if(SensorValues[3] < 0)//360 degrees
{
if(AccYAngle < 0)
{
AccYAngle = -180-AccYAngle;
}
else
{
AccYAngle = 180-AccYAngle;
}
} */

Serial.println(AccYAngle);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

{

SensorValues[0]=(int)gyro.g.y-256-CalibSensor[0]; //256 is for the gravity of the ACC
SensorValues[1]=(int)compass.a.x-256-CalibSensor[1];
SensorValues[2]=(int)compass.a.y-256-CalibSensor[2];
SensorValues[3]=(int)compass.a.z-256-CalibSensor[3];
}

void IMUCenter()
{

for(int x=0; x<=20; x++)
{
CalibSensor[0] += (int)gyro.g.y;
CalibSensor[1] += (int)compass.a.x;
CalibSensor[2] += (int)compass.a.y;
CalibSensor[3] += (int)compass.a.z;
}
//store values for centering after finding average(/20)
CalibSensor[0] = CalibSensor[0]/20;
CalibSensor[1] = CalibSensor[1]/20;
CalibSensor[2] = CalibSensor[2]/20;
CalibSensor[3] = CalibSensor[3]/20;
}
``````

now i need to do the following:
fix imu centering (y angle value reads ~105 when imu is in starting position)
doesnt have 360 deg of measurement
add kalman and PID back in and see what happens

OK found some things by commenting out and setting things to 0 in this part of the code:

``````int IMUCalc()
{
//IMU------------------------------------------------------------
AccAngle = 0;//arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle       <<<<<<<<<<<this line makes AccAngle = 0
GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}
``````

It seems the AccAngle portion causes something that looks like drift. When I set it to 0 the output and plat angle hold stable. Something in that line is incorrect. Why are we using terms like compassa.z when speaking of an accelerometer?

Here is the code with the comment to 0.

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
//#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(57600);
delay(1000);

//LM303 acc setup
Wire.begin();
compass.init();
delay(500);
LServoMtr.attach(LServoMtrPin); //attach then set servos to center
LServoMtr.write(90);
RServoMtr.attach(RServoMtrPin);
RServoMtr.write(90);

IMUCenter(); //center IMU at current position
delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

PlatformAngle = IMUCalc(); //final current angle of the bot

Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly
Serial.println("  ");
Serial.print("PA ");
Serial.println(PlatformAngle);
Serial.print("OP ");
Serial.println(Output);

LServoMtr.write(Output + offset);
RServoMtr.write(Output + offset);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
//IMU------------------------------------------------------------
AccAngle = 0;//arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle
GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
CalibSensor[1] = (int)gyro.g.y; //store values for centering
CalibSensor[2] = (int)compass.a.x;
CalibSensor[3] = (int)compass.a.z;
}

int PIDCalc()
{
//PID-------------------------------------------------------------
Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
Proportion = Error; //"makes a change to the output that is proportional to the current error value"
Integral += Error; //“if you aren’t quite getting to target, keep increasing over a period of time until you get there”
Derivative = (Error - PrevError); //"how fast a change in angle occurs"

//calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
//the program and the resulting values from Proportion, Integral, and Derivative calculations
Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
PrevError = Error; //puts current error in another value for next loop to use.
return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
int coeff_1 = 128;
int coeff_2 = 3*coeff_1;
float abs_z = abs(z)+1e-10;
float r, angle;

if (x >= 0)
{
r = (x - abs_z) / (x + abs_z);
angle = coeff_1 - coeff_1 * r;
}
else
{
r = (x + abs_z) / (abs_z - x);
angle = coeff_2 - coeff_1 * r;
}
if (z < 0)
{
return int(-angle);
}
else
{
return int(angle);
}
}

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
dt = float(SetDelay)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}
``````

I think you need this code somewhere in the sketch to replace your compass reads. Leave out the Z axis though. It is from Pololu.

``````// Reads x,y and z accelerometer registers
{

AN[3] = compass.a.x;
AN[4] = compass.a.y;
AN[5] = compass.a.z;
accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}
``````

I got the platform angle to hold almost steady! It was ramping up and down for no reason. The output still needs work but, it is getting closer. EDIT; The Platform angle is not true however, I am still trying to figure that out.
Here is the code portion that needed modified.

``````void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

``````

Whole code at the moment:

``````//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
//#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
Serial.begin(57600);
delay(1000);

//LM303 acc setup
Wire.begin();
compass.init();
delay(500);
LServoMtr.attach(LServoMtrPin); //attach then set servos to center
LServoMtr.write(90);
RServoMtr.attach(RServoMtrPin);
RServoMtr.write(90);

IMUCenter(); //center IMU at current position
delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
TimeOne = millis(); //recordes time at beginning of loop

PlatformAngle = IMUCalc(); //final current angle of the bot

Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly
Serial.println("  ");
Serial.print("PA ");
Serial.println(PlatformAngle);
Serial.print("OP ");
Serial.println(Output);

LServoMtr.write(Output + offset);
RServoMtr.write(Output + offset);

TimeTwo = millis(); //recordes time at end of loop
LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
//IMU------------------------------------------------------------
AccAngle = arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle
GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library
return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
CalibSensor[1] = (int)gyro.g.y; //store values for centering
CalibSensor[2] = (int)compass.a.x;
CalibSensor[3] = (int)compass.a.z;
}

int PIDCalc()
{
//PID-------------------------------------------------------------
Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
Proportion = Error; //"makes a change to the output that is proportional to the current error value"
Integral += Error; //“if you aren’t quite getting to target, keep increasing over a period of time until you get there”
Derivative = (Error - PrevError); //"how fast a change in angle occurs"

//calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
//the program and the resulting values from Proportion, Integral, and Derivative calculations
Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
PrevError = Error; //puts current error in another value for next loop to use.
return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
int coeff_1 = 128;
int coeff_2 = 3*coeff_1;
float abs_z = abs(z)+1e-10;
float r, angle;

if (x >= 0)
{
r = (x - abs_z) / (x + abs_z);
angle = coeff_1 - coeff_1 * r;
}
else
{
r = (x + abs_z) / (abs_z - x);
angle = coeff_2 - coeff_1 * r;
}
if (z < 0)
{
return int(-angle);
}
else
{
return int(angle);
}
}

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
dt = float(SetDelay)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}
``````