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