quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

PIDTheta.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 "PIDTheta.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 PIDTheta::PIDTheta( PinName pwm,PinName pwm2,float Listen_time_ms,float KP,float KI,float KD,float KDD ): _pwm(pwm),_pwm2(pwm2)/*, debugSerial(USBTX, USBRX)*/
khaledelmadawi 0:e63996fd7d3e 11 {
khaledelmadawi 0:e63996fd7d3e 12 //debugSerial.baud(D_BAUDRATE);
khaledelmadawi 0:e63996fd7d3e 13 Motor_init(Listen_time_ms);
khaledelmadawi 0:e63996fd7d3e 14 _pwm = 0;
khaledelmadawi 0:e63996fd7d3e 15 _pwm2 = 0;
khaledelmadawi 0:e63996fd7d3e 16 Out_PWM=0.001;
khaledelmadawi 0:e63996fd7d3e 17 Out_PWM2=0.001;
khaledelmadawi 0:e63996fd7d3e 18 _KP=KP;
khaledelmadawi 0:e63996fd7d3e 19 _KD=KD;
khaledelmadawi 0:e63996fd7d3e 20 _KI=KI;
khaledelmadawi 0:e63996fd7d3e 21 _KDD=KDD;
khaledelmadawi 0:e63996fd7d3e 22 h=0;
khaledelmadawi 0:e63996fd7d3e 23 ErrorDD=0;
khaledelmadawi 0:e63996fd7d3e 24 ErrorD=0;
khaledelmadawi 0:e63996fd7d3e 25 //debugSerial.printf("\r\n\r\n\r\nhere\r\n");
khaledelmadawi 0:e63996fd7d3e 26 Motor_Control_signal();
khaledelmadawi 0:e63996fd7d3e 27 PowerSetPoint=0.001;
khaledelmadawi 0:e63996fd7d3e 28 }
khaledelmadawi 0:e63996fd7d3e 29 void PIDTheta::Motor_init(float Listen_time_ms)
khaledelmadawi 0:e63996fd7d3e 30 {
khaledelmadawi 0:e63996fd7d3e 31 _pwm.period_ms(Listen_time_ms);
khaledelmadawi 0:e63996fd7d3e 32 _pwm2.period_ms(Listen_time_ms);
khaledelmadawi 0:e63996fd7d3e 33 }
khaledelmadawi 0:e63996fd7d3e 34 void PIDTheta::Motor_Control_signal()
khaledelmadawi 0:e63996fd7d3e 35 {
khaledelmadawi 0:e63996fd7d3e 36 //debugSerial.printf("\r\n\r\n\r\n1:%f 2:%f\r\n",Out_PWM,Out_PWM2);
khaledelmadawi 0:e63996fd7d3e 37
khaledelmadawi 0:e63996fd7d3e 38 _pwm.pulsewidth(Out_PWM);
khaledelmadawi 0:e63996fd7d3e 39 _pwm2.pulsewidth(Out_PWM2);
khaledelmadawi 0:e63996fd7d3e 40 }
khaledelmadawi 0:e63996fd7d3e 41 void PIDTheta::PWM_cal(float Theta_req,float Theta_curr,float req_power_SP)
khaledelmadawi 0:e63996fd7d3e 42 {
khaledelmadawi 0:e63996fd7d3e 43 PowerSetPoint=req_power_SP;
khaledelmadawi 0:e63996fd7d3e 44
khaledelmadawi 0:e63996fd7d3e 45 //debugSerial.printf("\r\nthetareq:%f thetacurr:%f\r\n",Theta_req,Theta_curr);
khaledelmadawi 0:e63996fd7d3e 46
khaledelmadawi 0:e63996fd7d3e 47 Check_IN_Limits(&Theta_req);
khaledelmadawi 0:e63996fd7d3e 48 Pulse_Error_cal(Theta_req,Theta_curr);
khaledelmadawi 0:e63996fd7d3e 49
khaledelmadawi 0:e63996fd7d3e 50 Out_PWM= PowerSetPoint-h;
khaledelmadawi 0:e63996fd7d3e 51 Out_PWM2= PowerSetPoint+h;
khaledelmadawi 0:e63996fd7d3e 52 Check_OUT_Limits(&Out_PWM);
khaledelmadawi 0:e63996fd7d3e 53 Check_OUT_Limits(&Out_PWM2);
khaledelmadawi 0:e63996fd7d3e 54 Motor_Control_signal();
khaledelmadawi 0:e63996fd7d3e 55 }
khaledelmadawi 0:e63996fd7d3e 56 //error calculating and out the relative output to the relative error.
khaledelmadawi 0:e63996fd7d3e 57 void PIDTheta::Pulse_Error_cal(float RPM_req,float RPM_curr)
khaledelmadawi 0:e63996fd7d3e 58 {
khaledelmadawi 0:e63996fd7d3e 59 //debugSerial.printf("req:%f curr:%f",RPM_req,RPM_curr);
khaledelmadawi 0:e63996fd7d3e 60 Error=RPM_req-RPM_curr;
khaledelmadawi 0:e63996fd7d3e 61 ErrorI=ErrorI+Error;
khaledelmadawi 0:e63996fd7d3e 62 ErrorD=Error-ErrorOLD;
khaledelmadawi 0:e63996fd7d3e 63 ErrorOLD=Error;
khaledelmadawi 0:e63996fd7d3e 64 ErrorDD=ErrorD-ErrorDOLD;
khaledelmadawi 0:e63996fd7d3e 65 ErrorDOLD=ErrorD;
khaledelmadawi 0:e63996fd7d3e 66 h=_KP*Error+_KI*ErrorI+_KD*ErrorD+_KDD*ErrorDD;
khaledelmadawi 0:e63996fd7d3e 67
khaledelmadawi 0:e63996fd7d3e 68 }
khaledelmadawi 0:e63996fd7d3e 69 //setting Output Limits.
khaledelmadawi 0:e63996fd7d3e 70 void PIDTheta::Set_OUT_Limits(float LowerLimt,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 71 {
khaledelmadawi 0:e63996fd7d3e 72 O_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 73 O_Lower_Limit=LowerLimt;
khaledelmadawi 0:e63996fd7d3e 74 }
khaledelmadawi 0:e63996fd7d3e 75 //setting Input Limits.
khaledelmadawi 0:e63996fd7d3e 76 void PIDTheta::Set_IN_Limits(float LowerLimit,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 77 {
khaledelmadawi 0:e63996fd7d3e 78 I_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 79 I_Lower_Limit=LowerLimit;
khaledelmadawi 0:e63996fd7d3e 80 }
khaledelmadawi 0:e63996fd7d3e 81 //checking Input Limits in the Realtime operation.
khaledelmadawi 0:e63996fd7d3e 82 void PIDTheta::Check_OUT_Limits(float *RPMSetPoint)
khaledelmadawi 0:e63996fd7d3e 83 {
khaledelmadawi 0:e63996fd7d3e 84 if(*RPMSetPoint>O_Upper_Limit)*RPMSetPoint=O_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 85 if(*RPMSetPoint<O_Lower_Limit)*RPMSetPoint=O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 86 }
khaledelmadawi 0:e63996fd7d3e 87 //checking Output Limits in Realtime operation.
khaledelmadawi 0:e63996fd7d3e 88 void PIDTheta::Check_IN_Limits(float *O_PWM)
khaledelmadawi 0:e63996fd7d3e 89 {
khaledelmadawi 0:e63996fd7d3e 90 if(*O_PWM>I_Upper_Limit)*O_PWM=I_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 91 if(*O_PWM<I_Lower_Limit)*O_PWM=I_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 92 }