Go Down

Topic: QUADCOPTER + conf [Confirm algo.] (Read 727 times) previous topic - next topic

freak174

Hello dear people,

I will upload parts of my code that involves the handling of PID control which includes transmitters and sensors (angles).

I'll start by writing a brief text in which I explain my variables/algorithms, and to make it simple I will describe only one axis (roll axis):
* Throttle, converted using the "map function" to 0-80 (min value = 1060, max value = 1900).

* Setpoint = Reading the value from the transmitter, minimum value = 1060, maximum value = 1900. These values ??are converted by using the "map" function.
When the lever is in the neutral position (In the middle) - Setpoint = 0 degree
The lever is on the bottom - Setpoint = -35 degrees
The lever is at the top - Setpoint = +35 degrees

* After the PID control, and the new motor-output-values ??I convert them back to the values ??of "1060 - 1900" with the "map" function and use the "constrain function" to not let the values ??go under "1060" or over "1900 ".


Main loop:
Code: [Select]
void setup(){
 Serial.begin(9600);
 PID_sample_init(PID_ROLL);

 sensor_timer = micros();
}

void loop(){
 MOTOR_prepare();
   
   // Just to make sure that the motor doesn't start if throttle level is at bottom.
   if(rc_values[THROTTLE] < 1070) {
   ESC_MOTOR_OUTPUT[LEFT] =1060;
   ESC_MOTOR_OUTPUT[RIGHT] =1060;
 }
 
 MOTOR_write();
}


PID ALGO.:
Initilization
Code: [Select]
void PID_sample_init(float PID_BUFFER[]){
 float  sample_time_sec = sample_time/1000;

 PID_BUFFER[KP] = PID_BUFFER[KP];
 PID_BUFFER[KI] = PID_BUFFER[KI]*sample_time_sec;
 PID_BUFFER[KD] = PID_BUFFER[KD]/sample_time_sec;  
 
}


PID-PREPARE:
Code: [Select]
//Getting readings from receiver and using the map function to make it easier to work with
 rc_read();  //Read the values from receiver
 throttle = map(rc_values[THROTTLE], 1060, 1900, 0, 80);
 PID_ROLL[SETPOINT] = map(rc_values[AILERON], 1060, 1900, 35, -35);  

 // The part where we get our actual position in roll-axis
 ADXL345_read();
 L3G4200D_read();
 Complementary_roll = (0.93*(Complementary_roll+(L3G4200D_DPS_x*(double)(micros()-sensor_timer)/1000000))) + (0.07*ACCEL_roll);  
 PID_ROLL[P_INPUT] = Complementary_roll;
 sensor_timer = micros();

// Making sure that we dont faster than our sample-time. PID_calculate function will be posted in the last quote.
  time_actual = millis();
  time_change = time_actual - time_last;
  if(time_change >= sample_time){
     PID_calculate(PID_ROLL);
     time_last       = time_actual;
  }
 
// Just to make sure we dont go over 80 (1900) and lower than 5 (minimum limit for ESC/motor)
ESC_MOTOR_OUTPUT[LEFT]=constrain((throttle - PID_ROLL[P_OUTPUT]), 5, 80);
ESC_MOTOR_OUTPUT[RIGHT]=constrain((throttle + PID_ROLL[P_OUTPUT]), 5, 80);

  //Now back to original values
  ESC_MOTOR_OUTPUT[LEFT] =map(ESC_MOTOR_OUTPUT[LEFT], 0, 80, 1060, 1900);
  ESC_MOTOR_OUTPUT[RIGHT]=map(ESC_MOTOR_OUTPUT[RIGHT], 0, 80, 1060, 1900);

}



Outputting values to ESC:
Code: [Select]

  ESC_MOTOR_LEFT.writeMicroseconds(ESC_MOTOR_OUTPUT[LEFT]);
  ESC_MOTOR_RIGHT.writeMicroseconds(ESC_MOTOR_OUTPUT[RIGHT]);



PID ALGORITHM:
             
Code: [Select]
void PID_calculate(float PID_BUFFER[]){    // PID_BUFFER[], is in our case PID_ROLL[]
   error_prop = PID_BUFFER[SETPOINT] - PID_BUFFER[P_INPUT];
   error_integral += error_prop;    
   error_derivata = PID_BUFFER[P_INPUT] - PID_BUFFER[LAST_INPUT];
   
   PID_BUFFER[P_OUTPUT] = PID_BUFFER[KP]*error_prop + PID_BUFFER[KI]*error_integral + PID_BUFFER[KD]*error_derivata;    
   PID_BUFFER[LAST_INPUT] = PID_BUFFER[P_INPUT];
 
}


The reason why I'm asking for help is because the tuning of the PID-paramters isn't working that great right now.  
The best result I've had just by tuning the with KP =0.06 and KD = 0.0055, giving me slow reactions.
1. Stays horizontall for like 10 seconds
2. Turns to left for a couple of seconds
3. Back to step 1.

With high KP values the quadcopter just bumps from one side to another side, uncontrolable.

Please observate that Ive removed other initliaztions in the setup loop because they work. So the sensor degree outputs are correct and also the readins from the receiver are correct.

If there is something wrong, I believe that this part has something to do with it:
Code: [Select]
throttle = map(rc_values[THROTTLE], 1060, 1900, 0, 80);
 PID_ROLL[SETPOINT] = map(rc_values[AILERON], 1060, 1900, 35, -35);  



Code: [Select]
and // Just to make sure we dont go over 80 (1900) and lower than 5 (minimum limit for ESC/motor)
ESC_MOTOR_OUTPUT[LEFT]=constrain((throttle - PID_ROLL[P_OUTPUT]), 5, 80);
ESC_MOTOR_OUTPUT[RIGHT]=constrain((throttle + PID_ROLL[P_OUTPUT]), 5, 80);

  //Now back to original values
  ESC_MOTOR_OUTPUT[LEFT] =map(ESC_MOTOR_OUTPUT[LEFT], 0, 80, 1060, 1900);
  ESC_MOTOR_OUTPUT[RIGHT]=map(ESC_MOTOR_OUTPUT[RIGHT], 0, 80, 1060, 1900);


Cheers!

Go Up