Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

GIMBAL_SAMPLE.cpp@0:f0b20f502059, 2018-05-24 (annotated)

Committer:
halusis
Date:
Thu May 24 10:19:44 2018 +0000
Revision:
0:f0b20f502059
Child:
1:dd6e70abeb8e
AA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
halusis 0:f0b20f502059 1 #include "mbed.h"
halusis 0:f0b20f502059 2 #include "Pin_Assign.h"
halusis 0:f0b20f502059 3 #include "GIMBAL_CMD.h"
halusis 0:f0b20f502059 4 #include "GET_ANGLE_FUNC.h"
halusis 0:f0b20f502059 5 #include "Global_Variables.h"
halusis 0:f0b20f502059 6 #include "PWMOut.h"
halusis 0:f0b20f502059 7 #include "RFDevCOMM.h"
halusis 0:f0b20f502059 8 #include "PPMIn.h"
halusis 0:f0b20f502059 9 #include "PPMOut.h"
halusis 0:f0b20f502059 10 #include "RCInputProc.h"
halusis 0:f0b20f502059 11
halusis 0:f0b20f502059 12 Ticker GetAngleTimer;
halusis 0:f0b20f502059 13 Ticker DebugTimer;
halusis 0:f0b20f502059 14 Timeout InitialPWM;
halusis 0:f0b20f502059 15
halusis 0:f0b20f502059 16 bool fGetAngleTimer=0;
halusis 0:f0b20f502059 17 bool fDebugTimer=0;
halusis 0:f0b20f502059 18 uint8_t fInitialPWM=0;
halusis 0:f0b20f502059 19
halusis 0:f0b20f502059 20 void GetAngleTimer_isr(void)
halusis 0:f0b20f502059 21 {
halusis 0:f0b20f502059 22 fGetAngleTimer=1;
halusis 0:f0b20f502059 23 }
halusis 0:f0b20f502059 24 void DebugTimer_isr(void)
halusis 0:f0b20f502059 25 {
halusis 0:f0b20f502059 26 fDebugTimer=1;
halusis 0:f0b20f502059 27 }
halusis 0:f0b20f502059 28 void InitialPWM_isr(void)
halusis 0:f0b20f502059 29 {
halusis 0:f0b20f502059 30 fInitialPWM++;
halusis 0:f0b20f502059 31 //pc.printf("1 %d\n",fInitialPWM);
halusis 0:f0b20f502059 32 }
halusis 0:f0b20f502059 33
halusis 0:f0b20f502059 34
halusis 0:f0b20f502059 35
halusis 0:f0b20f502059 36 int main()
halusis 0:f0b20f502059 37 {
halusis 0:f0b20f502059 38 pc.baud(115200);
halusis 0:f0b20f502059 39 Fcc.baud(115200); //reserved
halusis 0:f0b20f502059 40 RF.baud(115200); //RF Commucation Serial
halusis 0:f0b20f502059 41 Gimbal.baud(115200); //DO NOT CHANGE gimbal baudrate cannot changes.
halusis 0:f0b20f502059 42
halusis 0:f0b20f502059 43 YawCMD.period_ms(100);
halusis 0:f0b20f502059 44 PitchCMD.period_ms(100);
halusis 0:f0b20f502059 45
halusis 0:f0b20f502059 46 GetAngleTimer.attach(&GetAngleTimer_isr,dt); //5Hz
halusis 0:f0b20f502059 47 DebugTimer.attach(&DebugTimer_isr,0.5);
halusis 0:f0b20f502059 48
halusis 0:f0b20f502059 49 YawTarPWM=IdlePWM;
halusis 0:f0b20f502059 50 PitchTarPWM=IdlePWM;
halusis 0:f0b20f502059 51
halusis 0:f0b20f502059 52 pc.printf("start\n");
halusis 0:f0b20f502059 53
halusis 0:f0b20f502059 54 SndRFCMD();
halusis 0:f0b20f502059 55
halusis 0:f0b20f502059 56 while(1)
halusis 0:f0b20f502059 57 {
halusis 0:f0b20f502059 58 if(fpc==0)RCInputProc();
halusis 0:f0b20f502059 59
halusis 0:f0b20f502059 60 if(fGetAngleTimer==1)
halusis 0:f0b20f502059 61 {
halusis 0:f0b20f502059 62 CMD_GET_ANGLE();
halusis 0:f0b20f502059 63 fGetAngleTimer=0;
halusis 0:f0b20f502059 64
halusis 0:f0b20f502059 65 if(fInitialPWM==2)
halusis 0:f0b20f502059 66 {
halusis 0:f0b20f502059 67 //pc.printf("2 %d\n",fInitialPWM);
halusis 0:f0b20f502059 68 PWMOut();
halusis 0:f0b20f502059 69 }
halusis 0:f0b20f502059 70
halusis 0:f0b20f502059 71 }
halusis 0:f0b20f502059 72 if(Gimbal.readable())
halusis 0:f0b20f502059 73 {
halusis 0:f0b20f502059 74 GET_ANGLE_FUNC();
halusis 0:f0b20f502059 75 if(fInitialPWM==0&&YawRel!=0.0f&&PitchRel!=0.0f)
halusis 0:f0b20f502059 76 {
halusis 0:f0b20f502059 77 //pc.printf("0 %d\n",fInitialPWM);
halusis 0:f0b20f502059 78 InitialPWM.attach(&InitialPWM_isr,5);
halusis 0:f0b20f502059 79 fInitialPWM=1;
halusis 0:f0b20f502059 80 }
halusis 0:f0b20f502059 81 }
halusis 0:f0b20f502059 82 if(RF.readable())
halusis 0:f0b20f502059 83 {
halusis 0:f0b20f502059 84 RcvRFCMD();
halusis 0:f0b20f502059 85 }
halusis 0:f0b20f502059 86
halusis 0:f0b20f502059 87 if(pc.readable()) // for intant exp erase before final test
halusis 0:f0b20f502059 88 {
halusis 0:f0b20f502059 89 pcBuf=pc.getc();
halusis 0:f0b20f502059 90 fpc=1;
halusis 0:f0b20f502059 91
halusis 0:f0b20f502059 92 //pc.printf("pc\n");
halusis 0:f0b20f502059 93 if(pcBuf==0x00)
halusis 0:f0b20f502059 94 {
halusis 0:f0b20f502059 95 //pc.printf("pc\n");
halusis 0:f0b20f502059 96 if(fLock!=3){fLock=3; fpc=1; pc.printf("Controllable with Keyboard\n"); YawT=0; PitchT=0;}
halusis 0:f0b20f502059 97 else if(fLock==3){fpc=0;pc.printf("Controllable with RC\n"); }
halusis 0:f0b20f502059 98 }
halusis 0:f0b20f502059 99 else if(pcBuf==0x05)
halusis 0:f0b20f502059 100 {
halusis 0:f0b20f502059 101 fLock=1; pc.printf("Lock on target\n");
halusis 0:f0b20f502059 102 }
halusis 0:f0b20f502059 103 else if(pcBuf==0x01) //
halusis 0:f0b20f502059 104 {
halusis 0:f0b20f502059 105 YawT+=5;
halusis 0:f0b20f502059 106 }
halusis 0:f0b20f502059 107 else if(pcBuf==0x02)
halusis 0:f0b20f502059 108 {
halusis 0:f0b20f502059 109 YawT-=5;
halusis 0:f0b20f502059 110 }
halusis 0:f0b20f502059 111 else if(pcBuf==0x03)
halusis 0:f0b20f502059 112 {
halusis 0:f0b20f502059 113 PitchT+=5;
halusis 0:f0b20f502059 114 }
halusis 0:f0b20f502059 115 else if(pcBuf==0x04)
halusis 0:f0b20f502059 116 {
halusis 0:f0b20f502059 117 PitchT-=5;
halusis 0:f0b20f502059 118 }
halusis 0:f0b20f502059 119 else if(pcBuf==0x09)
halusis 0:f0b20f502059 120 {
halusis 0:f0b20f502059 121 SndRFCMD(); //RF Device offset data
halusis 0:f0b20f502059 122 pc.printf("RF Device offset data\n");
halusis 0:f0b20f502059 123 }
halusis 0:f0b20f502059 124 }
halusis 0:f0b20f502059 125
halusis 0:f0b20f502059 126 if(fDebugTimer==1)
halusis 0:f0b20f502059 127 {
halusis 0:f0b20f502059 128
halusis 0:f0b20f502059 129 //pc.printf("%f %f %f %f %d\n",PitchRel, YawRel,PitchTar,YawTar, YawPWM);
halusis 0:f0b20f502059 130 //pc.printf("%f %f %f %f %d\n",YawRel,YawTar, YawT, ErrorYI,(int)YawPWM);
halusis 0:f0b20f502059 131 //pc.printf("%f %f %f %f\n",YawRel,PitchRel, YawT, PitchT);
halusis 0:f0b20f502059 132 //pc.printf("%f %f %f %f\n",Ele,Azi, Ele_raw, Azi_raw);
halusis 0:f0b20f502059 133 //pc.printf("%x\n",rEle);
halusis 0:f0b20f502059 134 //pc.printf("%d %d %d \n", ppmin.channels[5],ppmin.channels[6],ppmin.channels[7]);
halusis 0:f0b20f502059 135
halusis 0:f0b20f502059 136 //pc.printf("%f %f %d\n", YawDegCMD, PitchDegCMD, fLock);
halusis 0:f0b20f502059 137 //pc.printf("%d %d %d\n", YawPPM, PitchPPM, fLock);
halusis 0:f0b20f502059 138 //pc.printf("%d %d %d\n", (int)YawPWM, (int)PitchPWM, fLock);
halusis 0:f0b20f502059 139
halusis 0:f0b20f502059 140
halusis 0:f0b20f502059 141 pc.printf("%.0f %.0f %.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,YawT,PitchT,YawPWM,PitchPWM, fLock);
halusis 0:f0b20f502059 142 //pc.printf("%.0f %.0f %.0f %d\n",YawRel,YawT,YawPWM, fLock);
halusis 0:f0b20f502059 143 //pc.printf("%.0f %.0f %.0f %d\n",PitchRel,PitchT,PitchPWM, fLock);
halusis 0:f0b20f502059 144 //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",Azi,Ele,V1,V2,V3,V4, fLock);
halusis 0:f0b20f502059 145 //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",YawRel,PitchRel,V1,V2,V3,V4, fLock);
halusis 0:f0b20f502059 146 //pc.printf("%.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,Azi,Ele, fLock);
halusis 0:f0b20f502059 147 //pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %d\n",YawRel,Azi,V1,V2,V3,V4, fLock);
halusis 0:f0b20f502059 148
halusis 0:f0b20f502059 149 RF2FCC();
halusis 0:f0b20f502059 150
halusis 0:f0b20f502059 151 fDebugTimer=0;
halusis 0:f0b20f502059 152 }
halusis 0:f0b20f502059 153 }
halusis 0:f0b20f502059 154 }