a
Dependencies: mbed
GIMBAL_SAMPLE.cpp
- Committer:
- halusis
- Date:
- 2018-05-25
- Revision:
- 1:dd6e70abeb8e
- Parent:
- 0:f0b20f502059
File content as of revision 1:dd6e70abeb8e:
#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; } } }