![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Quadcopter Attitude Control(Yaw-Pitch-Roll)
PID_Yaw.cpp
- Committer:
- khaledelmadawi
- Date:
- 2015-07-03
- Revision:
- 0:e63996fd7d3e
File content as of revision 0:e63996fd7d3e:
#ifndef _MBED_H #define _MBED_H #include "mbed.h" #endif #include "PID_Yaw.h" #define Error_operation_limits 0.25 #define sigmoid_fun(h) (2/(1+exp(-h)))-1 #define NULLAREA 0.015 //constructor PID_Yaw::PID_Yaw(float KP,float KI,float KD,float KDD ) /*, debugSerial(USBTX, USBRX)*/ { //debugSerial.baud(D_BAUDRATE); Out_PWM=0.001; Out_PWM2=0.001; _KP=KP; _KD=KD; _KI=KI; _KDD=KDD; h=0; ErrorDD=0; ErrorD=0; //debugSerial.printf("\r\n\r\n\r\nhere\r\n"); PowerSetPoint=0.001; } void PID_Yaw::PWM_cal(float Theta_req,float Theta_curr,float req_power_SP,float * xPlanePWM,float *yPlanePWM ) { PowerSetPoint=req_power_SP; //debugSerial.printf("\r\nthetareq:%f thetacurr:%f\r\n",Theta_req,Theta_curr); Check_IN_Limits(&Theta_req); Pulse_Error_cal(Theta_req,Theta_curr); Out_PWM= PowerSetPoint-h; Out_PWM2= PowerSetPoint+h; Check_OUT_Limits(&Out_PWM); Check_OUT_Limits(&Out_PWM2); *xPlanePWM=Out_PWM; *yPlanePWM=Out_PWM2; } //error calculating and out the relative output to the relative error. void PID_Yaw::Pulse_Error_cal(float RPM_req,float RPM_curr) { //debugSerial.printf("req:%f curr:%f",RPM_req,RPM_curr); Error=RPM_req-RPM_curr; ErrorI=ErrorI+Error; ErrorD=Error-ErrorOLD; ErrorOLD=Error; ErrorDD=ErrorD-ErrorDOLD; ErrorDOLD=ErrorD; h=_KP*Error+_KI*ErrorI+_KD*ErrorD+_KDD*ErrorDD; } //setting Output Limits. void PID_Yaw::Set_OUT_Limits(float LowerLimt,float UpperLimit) { O_Upper_Limit=UpperLimit; O_Lower_Limit=LowerLimt; } //setting Input Limits. void PID_Yaw::Set_IN_Limits(float LowerLimit,float UpperLimit) { I_Upper_Limit=UpperLimit; I_Lower_Limit=LowerLimit; } //checking Input Limits in the Realtime operation. void PID_Yaw::Check_OUT_Limits(float *RPMSetPoint) { if(*RPMSetPoint>O_Upper_Limit)*RPMSetPoint=O_Upper_Limit; if(*RPMSetPoint<O_Lower_Limit)*RPMSetPoint=O_Lower_Limit; } //checking Output Limits in Realtime operation. void PID_Yaw::Check_IN_Limits(float *O_PWM) { if(*O_PWM>I_Upper_Limit)*O_PWM=I_Upper_Limit; if(*O_PWM<I_Lower_Limit)*O_PWM=I_Lower_Limit; }