Go Down

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

anjos

Jan 24, 2012, 10:05 pm Last 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.

Any questions about the code, ask and I tell you...


Code: [Select]


int gyroPinX = 2; //compass.a.x            
int gyroPinY = 3; //compass.a.x            


float gyroVoltage = 5;         //Gyro is running at 5V
float gyroZeroVoltageX, gyroZeroVoltageY;//Gyro is zero ..setup
float 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 drifting
float gyroRateX,gyroRateY;
int angleX,angleY,accZeroValueX,accZeroValueY;
float currentAngleX,currentAngleY;   //Keep track of our current angle
int inByte = 0;  
int ground =1150;
int incomingByte;




//test int och float
float gyroAngle;

#include <Servo.h>
#include <Wire.h>
#include <LSM303DLH.h>
#include <PID_v1.h>

LSM303DLH compass;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double SetpointY, InputY, OutputY;
//Define the aggressive and conservative Tuning Parameters

double 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 currentAngleX
Input=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

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