Diff: PWMOut/PWMOut.cpp

Revision:

0:f0b20f502059

--- /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