![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: mbed
Fork of GIMBAL_SAMPLE4_PPM by
Diff: PWMOut/PWMOut.cpp
- Revision:
- 0:f0b20f502059
diff -r 000000000000 -r f0b20f502059 PWMOut/PWMOut.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PWMOut/PWMOut.cpp Thu May 24 10:19:44 2018 +0000 @@ -0,0 +1,59 @@ +#include "mbed.h" +#include "Global_Variables.h" +#include "Pin_Assign.h" +#include "PWMOut.h" +#include "PPMIn.h" +#include "PPMOut.h" + +void PWMPID(void) +{ + if(fLock==1&&fRFread==1) + { + fRFread=0; + YawT= YawRel+Azi; //Clockwise(+), CC(-) //RF CW(+),CC(-) + //PitchT= ; //down angle(+), up angle(-) //RF Up(+) + } + else if(fLock==0) + { + YawT = YawDegCMD; + PitchT = PitchDegCMD; + + //if(YawT>60.0f) YawT=60.0f; + //if(YawT<-60.0f) YawT=60.0f; + } + + //---------Yaw PID---------------- + ErrorY = YawT - YawRel; + ErrorYI += ErrorY*dt; + if(ErrorYI<-9.0f||ErrorYI>9.0f)ErrorYI=0; + + YawPWM = ErrorY*KpY + ErrorYI*KiY + ((ErrorY - prevErrorY)*KdY)/dt; + prevErrorY = ErrorY; + + + if(YawPWM<-YawLim) YawPWM = -YawLim; + else if(YawPWM>YawLim) YawPWM = YawLim; + + + //----------Pitch PID-------------- + ErrorP = PitchT - PitchRel; + ErrorPI += ErrorP*dt; + if(ErrorPI<-9.0f||ErrorPI>9.0f)ErrorPI=0; + + PitchPWM = ErrorP*KpP + ErrorPI*KiP + ((ErrorP - prevErrorP)*KdP)/dt; + prevErrorP = ErrorP; + + if(PitchPWM<-PitchLim) PitchPWM = -PitchLim; + else if(PitchPWM>PitchLim) PitchPWM = PitchLim; + +} +void PWMOut(void) +{ + PWMPID(); + + YawCMD.pulsewidth_us((int)YawPWM+IdlePWM); + PitchCMD.pulsewidth_us((int)PitchPWM+IdlePWM); + + //YawCMD.pulsewidth_us((int)YawPPM); + //PitchCMD.pulsewidth_us((int)PitchPPM); +} \ No newline at end of file