![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: mbed
Fork of GIMBAL_SAMPLE4_PPM by
PWMOut/PWMOut.cpp
- Committer:
- halusis
- Date:
- 2018-05-25
- Revision:
- 1:dd6e70abeb8e
- Parent:
- 0:f0b20f502059
File content as of revision 1:dd6e70abeb8e:
#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); }