Mex1005
September 22, 2015, 5:38am
#1
Buenas tardes,
Bueno después de varias búsquedas e intentos no logra dar con una solución para el programa PID que ando tratando de entender y desarrollar. Mi problema es que parece ser que el PID funciona a la perfección pero en cuanto se realiza una pequeña variación con el sensor IMU que utilizo para la estabilización del Drone hace unos picos muy extraños que impiden que se estabilice por completo haciéndolo permitirme la redundancia muy inestable. No se si esto se deba a alguna linea de código que puse de mas o que no e puesto...
Quisiera ver si me pudieran dar alguna guía o algún consejo respecto a este problema que tengo, a como lo veo, no se si sea así, ya solucionando ese problema volaría el Drone a la perfección.
De ante mano muchas gracias por su atención y tiempo.
P.D: Les adjunto el código que estoy utilizando:
Mex1005
September 22, 2015, 5:39am
#2
#include <PID_v1.h>
#include <Servo.h>
#include <Wire.h>
#include <Kalman.h>
#define RESTRICT_PITCH
Kalman kalmanX;
Kalman kalmanY;
double Setpointx, Outputx;
double Setpointy, Outputy;
double Inputx, Inputy;
double Kp = 0.5;
double Ki = 0.4;
double Kd = 0.6;
PID myPIDx(&Inputx, &Outputx, &Setpointx, Kp, Ki, Kd, DIRECT);
PID myPIDy(&Inputy, &Outputy, &Setpointy, Kp, Ki, Kd, DIRECT);
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
//////////////////// POTENCIOMEROS ///////////////////////////////////////
//const int analogInPin1 = A8;
//const int analogOutPin1 = 4;
//const int analogInPin2 = A9;
//const int analogOutPin2 = 3;
//int sensorValue1 = 0; // valor leido desde el potenciometro
//int outputValue1 = 0; // valor de la salida PWM (salida analógica)
//int sensorValue2 = 0; // valor leido desde el potenciometro
//int outputValue2 = 0; // valor de la salida PWM (salida analógica)
//////////////////////////////////////////////////////////////////////////
int cosix;
int cosiy;
int pos = 0;
int cont = 0;
int valmedia = 40;
char rxChar;
int val = 0;
int valx = 0;
int valy = 0;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Make calibration routine
Mex1005
September 22, 2015, 5:40am
#3
void setup() {
Serial.begin(9600);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
// PID x
Inputx = kalAngleX; //Se pone comovalor el potenciometro para el PID
// Inputx = kalAngleX;
Setpointx = 0;
myPIDx.SetMode(AUTOMATIC);
myPIDx.SetSampleTime(50);
myPIDx.SetOutputLimits(-20,20);
// PID y
Inputy = kalAngleY; //Se pone comovalor el potenciometro para el PID
// Inputy = kalAngleY;
Setpointy = 0;
myPIDy.SetMode(AUTOMATIC);
myPIDy.SetSampleTime(50);
myPIDy.SetOutputLimits(-20,20);
myservo3.attach(5);
myservo4.attach(6);
myservo1.attach(9);
myservo2.attach(10);
}
void Control_Angulo()
{
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
// Inputx = kalAngleX;
// Inputy = kalAngleY;
Inputx = kalAngleX;
Inputy = kalAngleY;
//
// cosix = Inputx;
// cosiy = Inputy;
//
// Inputx = cosix;
// Inputy = cosiy;
myPIDx.Compute();
myPIDy.Compute();
}
void Control_Motor()
{
double cositas1 = val + pos + (+ Outputx - Outputy)/2;
double cositas2 = val + pos + (+ Outputx + Outputy)/2;
double cositas3 = val + pos + (- Outputx + Outputy)/2;
double cositas4 = val + pos + (- Outputx - Outputy)/2;
if(cositas1 <= 47 && cont == 1)
cositas1 = 47;
if(cositas2 <= 47 && cont == 1)
cositas2 = 47;
if(cositas3 <= 47 && cont == 1)
cositas3 = 47;
if(cositas4 <= 47 && cont == 1)
cositas4 = 47;
if(cositas1 >= 179)
cositas1 = 180;
if(cositas2 >= 179)
cositas2 = 180;
if(cositas3 >= 179)
cositas3 = 180;
if(cositas4 >= 179)
cositas4 = 180;
myservo1.write(cositas1);
myservo2.write(cositas2);
myservo3.write(cositas3);
myservo4.write(cositas4);
}
Mex1005
September 22, 2015, 5:41am
#4
void loop() {
Control_Angulo();
////////////////////////// POTENCIOMETROS ////////////////////////////////////////
// // lectura de la entrada analógica
// sensorValue1 = analogRead(analogInPin1);
// // ubica el valor de la entrada en el rango de la salida analógica
// outputValue1 = map(sensorValue1, 0, 1023, -20, 20);
// // cambia el valor de salida analógica
// analogWrite(analogOutPin1, outputValue1);
//
// // lectura de la entrada analógica
// sensorValue2 = analogRead(analogInPin2);
// // ubica el valor de la entrada en el rango de la salida analógica
// outputValue2 = map(sensorValue2, 0, 1023, -20, 20);
// // cambia el valor de salida analógica
// analogWrite(analogOutPin2, outputValue2);
//////////////////////////////////////////////////////////////////////////////////
// Si el Dron se voltea se apagaranlos motores
// if(valx >= 50 || valy >= 50 || valx <= -50 || valy <= -50)
if(cont == 20)
{
val = 20;
pos = 0;
myservo1.write(val);
myservo2.write(val);
myservo3.write(val);
myservo4.write(val);
cont = 0;
}
else
{
if(Serial.available())
{
rxChar = Serial.read();
switch(rxChar)
{
// ENCENDIDO
case 'O':
val = 0;
val = valmedia + 7;
myservo1.write(val);
myservo2.write(val);
myservo3.write(val);
myservo4.write(val);
cont = 1;
break;
// APAGADO
case 'F':
val = 20;
pos = 0;
cont = 0;
Kp = 1;
Ki = 1;
Kd = 1;
myservo1.write(val);
myservo2.write(val);
myservo3.write(val);
myservo4.write(val);
break;
// ARRIBA
case 'U':
pos = pos + 5;
if(pos > 100)
pos = 100;
Control_Motor();
break;
// ABAJO
case 'D':
pos = pos - 5;
if(pos < 0)
pos =0;
Control_Motor();
break;
// GIRO DERECHA
case 'Y':
myservo1.write(val + pos + 10 - Outputx + Outputy);
myservo2.write(val + pos - 10 - Outputx - Outputy);
myservo3.write(val + pos + 10 + Outputx - Outputy);
myservo4.write(val + pos - 10 + Outputx + Outputy);
Control_Motor();
break;
// GIRO IZQUIRDA
case 'W':
myservo1.write(val + pos - 10 - Outputx + Outputy);
myservo2.write(val + pos + 10 - Outputx - Outputy);
myservo3.write(val + pos - 10 + Outputx - Outputy);
myservo4.write(val + pos + 10 + Outputx + Outputy);
Control_Motor();
break;
// ADELANTE
case 'A':
Setpointx = -17;
myPIDx.Compute();
Control_Motor();
break;
// ATRAS
case 'B':
Setpointx = 17;
myPIDx.Compute();
Control_Motor();
break;
// IZQUIERDA
case 'L':
Setpointy = -17;
myPIDy.Compute();
Control_Motor();
break;
// DERECHA
case 'R':
Setpointy = 17;
myPIDy.Compute();
Control_Motor();
break;
// BP
case 'Q':
Serial.println(" ");
break;
// BP
case 'Z':
Kp = Kp + .25;
myPIDx.Compute();
myPIDy.Compute();
break;
// BP
case 'X':
Kp = Kp - .25;
myPIDx.Compute();
myPIDy.Compute();
break;
// BP
case 'C':
Ki = Ki + .25;
myPIDx.Compute();
myPIDy.Compute();
break;
// BP
case 'V':
Ki = Ki - .25;
myPIDx.Compute();
myPIDy.Compute();
break;
// BP
case 'N':
Kd = Kd + .25;
myPIDx.Compute();
myPIDy.Compute();
break;
// BP
case 'M':
Kd = Kd - .25;
myPIDx.Compute();
myPIDy.Compute();
break;
}
}
}
if(cont == 1)
{
Setpointx = 0;
Setpointy = 0;
Control_Motor();
myPIDx.Compute();
myPIDy.Compute();
}
// Serial.print("Kp : ");
// Serial.print(cosa1);
// Serial.print(" ");
//
// Serial.print("Ki : ");
// Serial.print(cosa2);
// Serial.print(" ");
//
// Serial.print("KD : ");
// Serial.print(cosa3);
// Serial.print("\t\t");
// Serial.print("Input x: ");
// Serial.print(Inputx);
// Serial.print("\t");
//
// Serial.print("Input y: ");
// Serial.print(Inputy);
// Serial.print(" ----- \t");
Serial.print("Angulo X = " );
Serial.print(kalAngleX);
Serial.print(" ");
Serial.print("Angulo Y = " );
Serial.print(kalAngleY);
Serial.print(" ");
Serial.print(val + pos + (+ Outputx - Outputy)/2);
Serial.print(" ");
Serial.print(val + pos + (+ Outputx + Outputy)/2);
Serial.print(" ");
Serial.print(val + pos + (- Outputx + Outputy)/2);
Serial.print(" ");
Serial.print(val + pos + (- Outputx - Outputy)/2);
Serial.print(" \t");
Serial.print("Output x: ");
Serial.print(Outputx);
Serial.print(" \t");
Serial.print("Output y: ");
Serial.print(Outputy);
Serial.println(" ");
}
Mex1005
September 22, 2015, 5:42am
#5
Lo siento por tantos cuadros de respuesta para poder poner el código pero es la primera ves que realizo uno y no había como poner el código entero, de nuevo gracias por la atención y el tiempo.
leoniba
October 30, 2015, 2:17pm
#6
Hola estoy intentando hacer un drone, mi pregunat es, has conseguido con este codigo que funcione, lo estube mirando el codigo que tienes aqui pero no me aclaro