Pages: [1]   Go Down
Author Topic: PID tuning Quadrocopter  (Read 1041 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:

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);
    }
    
    }
}

« Last Edit: January 25, 2012, 01:11:49 am by anjos » Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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).
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Pages: [1]   Go Up
Jump to: