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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy