byeongjin kim
/
Hug2Go_ver_2
joystick_bjk
main.cpp@2:284491e0f6bf, 2019-09-05 (annotated)
- Committer:
- Bhoney
- Date:
- Thu Sep 05 10:02:40 2019 +0000
- Revision:
- 2:284491e0f6bf
- Parent:
- 1:fd1e7e2d0780
- Child:
- 3:cfb7cc54a3ba
190905 (2channel bt)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Bhoney | 0:6f0f41537e2f | 1 | #include "mbed.h" |
Bhoney | 1:fd1e7e2d0780 | 2 | #include "VL53L1X.h" |
Bhoney | 1:fd1e7e2d0780 | 3 | #include "CONFIG.h" |
Bhoney | 1:fd1e7e2d0780 | 4 | #define DEBUG |
Bhoney | 2:284491e0f6bf | 5 | //#define DEBUG_DAC |
Bhoney | 2:284491e0f6bf | 6 | #define BT_MODE |
Bhoney | 2:284491e0f6bf | 7 | //#define ToF_MODE |
Bhoney | 1:fd1e7e2d0780 | 8 | #ifdef DEBUG |
Bhoney | 1:fd1e7e2d0780 | 9 | DigitalOut led(LED1); |
Bhoney | 1:fd1e7e2d0780 | 10 | #endif |
Bhoney | 0:6f0f41537e2f | 11 | |
Bhoney | 1:fd1e7e2d0780 | 12 | Serial bt(BLUETOOTH_TX, BLUETOOTH_RX); |
Bhoney | 1:fd1e7e2d0780 | 13 | //Serial pc(USBTX, USBRX); |
Bhoney | 0:6f0f41537e2f | 14 | |
Bhoney | 1:fd1e7e2d0780 | 15 | #ifdef ToF |
Bhoney | 1:fd1e7e2d0780 | 16 | VL53L1X tof(ToF_I2C_SDA, ToF_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8) |
Bhoney | 0:6f0f41537e2f | 17 | |
Bhoney | 1:fd1e7e2d0780 | 18 | DigitalOut Xshut1(ToF_XShut_1); |
Bhoney | 1:fd1e7e2d0780 | 19 | DigitalOut Xshut2(ToF_XShut_2); |
Bhoney | 1:fd1e7e2d0780 | 20 | DigitalOut Xshut3(ToF_XShut_3); |
Bhoney | 1:fd1e7e2d0780 | 21 | #endif |
Bhoney | 0:6f0f41537e2f | 22 | |
Bhoney | 1:fd1e7e2d0780 | 23 | DigitalOut reverseR(MOTOR_REVERSE_R); |
Bhoney | 1:fd1e7e2d0780 | 24 | DigitalOut reverseL(MOTOR_REVERSE_L); |
Bhoney | 0:6f0f41537e2f | 25 | |
Bhoney | 1:fd1e7e2d0780 | 26 | AnalogOut throttleR(MOTOR_THROTTLE_R); |
Bhoney | 1:fd1e7e2d0780 | 27 | AnalogOut throttleL(MOTOR_THROTTLE_L); |
Bhoney | 1:fd1e7e2d0780 | 28 | |
Bhoney | 2:284491e0f6bf | 29 | Timer timer; |
Bhoney | 2:284491e0f6bf | 30 | #ifdef BT_MODE |
Bhoney | 1:fd1e7e2d0780 | 31 | char rxChar = 0; |
Bhoney | 1:fd1e7e2d0780 | 32 | bool rxFlag = false; |
Bhoney | 0:6f0f41537e2f | 33 | |
Bhoney | 1:fd1e7e2d0780 | 34 | void rxData(){ |
Bhoney | 1:fd1e7e2d0780 | 35 | rxChar = bt.getc(); |
Bhoney | 1:fd1e7e2d0780 | 36 | bt.putc(0); |
Bhoney | 1:fd1e7e2d0780 | 37 | rxFlag = true; |
Bhoney | 1:fd1e7e2d0780 | 38 | } |
Bhoney | 2:284491e0f6bf | 39 | #endif |
Bhoney | 0:6f0f41537e2f | 40 | int main() |
Bhoney | 0:6f0f41537e2f | 41 | { |
Bhoney | 2:284491e0f6bf | 42 | #ifdef DEBUG_DAC |
Bhoney | 2:284491e0f6bf | 43 | while(1){ |
Bhoney | 2:284491e0f6bf | 44 | throttleR = 1; |
Bhoney | 2:284491e0f6bf | 45 | throttleL = 1; |
Bhoney | 2:284491e0f6bf | 46 | wait(2); |
Bhoney | 2:284491e0f6bf | 47 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 48 | throttleL = 0; |
Bhoney | 2:284491e0f6bf | 49 | wait(2); |
Bhoney | 2:284491e0f6bf | 50 | |
Bhoney | 2:284491e0f6bf | 51 | } |
Bhoney | 2:284491e0f6bf | 52 | #else |
Bhoney | 2:284491e0f6bf | 53 | #ifdef BT_MODE |
Bhoney | 2:284491e0f6bf | 54 | bt.baud(UART_BAUD); |
Bhoney | 2:284491e0f6bf | 55 | bt.attach(&rxData, Serial::RxIrq); |
Bhoney | 2:284491e0f6bf | 56 | #endif |
Bhoney | 2:284491e0f6bf | 57 | #ifdef ToF_MODE |
Bhoney | 2:284491e0f6bf | 58 | int dis1, dis2, dis3; |
Bhoney | 2:284491e0f6bf | 59 | |
Bhoney | 2:284491e0f6bf | 60 | reverseR = 0; |
Bhoney | 2:284491e0f6bf | 61 | reverseL = 0; |
Bhoney | 2:284491e0f6bf | 62 | |
Bhoney | 2:284491e0f6bf | 63 | bool wheelDirection = true; |
Bhoney | 2:284491e0f6bf | 64 | #endif |
Bhoney | 2:284491e0f6bf | 65 | while(1) { |
Bhoney | 2:284491e0f6bf | 66 | |
Bhoney | 2:284491e0f6bf | 67 | #ifdef ToF_MODE |
Bhoney | 2:284491e0f6bf | 68 | Xshut1 = 1; |
Bhoney | 2:284491e0f6bf | 69 | Xshut2 = 0; |
Bhoney | 2:284491e0f6bf | 70 | Xshut3 = 0; |
Bhoney | 2:284491e0f6bf | 71 | |
Bhoney | 2:284491e0f6bf | 72 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 73 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 74 | wait(ToF_SETUP_TIME); |
Bhoney | 2:284491e0f6bf | 75 | |
Bhoney | 2:284491e0f6bf | 76 | dis1 = tof.getDistance(); |
Bhoney | 2:284491e0f6bf | 77 | // bt.printf("dis1: %d\n", tof.getDistance()); |
Bhoney | 2:284491e0f6bf | 78 | |
Bhoney | 2:284491e0f6bf | 79 | Xshut1 = 0; |
Bhoney | 2:284491e0f6bf | 80 | Xshut2 = 1; |
Bhoney | 2:284491e0f6bf | 81 | Xshut3 = 0; |
Bhoney | 2:284491e0f6bf | 82 | |
Bhoney | 2:284491e0f6bf | 83 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 84 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 85 | wait(ToF_SETUP_TIME); |
Bhoney | 2:284491e0f6bf | 86 | dis2 = tof.getDistance(); |
Bhoney | 2:284491e0f6bf | 87 | bt.printf("dis2: %d\n", tof.getDistance() ); |
Bhoney | 2:284491e0f6bf | 88 | |
Bhoney | 2:284491e0f6bf | 89 | |
Bhoney | 2:284491e0f6bf | 90 | Xshut1 = 0; |
Bhoney | 2:284491e0f6bf | 91 | Xshut2 = 0; |
Bhoney | 2:284491e0f6bf | 92 | Xshut3 = 1; |
Bhoney | 2:284491e0f6bf | 93 | |
Bhoney | 2:284491e0f6bf | 94 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 95 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 96 | wait(ToF_SETUP_TIME); |
Bhoney | 0:6f0f41537e2f | 97 | |
Bhoney | 2:284491e0f6bf | 98 | dis3 = tof.getDistance(); |
Bhoney | 2:284491e0f6bf | 99 | // bt.printf("dis3: %d\n", tof.getDistance() ); |
Bhoney | 2:284491e0f6bf | 100 | |
Bhoney | 2:284491e0f6bf | 101 | if( dis2 > HIGH_BOUND ){ |
Bhoney | 2:284491e0f6bf | 102 | if(!wheelDirection){ |
Bhoney | 2:284491e0f6bf | 103 | throttleL = 0; |
Bhoney | 2:284491e0f6bf | 104 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 105 | reverseR = 0; |
Bhoney | 2:284491e0f6bf | 106 | reverseL = 0; |
Bhoney | 2:284491e0f6bf | 107 | wait(1.2f); |
Bhoney | 2:284491e0f6bf | 108 | wheelDirection = true; |
Bhoney | 2:284491e0f6bf | 109 | |
Bhoney | 2:284491e0f6bf | 110 | } |
Bhoney | 0:6f0f41537e2f | 111 | |
Bhoney | 2:284491e0f6bf | 112 | throttleL = 0.9f; |
Bhoney | 2:284491e0f6bf | 113 | throttleR = 0.9f; |
Bhoney | 2:284491e0f6bf | 114 | } |
Bhoney | 2:284491e0f6bf | 115 | |
Bhoney | 2:284491e0f6bf | 116 | else if ( dis2 < LOW_BOUND ){ |
Bhoney | 2:284491e0f6bf | 117 | if(wheelDirection){ |
Bhoney | 2:284491e0f6bf | 118 | throttleL = 0; |
Bhoney | 2:284491e0f6bf | 119 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 120 | |
Bhoney | 2:284491e0f6bf | 121 | reverseR = 1; |
Bhoney | 2:284491e0f6bf | 122 | reverseL = 1; |
Bhoney | 2:284491e0f6bf | 123 | wait(1.2f); |
Bhoney | 2:284491e0f6bf | 124 | wheelDirection = false; |
Bhoney | 2:284491e0f6bf | 125 | } |
Bhoney | 2:284491e0f6bf | 126 | |
Bhoney | 2:284491e0f6bf | 127 | wait(0.01f); |
Bhoney | 2:284491e0f6bf | 128 | |
Bhoney | 2:284491e0f6bf | 129 | throttleL = 0.9f; |
Bhoney | 2:284491e0f6bf | 130 | throttleR = 0.9f; |
Bhoney | 2:284491e0f6bf | 131 | } |
Bhoney | 2:284491e0f6bf | 132 | else{ |
Bhoney | 2:284491e0f6bf | 133 | throttleL = 0; |
Bhoney | 2:284491e0f6bf | 134 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 135 | } |
Bhoney | 2:284491e0f6bf | 136 | #endif |
Bhoney | 1:fd1e7e2d0780 | 137 | |
Bhoney | 2:284491e0f6bf | 138 | #ifdef BT_MODE |
Bhoney | 1:fd1e7e2d0780 | 139 | if(rxFlag) |
Bhoney | 1:fd1e7e2d0780 | 140 | { |
Bhoney | 1:fd1e7e2d0780 | 141 | #ifdef DEBUG |
Bhoney | 1:fd1e7e2d0780 | 142 | bt.puts("get\r\n"); |
Bhoney | 1:fd1e7e2d0780 | 143 | #endif |
Bhoney | 1:fd1e7e2d0780 | 144 | |
Bhoney | 1:fd1e7e2d0780 | 145 | switch(rxChar){ |
Bhoney | 1:fd1e7e2d0780 | 146 | case '1': |
Bhoney | 1:fd1e7e2d0780 | 147 | throttleR = throttleR + DAC_ONE_STEP; |
Bhoney | 1:fd1e7e2d0780 | 148 | break; |
Bhoney | 1:fd1e7e2d0780 | 149 | |
Bhoney | 1:fd1e7e2d0780 | 150 | case '2': |
Bhoney | 1:fd1e7e2d0780 | 151 | throttleR = throttleR - DAC_ONE_STEP; |
Bhoney | 1:fd1e7e2d0780 | 152 | break; |
Bhoney | 1:fd1e7e2d0780 | 153 | |
Bhoney | 1:fd1e7e2d0780 | 154 | case '3': |
Bhoney | 1:fd1e7e2d0780 | 155 | throttleL = throttleL + DAC_ONE_STEP; |
Bhoney | 1:fd1e7e2d0780 | 156 | break; |
Bhoney | 1:fd1e7e2d0780 | 157 | |
Bhoney | 1:fd1e7e2d0780 | 158 | case '4': |
Bhoney | 1:fd1e7e2d0780 | 159 | throttleL = throttleL - DAC_ONE_STEP; |
Bhoney | 1:fd1e7e2d0780 | 160 | break; |
Bhoney | 1:fd1e7e2d0780 | 161 | |
Bhoney | 1:fd1e7e2d0780 | 162 | case '5': |
Bhoney | 1:fd1e7e2d0780 | 163 | reverseR = !reverseR; |
Bhoney | 1:fd1e7e2d0780 | 164 | break; |
Bhoney | 1:fd1e7e2d0780 | 165 | |
Bhoney | 1:fd1e7e2d0780 | 166 | case '6': |
Bhoney | 1:fd1e7e2d0780 | 167 | reverseL = !reverseL; |
Bhoney | 1:fd1e7e2d0780 | 168 | break; |
Bhoney | 1:fd1e7e2d0780 | 169 | |
Bhoney | 1:fd1e7e2d0780 | 170 | default: |
Bhoney | 1:fd1e7e2d0780 | 171 | reverseR = 0; |
Bhoney | 1:fd1e7e2d0780 | 172 | reverseL = 0; |
Bhoney | 1:fd1e7e2d0780 | 173 | throttleL = 0; |
Bhoney | 2:284491e0f6bf | 174 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 175 | break; |
Bhoney | 1:fd1e7e2d0780 | 176 | |
Bhoney | 1:fd1e7e2d0780 | 177 | } |
Bhoney | 1:fd1e7e2d0780 | 178 | rxFlag = false; |
Bhoney | 1:fd1e7e2d0780 | 179 | } |
Bhoney | 2:284491e0f6bf | 180 | #endif |
Bhoney | 0:6f0f41537e2f | 181 | } |
Bhoney | 2:284491e0f6bf | 182 | #endif |
Bhoney | 0:6f0f41537e2f | 183 | } |