Author Topic: Problems with tuning PID(Arduino)  (Read 510 times)

0 Members and 1 Guest are viewing this topic.

Offline MarkusAndTopic starter

  • Contributor
  • Posts: 37
  • Country: fi
Problems with tuning PID(Arduino)
« on: May 20, 2020, 08:39:47 pm »
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: [Select]
/* 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
 

Offline Mechatrommer

  • Super Contributor
  • ***
  • Posts: 11714
  • Country: my
  • reassessing directives...
Nature: Evolution and the Illusion of Randomness (Stephen L. Talbott): Its now indisputable that... organisms “expertise” contextualizes its genome, and its nonsense to say that these powers are under the control of the genome being contextualized - Barbara McClintock
 


Share me

Digg  Facebook  SlashDot  Delicious  Technorati  Twitter  Google  Yahoo
Smf