Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }