Hello!
So I'm currently working on a 2WD self balancing robot. It uses a MPU6050 which gives me gyro and accelerometer values. I'm then using a Kalman filter to get the pitch and roll. To get the robot to balance I'm using a PID control loop. Here is my code:
/* Self balancing robot.
* Author: Markus Andersson 2020
*/
#include <PID_v1.h>
#include <Wire.h>
#include <Kalman.h>
Kalman kalmanX;
Kalman kalmanY;
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float AX_angle, AY_angle, GX_angle, GY_angle;
float KangleX, KangleY;
float roll, pitch, yaw;
float elapsedTime, currentTime, previousTime;
// Motor control
const int LM1 = 2;
const int LM2 = 4;
const int RM1 = 6;
const int RM2 = 7;
const int LMpwm = 3;
const int RMpwm = 5;
const int ctrl5V = 10;
// PID
double setPoint;
double input = 0;
double output = 0;
double Kp = 180;
double Ki;
double Kd = 3;
PID pid(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);
void setup(){
Serial.begin(19200);
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission(true);
// Motor control
pinMode(LM1, OUTPUT);
pinMode(LM2, OUTPUT);
pinMode(RM1, OUTPUT);
pinMode(RM2, OUTPUT);
pinMode(LMpwm, OUTPUT);
pinMode(RMpwm, OUTPUT);
pinMode(ctrl5V, OUTPUT);
digitalWrite(ctrl5V, HIGH);
// PID
setPoint = input;
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
void loop(){
// Accelerometer angle:
Wire.beginTransmission(MPU);
Wire.write(0x3B); // ACC Address
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Kombinerar 2 mätvärden
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;
AX_angle = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 3.4;
AY_angle = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.9;
// Gyro angle:
previousTime = currentTime;
currentTime = millis();
elapsedTime = (currentTime - previousTime) / 1000;
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroX = GyroX - 0.28;
GyroY = GyroY + 0.26;
// Kalman filter
KangleX = kalmanX.getAngle(AX_angle, GyroX, elapsedTime);
KangleY = kalmanY.getAngle(AY_angle, GyroY, elapsedTime);
//Serial.print(KangleX);
input = KangleX;
if((setPoint - input) < 0 && output < 0)
Ki = 0;
if((setPoint - input) > 0 && output > 0)
Ki = 0;
else
Ki = 9; // Variable
pid.Compute();
if(output < 0)
forward();
else
backwards();
Serial.println(input);
}
void forward(){
digitalWrite(LM1, HIGH);
digitalWrite(LM2, LOW);
digitalWrite(RM1, LOW);
digitalWrite(RM2, HIGH);
analogWrite(LMpwm, -output);
analogWrite(RMpwm, -output);
}
void backwards(){
digitalWrite(LM1, LOW);
digitalWrite(LM2, HIGH);
digitalWrite(RM1, HIGH);
digitalWrite(RM2, LOW);
analogWrite(LMpwm, output);
analogWrite(RMpwm, output);
}
void halt(){
analogWrite(LMpwm, 0);
analogWrite(RMpwm, 0);
}
With the following values(Kp = 180; Ki = 9; Kd = 3) the robot balances well for a while. After about 30s it starts going back and forth(oscillating). The oscillations grow until it finally goes into one direction and falls over.
Which of the control values can prevent this from happening? I've tried adjusting these values for a couple of days but can't get the hang of it.
// Markus