quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

PID_Yaw.cpp@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:e63996fd7d3e 1 #ifndef _MBED_H
khaledelmadawi 0:e63996fd7d3e 2 #define _MBED_H
khaledelmadawi 0:e63996fd7d3e 3 #include "mbed.h"
khaledelmadawi 0:e63996fd7d3e 4 #endif
khaledelmadawi 0:e63996fd7d3e 5 #include "PID_Yaw.h"
khaledelmadawi 0:e63996fd7d3e 6 #define Error_operation_limits 0.25
khaledelmadawi 0:e63996fd7d3e 7 #define sigmoid_fun(h) (2/(1+exp(-h)))-1
khaledelmadawi 0:e63996fd7d3e 8 #define NULLAREA 0.015
khaledelmadawi 0:e63996fd7d3e 9 //constructor
khaledelmadawi 0:e63996fd7d3e 10 PID_Yaw::PID_Yaw(float KP,float KI,float KD,float KDD ) /*, debugSerial(USBTX, USBRX)*/
khaledelmadawi 0:e63996fd7d3e 11 {
khaledelmadawi 0:e63996fd7d3e 12 //debugSerial.baud(D_BAUDRATE);
khaledelmadawi 0:e63996fd7d3e 13 Out_PWM=0.001;
khaledelmadawi 0:e63996fd7d3e 14 Out_PWM2=0.001;
khaledelmadawi 0:e63996fd7d3e 15 _KP=KP;
khaledelmadawi 0:e63996fd7d3e 16 _KD=KD;
khaledelmadawi 0:e63996fd7d3e 17 _KI=KI;
khaledelmadawi 0:e63996fd7d3e 18 _KDD=KDD;
khaledelmadawi 0:e63996fd7d3e 19 h=0;
khaledelmadawi 0:e63996fd7d3e 20 ErrorDD=0;
khaledelmadawi 0:e63996fd7d3e 21 ErrorD=0;
khaledelmadawi 0:e63996fd7d3e 22 //debugSerial.printf("\r\n\r\n\r\nhere\r\n");
khaledelmadawi 0:e63996fd7d3e 23 PowerSetPoint=0.001;
khaledelmadawi 0:e63996fd7d3e 24 }
khaledelmadawi 0:e63996fd7d3e 25 void PID_Yaw::PWM_cal(float Theta_req,float Theta_curr,float req_power_SP,float * xPlanePWM,float *yPlanePWM )
khaledelmadawi 0:e63996fd7d3e 26 {
khaledelmadawi 0:e63996fd7d3e 27 PowerSetPoint=req_power_SP;
khaledelmadawi 0:e63996fd7d3e 28
khaledelmadawi 0:e63996fd7d3e 29 //debugSerial.printf("\r\nthetareq:%f thetacurr:%f\r\n",Theta_req,Theta_curr);
khaledelmadawi 0:e63996fd7d3e 30
khaledelmadawi 0:e63996fd7d3e 31 Check_IN_Limits(&Theta_req);
khaledelmadawi 0:e63996fd7d3e 32 Pulse_Error_cal(Theta_req,Theta_curr);
khaledelmadawi 0:e63996fd7d3e 33
khaledelmadawi 0:e63996fd7d3e 34 Out_PWM= PowerSetPoint-h;
khaledelmadawi 0:e63996fd7d3e 35 Out_PWM2= PowerSetPoint+h;
khaledelmadawi 0:e63996fd7d3e 36 Check_OUT_Limits(&Out_PWM);
khaledelmadawi 0:e63996fd7d3e 37 Check_OUT_Limits(&Out_PWM2);
khaledelmadawi 0:e63996fd7d3e 38 *xPlanePWM=Out_PWM;
khaledelmadawi 0:e63996fd7d3e 39 *yPlanePWM=Out_PWM2;
khaledelmadawi 0:e63996fd7d3e 40 }
khaledelmadawi 0:e63996fd7d3e 41 //error calculating and out the relative output to the relative error.
khaledelmadawi 0:e63996fd7d3e 42 void PID_Yaw::Pulse_Error_cal(float RPM_req,float RPM_curr)
khaledelmadawi 0:e63996fd7d3e 43 {
khaledelmadawi 0:e63996fd7d3e 44 //debugSerial.printf("req:%f curr:%f",RPM_req,RPM_curr);
khaledelmadawi 0:e63996fd7d3e 45 Error=RPM_req-RPM_curr;
khaledelmadawi 0:e63996fd7d3e 46 ErrorI=ErrorI+Error;
khaledelmadawi 0:e63996fd7d3e 47 ErrorD=Error-ErrorOLD;
khaledelmadawi 0:e63996fd7d3e 48 ErrorOLD=Error;
khaledelmadawi 0:e63996fd7d3e 49 ErrorDD=ErrorD-ErrorDOLD;
khaledelmadawi 0:e63996fd7d3e 50 ErrorDOLD=ErrorD;
khaledelmadawi 0:e63996fd7d3e 51 h=_KP*Error+_KI*ErrorI+_KD*ErrorD+_KDD*ErrorDD;
khaledelmadawi 0:e63996fd7d3e 52
khaledelmadawi 0:e63996fd7d3e 53 }
khaledelmadawi 0:e63996fd7d3e 54 //setting Output Limits.
khaledelmadawi 0:e63996fd7d3e 55 void PID_Yaw::Set_OUT_Limits(float LowerLimt,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 56 {
khaledelmadawi 0:e63996fd7d3e 57 O_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 58 O_Lower_Limit=LowerLimt;
khaledelmadawi 0:e63996fd7d3e 59 }
khaledelmadawi 0:e63996fd7d3e 60 //setting Input Limits.
khaledelmadawi 0:e63996fd7d3e 61 void PID_Yaw::Set_IN_Limits(float LowerLimit,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 62 {
khaledelmadawi 0:e63996fd7d3e 63 I_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 64 I_Lower_Limit=LowerLimit;
khaledelmadawi 0:e63996fd7d3e 65 }
khaledelmadawi 0:e63996fd7d3e 66 //checking Input Limits in the Realtime operation.
khaledelmadawi 0:e63996fd7d3e 67 void PID_Yaw::Check_OUT_Limits(float *RPMSetPoint)
khaledelmadawi 0:e63996fd7d3e 68 {
khaledelmadawi 0:e63996fd7d3e 69 if(*RPMSetPoint>O_Upper_Limit)*RPMSetPoint=O_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 70 if(*RPMSetPoint<O_Lower_Limit)*RPMSetPoint=O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 71 }
khaledelmadawi 0:e63996fd7d3e 72 //checking Output Limits in Realtime operation.
khaledelmadawi 0:e63996fd7d3e 73 void PID_Yaw::Check_IN_Limits(float *O_PWM)
khaledelmadawi 0:e63996fd7d3e 74 {
khaledelmadawi 0:e63996fd7d3e 75 if(*O_PWM>I_Upper_Limit)*O_PWM=I_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 76 if(*O_PWM<I_Lower_Limit)*O_PWM=I_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 77 }