Go Down

### Topic: PID tuning Quadrocopter (Read 1 time)previous topic - next topic

#### anjos

##### Jan 24, 2012, 10:05 pmLast Edit: Jan 25, 2012, 07:11 am by anjos Reason: 1
Hello!

We have big problems with the PID tuning parameters for our quadrocopter.

We have, filtered the acc and gyro readings and ploted them with processing.
A lot of testing but the only thing we can get is a oscillation.

Have missed something essential?

Posting the code.

Please give some feedback or tips, we have been testing for 35-40 hours. We are stuck here.

Code: [Select]
`int gyroPinX = 2; //compass.a.x             int gyroPinY = 3; //compass.a.x             float gyroVoltage = 5;         //Gyro is running at 5Vfloat gyroZeroVoltageX, gyroZeroVoltageY;//Gyro is zero ..setupfloat gyroSensitivity = .00333;  //Our example gyro is 3,3mV/deg/sec  ungefär.float rotationThreshold = 3;   //Minimum deg/sec to keep track of - helps with gyro driftingfloat gyroRateX,gyroRateY;int angleX,angleY,accZeroValueX,accZeroValueY;float currentAngleX,currentAngleY;   //Keep track of our current angleint inByte = 0;  int ground =1150;int incomingByte; //test int och floatfloat gyroAngle;#include <Servo.h> #include <Wire.h>#include <LSM303DLH.h>#include <PID_v1.h>LSM303DLH compass;//Define Variables we'll be connecting todouble Setpoint, Input, Output;double SetpointY, InputY, OutputY;//Define the aggressive and conservative Tuning Parametersdouble consKp=40.0, consKi=0.0, consKd=4.0;double consKpY=2.0, consKiY=0.2, consKdY=0.4;PID xPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);PID yPID(&InputY, &OutputY, &SetpointY, consKpY, consKiY, consKdY, DIRECT);Servo esc1x,esc2x,esc1y,esc2y;void setup() {  Serial.begin (9600);  gyroSetup();   Wire.begin();  compass.enable();   accSetup();  esc1x.attach(6);  esc2x.attach(10);    esc1y.attach(9);  esc2y.attach(11);  delay(10);    esc1x.writeMicroseconds(0);  esc2x.writeMicroseconds(0);  esc1y.writeMicroseconds(0);  esc2y.writeMicroseconds(0);  delay(3000);  esc1x.writeMicroseconds(1400);  esc2x.writeMicroseconds(1400);  esc1y.writeMicroseconds(1400);  esc2y.writeMicroseconds(1400);  esc1x.writeMicroseconds(1000);esc2x.writeMicroseconds(1000);    Setpoint=0;  //myPID.SetControllerDirection(REVERSE);    xPID.SetOutputLimits(-40, 40);    xPID.SetMode(AUTOMATIC);    xPID.SetTunings(consKp, consKi, consKd);    yPID.SetOutputLimits(-60, 60);    yPID.SetMode(AUTOMATIC);    yPID.SetTunings(consKpY, consKiY, consKdY);}void loop() {  Xangle (); // return X angle as float currentAngleXInput=currentAngleX;xPID.Compute();   /*if (Output>=0){esc1x.writeMicroseconds(ground);esc2x.writeMicroseconds(ground+Output);}else{esc1x.writeMicroseconds(ground-Output); //--=+esc2x.writeMicroseconds(ground);}*/Yangle ();// return Y angle       -||-InputY=currentAngleY;if (InputY>3 || InputY<-3){  yPID.SetTunings(consKpY, consKiY, consKdY); // felet större än...}else{  yPID.SetTunings(consKpY/2, consKiY, consKdY);}yPID.Compute();if (OutputY>=0){esc1y.writeMicroseconds(ground+OutputY);esc2y.writeMicroseconds(ground);}else{esc1y.writeMicroseconds(ground); //--=+esc2y.writeMicroseconds(ground-OutputY);}  /*esc1y.writeMicroseconds(1000);esc2y.writeMicroseconds(1000);*/ serialRead(); int intprint=currentAngleY;int intprint2=OutputY;Serial.print(intprint); Serial.print(","); Serial.println(intprint2);delay(10);}void gyroSetup(){  delay(10);  gyroZeroVoltageX= (analogRead(gyroPinX) * gyroVoltage) / 1023;  delay(10);  gyroZeroVoltageY= (analogRead(gyroPinY) * gyroVoltage) / 1023;  delay(10);}void accSetup(){  compass.read();  delay(5);  accZeroValueX=compass.a.x/11.67;  accZeroValueY=compass.a.y/11.67;}void Xangle () {  gyroRateX = (analogRead(gyroPinX) * gyroVoltage ) / 1023;  gyroRateX -= gyroZeroVoltageX;  gyroRateX /= gyroSensitivity;    if (gyroRateX >= rotationThreshold || gyroRateX <= -rotationThreshold) {    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)    //Acc reading    compass.read();    angleX=(-compass.a.x/11.67)+accZeroValueX;    gyroRateX /= 55; // konvertera till grafiskt korrekt    currentAngleX = 0.98 * (currentAngleX + gyroRateX) + 0.02 * angleX;    }   }void Yangle() { // copy of void Xangle    gyroRateY = (analogRead(gyroPinY) * gyroVoltage ) / 1023;  gyroRateY -= gyroZeroVoltageY;  gyroRateY /= gyroSensitivity;    if (gyroRateY >= rotationThreshold || gyroRateY <= -rotationThreshold) {    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)    //Acc reading    compass.read();    angleY=(compass.a.y/11.67)-accZeroValueY;    gyroRateY /= 55; // konvertera till grafiskt korrekt    currentAngleY = 0.98 * (currentAngleY + gyroRateY) + 0.02 * angleY;    }   }void serialRead() {   if (Serial.available() > 0) {    // read the oldest byte in the serial buffer:    incomingByte = Serial.read();    // if it's a capital H (ASCII 72), turn on the LED:    if (incomingByte == 'H') {      ground=1500;esc1x.writeMicroseconds(1550);esc2x.writeMicroseconds(1550);/*    consKpY=consKpY+0.1;   yPID.SetTunings(consKpY, consKiY, consKdY);  */   delay(100);  }            if (incomingByte == 'L') {      ground=1100;esc1x.writeMicroseconds(1000);esc2x.writeMicroseconds(1000);      delay(50);    }        }}`

#### PeterH

#1
##### Jan 24, 2012, 10:48 pm
Two things.

When posting code, please enclose the code in [ code ] and [ / code ] tags (click the '#' button in the editor) to stop the forum from munging your code. Could you delete your posted code and re-post it in code tags?

Secondly, in order to diagnose problems with the PID settings you need to provide the input and output readings for the PID algorithm. I recommend you limit yourself to dealing with one degree of freedom at a time (restrain your vehicle on one axis while you tune the algorithm to control it on the other axis).
I only provide help via the forum - please do not contact me for private consultancy.

Go Up

Please enter a valid email to subscribe