Problems with tuning PID(Arduino) - Page 1
Forum section: electronics / projects

author: MarkusAnd
author_rank: Contributor
timestamp: May 20, 2020, 08:39:47 pm
content: 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: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 addressfloat 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 controlconst 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;// PIDdouble 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 controlpinMode(LM1, OUTPUT);pinMode(LM2, OUTPUT);pinMode(RM1, OUTPUT);pinMode(RM2, OUTPUT);pinMode(LMpwm, OUTPUT);pinMode(RMpwm, OUTPUT);pinMode(ctrl5V, OUTPUT);digitalWrite(ctrl5V, HIGH);// PIDsetPoint = input;pid.SetMode(AUTOMATIC);pid.SetSampleTime(10);pid.SetOutputLimits(-255, 255);}void loop(){// Accelerometer angle:Wire.beginTransmission(MPU);Wire.write(0x3B); // ACC AddressWire.endTransmission(false);Wire.requestFrom(MPU, 6, true);AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Kombinerar 2 mätvärdenAccY = (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 filterKangleX = 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;elseKi = 9; // Variablepid.Compute();if(output < 0)forward();elsebackwards();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

author: Mechatrommer
author_rank: Super Contributor
timestamp: May 20, 2020, 10:09:58 pm
content: http://faculty.mercer.edu/jenkins_he/documents/TuningforPIDControllers.pdf