PID QuadCopter Help Pls

Other Hardware Robotics
1

hi everyone..... i have a problem with my Quadcopter. PID problem to be exact....swings and tilts .... uff
I tell you what i did.... The codes below are to Compute the PID_Roll and to PID_Pitch....

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




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

I tried any coefficent but nothing....the Quadcopter most of the time tilts or swings.This is the code to the motors:

  v0 = (throttle - AccX + AccY - AccZ - PID_Pitch );
  v1 = (throttle - AccX - AccY + AccZ - PID_Roll);
  v2 = (throttle + AccX - AccY - AccZ + PID_Pitch );
  v3 = (throttle + AccX + AccY + AccZ + PID_Roll );  
   
  motore0.write(v0);
  motore1.write(v1);
  motore2.write(v2);
  motore3.write(v3);

exluding the Accx,Accy,Accz that are used to turn the QuadCopter......when i pull the throttle stick my drone swings very high ..... and many times it is overturned... The range of PWM (throttle range) is 90-120 and the limt value for PID is (-10, 10)...
Help me pls

thx in advance

2

Ok Guys i found the problem ..... My DC brushless doesn't read the PID value.... :frowning:

void Xwing()
{ 
Update_PID();
    
 if (value[1] < THROTTLE_RMIN){
    AccX =0;
    AccY =0;
    AccZ =0;
   }else {
  
  
  
 /***************************************************************************************************/
/*                                    // controllo volo //                                        */
/***************************************************************************************************/
  if (value[4] > 1200){  // 4th canale che serve per l'accensione e lo spegnimento 1000 = spento 2000 = acceso //
   
   throttle=map(value[1],THROTTLE_RMIN,THROTTLE_RMAX,MIN_LVL_MOTORI,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 //
 
  v0 =throttle - PID_Pitch - AccX + AccY - AccZ;
  v1 =throttle - AccX - AccY + AccZ - PID_Roll;
  v2 =throttle + PID_Pitch + AccX - AccY - AccZ;
  v3 =throttle + AccX + AccY + AccZ + PID_Roll;  
  motore0.writeMicroseconds(v0);
  motore1.writeMicroseconds(v1);
  motore2.writeMicroseconds(v2);
  motore3.writeMicroseconds(v3); 
 
 
}

Maybe is a synchronization problem, the Channels from RC work at 50 Hz (20 ms) while the PID value i don't know wich sample rate works. I have to simply add the PID value at the throttle . The strange thing is that if print the V0,V1,V2,V3 values on the Serial Monitor i get the value sought, so the operation is right, but the function writeMicroseconds only reads the Throttle value and doesn't add the PID value.. :frowning:

i'm stuck.....Help pls