quadcopter cufe
/
2006_Theta_Control_yaw
Quadcopter Attitude Control(Yaw-Pitch-Roll)
Diff: PIDTheta.cpp
- Revision:
- 0:e63996fd7d3e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDTheta.cpp Fri Jul 03 11:16:02 2015 +0000 @@ -0,0 +1,92 @@ +#ifndef _MBED_H +#define _MBED_H +#include "mbed.h" +#endif +#include "PIDTheta.h" +#define Error_operation_limits 0.25 +#define sigmoid_fun(h) (2/(1+exp(-h)))-1 +#define NULLAREA 0.015 +//constructor +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)*/ +{ + //debugSerial.baud(D_BAUDRATE); + Motor_init(Listen_time_ms); + _pwm = 0; + _pwm2 = 0; + 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"); + Motor_Control_signal(); + PowerSetPoint=0.001; +} +void PIDTheta::Motor_init(float Listen_time_ms) +{ + _pwm.period_ms(Listen_time_ms); + _pwm2.period_ms(Listen_time_ms); +} +void PIDTheta::Motor_Control_signal() +{ + //debugSerial.printf("\r\n\r\n\r\n1:%f 2:%f\r\n",Out_PWM,Out_PWM2); + + _pwm.pulsewidth(Out_PWM); + _pwm2.pulsewidth(Out_PWM2); +} +void PIDTheta::PWM_cal(float Theta_req,float Theta_curr,float req_power_SP) +{ + 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); + Motor_Control_signal(); +} +//error calculating and out the relative output to the relative error. +void PIDTheta::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 PIDTheta::Set_OUT_Limits(float LowerLimt,float UpperLimit) +{ + O_Upper_Limit=UpperLimit; + O_Lower_Limit=LowerLimt; +} +//setting Input Limits. +void PIDTheta::Set_IN_Limits(float LowerLimit,float UpperLimit) +{ + I_Upper_Limit=UpperLimit; + I_Lower_Limit=LowerLimit; +} +//checking Input Limits in the Realtime operation. +void PIDTheta::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 PIDTheta::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; +}