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
There was an error while thanking
Thanking...

Go to full version
Powered by SMFPacks Advanced Attachments Uploader Mod