Here i am attached my code for stabilizing 3-axis platform using MPU6050.....but i didn't get proper control signals which are to be given three servo motor to control the platform..
Please anyone check my code and tell me where i have to modify....please help me..
(CODE TAGS PLEASE!)
#include <kalman.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#include <LiquidCrystal.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
const uint8_t IMUAddress = 0x68;
int pwmpin1= 8;
double out1= 0;
int pwmpin2= 9;
double out2 =0;
int pwmpin3= 10;
double out3 =0;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double accZangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double gyroZangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double compAngleZ = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
double kalAngleZ;
double a;
double b;
double c;
uint32_t timer;
void setup() {
Serial.begin(115200);
Wire.begin();
i2cWrite(0x6B,0x00); // Disable sleep mode
if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("MPU-6050 with address 0x"));
Serial.print(IMUAddress,HEX);
Serial.println(F(" is not connected"));
while(1);
}
kalmanX.setAngle(180); // Set starting angle
kalmanY.setAngle(180);
kalmanZ.setAngle(0);
timer = micros();
// declare pin 9 to be an output:
pinMode(8, OUTPUT);
analogWrite(8, 50);
pinMode(9, OUTPUT);
analogWrite(9, 50);
pinMode(10, OUTPUT);
analogWrite(10, 50);
// set up the LCD's number of columns and rows:
lcd.begin(16, 4);
// Print a message to the LCD.
lcd.print(" GYRO STABLE ");
delay(1000);
lcd.setCursor(0, 0);
//lcd.print(" ");
}
void loop() {
/* Update all the values */
// lcd.setCursor(0, 0);
//lcd.print("GYRO STABLE ");
//lcd.setCursor(0, 1);
//lcd.print("X-AXIS ");
//lcd.setCursor(0, 1);
//lcd.print(kalAngleX);
//delay(1);
//lcd.setCursor(7, 1);
// lcd.print(kalAngleY);
lcd.setCursor(0, 1);
lcd.print("X-");
lcd.setCursor(2, 1);
lcd.print(kalAngleX);
lcd.setCursor(-4, 2);
lcd.print("Y-");
lcd.setCursor(-2, 2);
lcd.print(kalAngleY);
lcd.setCursor(-4, 3);
lcd.print("Z-");
lcd.setCursor(-2, 3);
lcd.print(kalAngleZ);
lcd.setCursor(9, 1);
lcd.print("ER");
lcd.setCursor(12, 1);
lcd.print(a);
lcd.setCursor(5, 2);
lcd.print("ER");
lcd.setCursor(8, 2);
lcd.print(b);
lcd.setCursor(5, 3);
lcd.print("ER");
lcd.setCursor(7, 3);
lcd.print(c);
delay(100);
uint8_t* data = i2cRead(0x3B,14);
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);
/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accYangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);
double gyroZrate = -(double)gyroZ/131.0;
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
compAngleZ = (0.93*(compAngleZ+(gyroZrate*(double)(micros()-timer)/1000000)))+(0.07*accZangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
timer = micros();
a = (180 - kalAngleX);
b = (180 - kalAngleY);
c= (0 - kalAngleZ);
temp = ((double)tempRaw + 12412.0) / 340.0;
/* Print Data */
/*
Serial.print(accX);Serial.print("\t");
Serial.print(accY);Serial.print("\t");
Serial.print(accZ);Serial.print("\t");
Serial.print(gyroX);Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ);Serial.print("\t");
*/
/* Serial.print(accXangle);Serial.print("\t");
Serial.print(accYangle);Serial.print("\t");
Serial.print(gyroXangle);Serial.print("\t");
Serial.print(gyroYangle);Serial.print("\t");
Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleX);Serial.print("\t");
Serial.print(kalAngleY);Serial.print("\t");
//Serial.print(temp);Serial.print("\t");
Serial.print("\n");*/
out1 = (int) 255.0*((float)kalAngleX)/ 360.0;
// out=kalAngleX-41;
if(( out1 > 128) && ( out1 < 140))
{
analogWrite(pwmpin1,(out1 + 11));
}
else if(( out1 < 126) && ( out1 > 115))
{
analogWrite( pwmpin1,(out1 - 11) );
}
//else if(out < 99)
//{
// Serial.print(out);
// analogWrite( pwmpin, 127);
//}
//else if(out > 155){
// Serial.print(out);
// analogWrite( pwmpin, 127);
// }
else{
analogWrite( pwmpin1,out1);
}
delay(1); // The accelerometer's maximum samples rate is 1kHz
out2 = (int) 255.0*((float)kalAngleY)/ 360.0;
// out=kalAngleX-41;
if(( out2 > 128) && ( out2 < 140))
{
analogWrite(pwmpin2,(out2 + 11));
}
else if(( out2 < 126) && ( out2 > 115))
{
analogWrite( pwmpin2,(out2 - 11) );
}
//else if(out < 99)
//{
// Serial.print(out);
// analogWrite( pwmpin, 127);
//}
//else if(out > 155){
// Serial.print(out);
// analogWrite( pwmpin, 127);
// }
else{
analogWrite( pwmpin2,out2);
}
delay(1); // The accelerometer's maximum samples rate is 1kHz
out3 = (int) 255.0*((float)kalAngleZ)/ 360.0;
// out=kalAngleX-41;
if(( out3 > 128) && ( out3 < 140))
{
analogWrite(pwmpin3,(out3 + 11));
}
else if(( out3 < 126) && ( out3 > 115))
{
analogWrite( pwmpin3,(out3 - 11) );
}
//else if(out < 99)
//{
// Serial.print(out);
// analogWrite( pwmpin, 127);
//}
//else if(out > 155){
// Serial.print(out);
// analogWrite( pwmpin, 127);
// }
else{
analogWrite( pwmpin3,out3);
}
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data[i] = Wire.read();
return data;
}