The direction of moving of bot doesn't change as the bot's orientation changes(float l's value is always -ve in applyPID(1)The values drifts to the extreme every time I run this )., I guess this is due to my mistake in the calculation of angles or in the filter that I have applied which I have been trying to but can't find it. 2)I have tried to calibrate MPU many times but failed as the x-axis value is showing 0 only when I tilt the bot to 12-14 degrees on x-axis(data from only accelometer),(if you can just tell me the variable to modify)
#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ,elapsedTime;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
double x,y,z,dx,dy,dz,ex,ey,ez,desx=0,desy=0,desz=0;
float PID, PWM, error, previous_error,timePrev,time1;
float pid_p;
float pid_i;
float pid_d;
float rad_to_deg = 180/3.141592654;
/////////////////PID CONSTANTS/////////////////
double kp=0.4;kp=2;
double ki=0;
double kd=2;kd=0;
int motor1pin1 = 2;
int motor1pin2 = 3;
int led = 9; // the PWM pin the LED is attached to
int brightness = 0; // how bright the LED is
int fadeAmount = 5;
void setup()
{
Serial.begin(9600115200);
Wire.begin();
pinMode(motor1pin1, OUTPUT);
pinMode(motor1pin2, OUTPUT);
pinMode(9, OUTPUT);
pinMode(13, OUTPUT);
setupMPU();
delay(500);
}
void loop()
{
timePrev = time1; // the previous time is stored before the actual time read
time1 = millis(); // actual time read/
elapsedTime = (time1 - timePrev) / 1000;
recordAccelRegisters();
recordGyroRegisters();
processangle();
Apply_PID();
printData();
delay(100);
}
void right(int PWM)
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
digitalWrite(13, HIGH);
Serial.println("RIGHT");
}
void left(int PWM)
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
Serial.println("LEFT");
}
void stopper()
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
Serial.println("STOPPED");
}
void setupMPU()
{
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}
void recordAccelRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processAccelData();
}
void processAccelData()
{
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
x= (RAD_TO_DEG * (atan2(-gForceY, -gForceZ)+PI));
y= RAD_TO_DEG * (atan2(-gForceX, -gForceZ)+PI);
z= RAD_TO_DEG * (atan(sqrt(square(-gForceY) + square(-gForceX)) / -gForceZ));
/* x=map(x,340,11,-90,90);*/
Serial.print(" AccelX=");
Serial.print(x);
}
void recordGyroRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processGyroData();
}
void processGyroData()
{
rotX = (gyroX / 131.0;0);
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.print(rotY);
Serial.print(" Z="GyroX=");
Serial.printprintln(rotZrotX*elapsedTime);
}
void processangle()
{
dx=0.98 *(dx + rotX*elapsedTime) + 0.02*x;
dy=0.98 *(dy + rotY*elapsedTime) + 0.02*y;
dz=0.98 *(dz + rotZ*elapsedTime) + 0.02*z;
ex=dx-desx;
ey=dy-desy;
ez=dz-desz;
/*Serial.print(" EX=");
Serial.print(ex);
"After Serial.print("Filter dX="Angle");
Serial.println(dxex);*/;
}
void Apply_PID()
{
float l=(float)ex;
pid_p=kp*l;
pid_i = pid_i+(ki*l);
pid_d = kd *((l - previous_error)/elapsedTime);
PID = pid_p + pid_i + pid_d;
/*Serial.print("I=");
Serial.println(pid_i);
Serial.print("d=");
Serial.println(pid_d);
Serial.print("p=");
Serial.println(pid_p);*/
Serial.print("PID");
Serial.println(PID);
if(l<0l>5.5 || l<5.5)
{
if(l<0)
right(PID);
else
left(PID);
}
else
{
leftstopper(PID);
}
previous_error = l;
}
void printData()
{
/*Serial.print("Gyro (deg)");
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.print(rotY);
Serial.print(" Z=");
Serial.print(rotZ);
Serial.print(" Accel (g)");
Serial.print(" X=");
Serial.print(gForceX);
Serial.print(" Y=");
Serial.print(gForceY);
Serial.print(" Z=");
Serial.println(gForceZ);*/
/* Serial.print(" PWM");
Serial.println(PID);*/
}