| Electronics > Projects, Designs, and Technical Stuff |
| Problems with tuning PID(Arduino) |
| (1/1) |
| MarkusAnd:
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 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); } --- End code --- 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 |
| Mechatrommer:
http://faculty.mercer.edu/jenkins_he/documents/TuningforPIDControllers.pdf |
| Navigation |
| Message Index |