![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: mbed
Fork of GIMBAL_SAMPLE4_PPM by
PWMOut/PWMOut.cpp@0:f0b20f502059, 2018-05-24 (annotated)
- Committer:
- halusis
- Date:
- Thu May 24 10:19:44 2018 +0000
- Revision:
- 0:f0b20f502059
AA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
halusis | 0:f0b20f502059 | 1 | #include "mbed.h" |
halusis | 0:f0b20f502059 | 2 | #include "Global_Variables.h" |
halusis | 0:f0b20f502059 | 3 | #include "Pin_Assign.h" |
halusis | 0:f0b20f502059 | 4 | #include "PWMOut.h" |
halusis | 0:f0b20f502059 | 5 | #include "PPMIn.h" |
halusis | 0:f0b20f502059 | 6 | #include "PPMOut.h" |
halusis | 0:f0b20f502059 | 7 | |
halusis | 0:f0b20f502059 | 8 | void PWMPID(void) |
halusis | 0:f0b20f502059 | 9 | { |
halusis | 0:f0b20f502059 | 10 | if(fLock==1&&fRFread==1) |
halusis | 0:f0b20f502059 | 11 | { |
halusis | 0:f0b20f502059 | 12 | fRFread=0; |
halusis | 0:f0b20f502059 | 13 | YawT= YawRel+Azi; //Clockwise(+), CC(-) //RF CW(+),CC(-) |
halusis | 0:f0b20f502059 | 14 | //PitchT= ; //down angle(+), up angle(-) //RF Up(+) |
halusis | 0:f0b20f502059 | 15 | } |
halusis | 0:f0b20f502059 | 16 | else if(fLock==0) |
halusis | 0:f0b20f502059 | 17 | { |
halusis | 0:f0b20f502059 | 18 | YawT = YawDegCMD; |
halusis | 0:f0b20f502059 | 19 | PitchT = PitchDegCMD; |
halusis | 0:f0b20f502059 | 20 | |
halusis | 0:f0b20f502059 | 21 | //if(YawT>60.0f) YawT=60.0f; |
halusis | 0:f0b20f502059 | 22 | //if(YawT<-60.0f) YawT=60.0f; |
halusis | 0:f0b20f502059 | 23 | } |
halusis | 0:f0b20f502059 | 24 | |
halusis | 0:f0b20f502059 | 25 | //---------Yaw PID---------------- |
halusis | 0:f0b20f502059 | 26 | ErrorY = YawT - YawRel; |
halusis | 0:f0b20f502059 | 27 | ErrorYI += ErrorY*dt; |
halusis | 0:f0b20f502059 | 28 | if(ErrorYI<-9.0f||ErrorYI>9.0f)ErrorYI=0; |
halusis | 0:f0b20f502059 | 29 | |
halusis | 0:f0b20f502059 | 30 | YawPWM = ErrorY*KpY + ErrorYI*KiY + ((ErrorY - prevErrorY)*KdY)/dt; |
halusis | 0:f0b20f502059 | 31 | prevErrorY = ErrorY; |
halusis | 0:f0b20f502059 | 32 | |
halusis | 0:f0b20f502059 | 33 | |
halusis | 0:f0b20f502059 | 34 | if(YawPWM<-YawLim) YawPWM = -YawLim; |
halusis | 0:f0b20f502059 | 35 | else if(YawPWM>YawLim) YawPWM = YawLim; |
halusis | 0:f0b20f502059 | 36 | |
halusis | 0:f0b20f502059 | 37 | |
halusis | 0:f0b20f502059 | 38 | //----------Pitch PID-------------- |
halusis | 0:f0b20f502059 | 39 | ErrorP = PitchT - PitchRel; |
halusis | 0:f0b20f502059 | 40 | ErrorPI += ErrorP*dt; |
halusis | 0:f0b20f502059 | 41 | if(ErrorPI<-9.0f||ErrorPI>9.0f)ErrorPI=0; |
halusis | 0:f0b20f502059 | 42 | |
halusis | 0:f0b20f502059 | 43 | PitchPWM = ErrorP*KpP + ErrorPI*KiP + ((ErrorP - prevErrorP)*KdP)/dt; |
halusis | 0:f0b20f502059 | 44 | prevErrorP = ErrorP; |
halusis | 0:f0b20f502059 | 45 | |
halusis | 0:f0b20f502059 | 46 | if(PitchPWM<-PitchLim) PitchPWM = -PitchLim; |
halusis | 0:f0b20f502059 | 47 | else if(PitchPWM>PitchLim) PitchPWM = PitchLim; |
halusis | 0:f0b20f502059 | 48 | |
halusis | 0:f0b20f502059 | 49 | } |
halusis | 0:f0b20f502059 | 50 | void PWMOut(void) |
halusis | 0:f0b20f502059 | 51 | { |
halusis | 0:f0b20f502059 | 52 | PWMPID(); |
halusis | 0:f0b20f502059 | 53 | |
halusis | 0:f0b20f502059 | 54 | YawCMD.pulsewidth_us((int)YawPWM+IdlePWM); |
halusis | 0:f0b20f502059 | 55 | PitchCMD.pulsewidth_us((int)PitchPWM+IdlePWM); |
halusis | 0:f0b20f502059 | 56 | |
halusis | 0:f0b20f502059 | 57 | //YawCMD.pulsewidth_us((int)YawPPM); |
halusis | 0:f0b20f502059 | 58 | //PitchCMD.pulsewidth_us((int)PitchPPM); |
halusis | 0:f0b20f502059 | 59 | } |