Pages: [1]   Go Down
Author Topic: QUADCOPTER + conf [Confirm algo.]  (Read 676 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 0
Posts: 101
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
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:
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:
//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:
  ESC_MOTOR_LEFT.writeMicroseconds(ESC_MOTOR_OUTPUT[LEFT]);
   ESC_MOTOR_RIGHT.writeMicroseconds(ESC_MOTOR_OUTPUT[RIGHT]);


PID ALGORITHM:
              
Code:
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:
throttle = map(rc_values[THROTTLE], 1060, 1900, 0, 80);
  PID_ROLL[SETPOINT] = map(rc_values[AILERON], 1060, 1900, 35, -35);  


Code:
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!
Logged

Pages: [1]   Go Up
Jump to: