Buenas jefes !
Si tengo tiempo probare el DMP no lo dudes.
He mejorado mi codigo y ahora estabiliza incluso con el cable del arduino conectado en el medio, aunque no es 100% estable.
Hay un valor que no acabo de tener claro; SetSampleTime(); Le he variado entre 500 y 5 notando la diferencia pero nose si entre 5 y 50 sera muy significativo.
Determina la frecuencia con la que evalúa el algoritmo PID. El valor predeterminado es 200 ms.
El codigo de lo que e echo
#include <PID_v1.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#include <Servo.h>
#define PINMOTOR 8 //pin conectado al ESC, en este caso CH_5 en ARDUPILOT corresponde al pin 8
#define MAXIMOPWM 160 // Son grados Podia llegar hasta 180,paramas seguridad lo dejo bajo
#define MINIMOPWM 30 // por si acaso empezar con un valor inferior, mi motor no arranca hasta 65
#define PINMOTOR2 9
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
float inverOutput;
boolean e=false;
float kp=0.9;
float ki=3.6;
float kd=0.2;
/* Motor Data */
int pulsoMotor = 30;
int pulsoMotor2 = 30;
Servo myservo; // creamos el motor como elemento en la libreria
Servo myservo2;
byte recibiendoByte ;
boolean iniciado = false;
boolean carga = false;
/* PID Data */
double Setpoint, Input, Output; //Define Variables we'll be connecting to
PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT); //Specify the links and initial tuning parameters
/* IMU Data */
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
void setup() {
Serial.begin(115200);
Setpoint = 180;
myservo.attach(PINMOTOR); // inicializo el ESC en el pin determinado
myservo2.attach(PINMOTOR2);
Serial.println(" Comienzo del test"); //
Serial.println (" Pulsar 'A' para arrancar \n Cuando escuche el pitido de confirmación");
while ( iniciado==false ){
myservo.write(0);
myservo2.write(0); // Armado
recibiendoByte = Serial.read(); // Leemos el Byte recibido
if (recibiendoByte == 65 || recibiendoByte ==97) { // A o a Mayusculas o minusculas
iniciado=true;
}
}
Serial.println (" Pulsar 'C' para cargar");
while ( carga==false ){
myservo.write(30); // Armado
myservo2.write(30);
recibiendoByte = Serial.read(); // Leemos el Byte recibido
if (recibiendoByte == 67 || recibiendoByte ==99) { // C o c Mayusculas o minusculas
carga=true;
}
}
//turn the PID on
myPID.SetMode(AUTOMATIC);
// myPID.SetOutputLimits(90,179);
myPID.SetSampleTime(10);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
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]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
timer = micros();
}
void loop() {
/* 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]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
double gyroXrate = (double)gyroX / 131.0;
double gyroYrate = -((double)gyroY / 131.0);
gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
timer = micros();
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
#endif
//Serial.print(accXangle); Serial.print("\t");
//Serial.print(gyroXangle); Serial.print("\t");
//Serial.print(compAngleX); Serial.print("\t");
//Serial.print(kalAngleX); Serial.print("\t");
//Serial.print("\t");
//Serial.print(accYangle); Serial.print("\t");
//Serial.print(gyroYangle); Serial.print("\t");
//Serial.print(compAngleY); Serial.print("\t");
//Serial.print(kalAngleY); Serial.print("\t");
//Serial.print("\r\n");
//modificar valores
recibiendoByte = Serial.read();
if(recibiendoByte == 112){ // p minuscula
kp= kp - 0.1;
Serial.print (" kp - ");
Serial.println (kp);
}
if(recibiendoByte == 80){ // P mayuscula
kp = kp + 0.1;
Serial.print (" kp + ");
Serial.println (kp);
}
if(recibiendoByte == 105){ // i minuscula
ki = ki - 0.1;
Serial.print (" ki - ");
Serial.println (ki);
}
if(recibiendoByte == 73){ // I mayuscula
ki = ki + 0.1;
Serial.print (" ki + ");
Serial.println (ki);
}
if(recibiendoByte == 100){ // d minuscula
kd = kd - 0.1;
Serial.print (" kd - ");
Serial.println (kd);
}
if(recibiendoByte == 68){ // D mayuscula
kd = kd + 0.1;
Serial.print (" kd + ");
Serial.println (kd);
}
//codigo parada de motores
if(recibiendoByte == 115){ // s -> parada
Serial.print ("entrado");
myservo.write(30);
myservo2.write(30);
e = false;
while ( e==false ){
recibiendoByte = Serial.read(); // Leemos el Byte recibido
if (recibiendoByte == 67 || recibiendoByte ==99) { // C o c Mayusculas o minusculas
e=true;
}
}
}
// PID
Input = kalAngleX;
myPID.Compute();
pulsoMotor2= map (Output,0,255,94,98);
pulsoMotor= 192 - pulsoMotor2 ;
myservo.write(pulsoMotor);
myservo2.write(pulsoMotor2);
/*delay(250);
Serial.print("Velocidad del pulso-1-> ");
Serial.println (pulsoMotor);
Serial.print("PID 11111 --> ");
Serial.println (inverOutput);
Serial.print("Velocidad del pulso-2-> ");
Serial.println (pulsoMotor2);
Serial.print("PID 22222 --> ");
Serial.println (Output);
*/
}
de momento han quedado asi los valores
float kp=0.9;
float ki=3.6;
float kd=0.2;
