a
Dependencies: mbed
GIMBAL_SAMPLE.cpp@1:dd6e70abeb8e, 2018-05-25 (annotated)
- Committer:
- halusis
- Date:
- Fri May 25 08:09:17 2018 +0000
- Revision:
- 1:dd6e70abeb8e
- Parent:
- 0:f0b20f502059
a
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:dd6e70abeb8e | 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 | 1:dd6e70abeb8e | 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 | } |