Aiuto PID Quadricottero

[Nazukao80]

Ciao a Tutti!

Volevo esporvi il mio problema:
Ho creato un programma per un quadricottero.....Ma ho problemi con il PID:

Questo è il codice di PID....Praticamente ottengo l'angolo di Pitch o di Roll e il PID mi calcola l'errore.

double Compute_PID(double angle)
{ // Ottengo Pitch e Roll
unsigned long now = millis(); // comincia a contare
double timeChange = (double)(now - lastTime); // Delta t calcolato
double error = SETPOINT - angle; // Calcolo l'errore: Vorrei rimanere a 0.00°
if (abs(error) < threshold){ // windup control
errSum += (error * timeChange);} // Sommo l'errore attuale con i precedenti : sistema integrale
else {
errSum = 0;
}
double dErr = (error - lastErr)/timeChange; // Rapporto incrementale del sistema derivativo
Output = kp * error + ki * errSum + kd * dErr; // Calcolo il PID, dovrà essere in ingresso ai motori
if (Output > VAL_MAX) Output= VAL_MAX;
if (Output < VAL_MIN) Output= VAL_MIN;
lastErr = error; // registro l'errore
lastTime = now; // registro il tempo
return Output;
}


Questo programma funziona alla perfezione tanto è vero che se vado a stampare su video i valori di PID + Throttle e comincio a inclinare il mio quadricottero, ottengo i valori esatti di accelerazione e decelerazione dei 4 motori ..quindi vuol dire che l'algoritmo del PID funziona. Però se accendo e testo l'autobilanciamento succede una cosa strana: ogni tot i motori accelerano e decelerano come se vedessero sempre un errore, anche quando il Quadricottero è in posizione orizzontale, vi posto il programma del flight control:


if (value[4] > 1520){ // 4th canale che serve per l'accensione e lo spegnimento 1000 = spento 2000 = acceso //

/************************************************** *************************************************/
/* // controllo volo // */
/************************************************** *************************************************/


throttle=map(value[1],THROTTLE_RMIN,THROTTLE_RMAX,THROTTLE_WMIN ,THROTTLE_WMAX); // mappo il canale di Throttle 1100-1400 //

// Situazione di Roll Comandata //
if (value[3] < 1490){
AccX = map(value[3], VELO_RMIN, 1500, ACC_MIN, 0);
}
else{
AccX = map(value[3], 1500, VELO_RMAX, 0, ACC_MAX);
}
// Situazione di Pitch //
if (value[2] < 1510 ){
AccY = map(value[2], VELO_RMIN, 1528, ACC_MIN, 0);
}
else{
AccY = map(value[2], 1530, VELO_RMAX, 0, ACC_MAX);
}
// Situazione di Yaw //
if (value[0] < 1490 ){
AccZ = map(value[0], 1200, 1500, ACC_MIN, 0);
}
else{
AccZ = map(value[0], 1500, 1850, 0, ACC_MAX);
}
}// end if //
else {throttle = MIN_SPEGNIMENTO; // se il 4th canale va a 1000 spenge il Drone //
}

// Trigger del PID //
// in condizione di riposo tutti i canali inviano 1500 come segnale quindi AccX AccY AccZ = 0 //
//Update_PID();
Pitch_Roll();
PID_Pitch= Compute_PID(pitch);
if (value[4]<1520){
PID_Pitch = 0;
PID_Roll=0;}
v0 = throttle - PID_Pitch;
v1 = throttle - PID_Roll;
v2 = throttle + PID_Pitch;
v3 = throttle + PID_Roll;
// limite inferiore //
if (v0 <60) v0=60;
if (v2<60) v2=60;
if (v1 <155) v1=155;
if (v3<155) v3=155;
//limite superiore //
motore0.write(v0);
/*analogWrite(MOTORE1,v1);*/
motore2.write(v2);
/*analogWrite(MOTORE3,v3);*/


Ovviamente il Programma è più grande vi ho postato solo l'indispensabile. Sto programmando sull'IDE di Arduino e gli angoli di Pitch e di Roll li ottengo tramite imu mpu6050 con gli accelerometri. I valori iniziali di Throttle quando lo stick è tutto basso è di 60, e il massimo valore è 120. Vi prego aiutatemi sono bloccato da 2 giorni ormai.

thx in advance