Diff: GIMBAL_SAMPLE.cpp

Revision:

0:f0b20f502059

Child:

1:dd6e70abeb8e

diff -r 000000000000 -r f0b20f502059 GIMBAL_SAMPLE.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GIMBAL_SAMPLE.cpp	Thu May 24 10:19:44 2018 +0000
@@ -0,0 +1,154 @@
+#include "mbed.h"
+#include "Pin_Assign.h"
+#include "GIMBAL_CMD.h"
+#include "GET_ANGLE_FUNC.h"
+#include "Global_Variables.h"
+#include "PWMOut.h"
+#include "RFDevCOMM.h"
+#include "PPMIn.h"
+#include "PPMOut.h"
+#include "RCInputProc.h"
+
+Ticker GetAngleTimer;
+Ticker DebugTimer;
+Timeout InitialPWM;
+
+bool fGetAngleTimer=0;
+bool fDebugTimer=0;
+uint8_t fInitialPWM=0;
+
+void GetAngleTimer_isr(void)
+{
+    fGetAngleTimer=1;
+}
+void DebugTimer_isr(void)
+{
+    fDebugTimer=1;
+}
+void InitialPWM_isr(void)
+{
+    fInitialPWM++;
+    //pc.printf("1 %d\n",fInitialPWM);
+}
+
+
+
+int main() 
+{
+    pc.baud(115200);
+    Fcc.baud(115200);        //reserved
+    RF.baud(115200);        //RF Commucation Serial
+    Gimbal.baud(115200); //DO NOT CHANGE gimbal baudrate cannot changes.
+    
+    YawCMD.period_ms(100);
+    PitchCMD.period_ms(100);
+    
+    GetAngleTimer.attach(&GetAngleTimer_isr,dt); //5Hz
+    DebugTimer.attach(&DebugTimer_isr,0.5);
+    
+    YawTarPWM=IdlePWM;
+    PitchTarPWM=IdlePWM;
+    
+    pc.printf("start\n");
+    
+    SndRFCMD();
+    
+    while(1)
+    {
+        if(fpc==0)RCInputProc();
+        
+        if(fGetAngleTimer==1)
+        {
+            CMD_GET_ANGLE();
+            fGetAngleTimer=0;
+            
+            if(fInitialPWM==2)
+            {
+                //pc.printf("2 %d\n",fInitialPWM);
+                PWMOut();
+            }
+            
+        }
+        if(Gimbal.readable())
+        {
+            GET_ANGLE_FUNC();
+            if(fInitialPWM==0&&YawRel!=0.0f&&PitchRel!=0.0f)
+            {
+                //pc.printf("0 %d\n",fInitialPWM);
+                InitialPWM.attach(&InitialPWM_isr,5);
+                fInitialPWM=1;
+            }
+        }
+        if(RF.readable())
+        {
+            RcvRFCMD();
+        }
+        
+        if(pc.readable())     //   for intant exp erase before final test
+        {
+            pcBuf=pc.getc();
+            fpc=1;
+            
+            //pc.printf("pc\n");
+            if(pcBuf==0x00)
+            {
+                //pc.printf("pc\n");
+                if(fLock!=3){fLock=3; fpc=1; pc.printf("Controllable with Keyboard\n"); YawT=0; PitchT=0;}
+                else if(fLock==3){fpc=0;pc.printf("Controllable with RC\n"); }
+            }
+            else if(pcBuf==0x05)
+            {
+                fLock=1; pc.printf("Lock on target\n");
+            }
+            else if(pcBuf==0x01) //
+            {
+                YawT+=5;
+            }
+            else if(pcBuf==0x02)
+            {
+                YawT-=5;
+            }
+            else if(pcBuf==0x03)
+            {
+                PitchT+=5;
+            }
+            else if(pcBuf==0x04)
+            {
+                PitchT-=5;
+            }
+            else if(pcBuf==0x09)
+            {
+                SndRFCMD(); //RF Device offset data
+                pc.printf("RF Device offset data\n");
+                }
+        }
+        
+        if(fDebugTimer==1)
+        {
+            
+            //pc.printf("%f %f %f %f %d\n",PitchRel, YawRel,PitchTar,YawTar, YawPWM);
+            //pc.printf("%f %f %f %f %d\n",YawRel,YawTar, YawT, ErrorYI,(int)YawPWM);
+            //pc.printf("%f %f %f %f\n",YawRel,PitchRel, YawT, PitchT);
+            //pc.printf("%f %f %f %f\n",Ele,Azi, Ele_raw, Azi_raw);
+            //pc.printf("%x\n",rEle);
+            //pc.printf("%d %d %d \n", ppmin.channels[5],ppmin.channels[6],ppmin.channels[7]);
+            
+            //pc.printf("%f %f %d\n", YawDegCMD, PitchDegCMD, fLock);
+            //pc.printf("%d %d %d\n", YawPPM, PitchPPM, fLock);
+            //pc.printf("%d %d %d\n", (int)YawPWM, (int)PitchPWM, fLock);
+            
+            
+            pc.printf("%.0f %.0f %.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,YawT,PitchT,YawPWM,PitchPWM, fLock);
+            //pc.printf("%.0f %.0f %.0f %d\n",YawRel,YawT,YawPWM, fLock);
+            //pc.printf("%.0f %.0f %.0f %d\n",PitchRel,PitchT,PitchPWM, fLock);
+            //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",Azi,Ele,V1,V2,V3,V4, fLock);
+            //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",YawRel,PitchRel,V1,V2,V3,V4, fLock);
+            //pc.printf("%.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,Azi,Ele, fLock);
+            //pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %d\n",YawRel,Azi,V1,V2,V3,V4, fLock);
+            
+            RF2FCC();
+
+            fDebugTimer=0;
+        }
+    }
+}