byeongjin kim
/
Hug2Go_ver_2
joystick_bjk
main.cpp@5:fb5a46e37787, 2019-09-09 (annotated)
- Committer:
- Haeun
- Date:
- Mon Sep 09 07:53:29 2019 +0000
- Revision:
- 5:fb5a46e37787
- Parent:
- 4:97a11deb0ab7
- Child:
- 6:beddcb25ad4e
bt1 only
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 | 3:cfb7cc54a3ba | 4 | |
Bhoney | 1:fd1e7e2d0780 | 5 | #define DEBUG |
Bhoney | 2:284491e0f6bf | 6 | #define BT_MODE |
Bhoney | 3:cfb7cc54a3ba | 7 | |
Haeun | 5:fb5a46e37787 | 8 | //#define ToF_MODE |
Bhoney | 3:cfb7cc54a3ba | 9 | |
Bhoney | 3:cfb7cc54a3ba | 10 | |
Bhoney | 1:fd1e7e2d0780 | 11 | #ifdef DEBUG |
Haeun | 4:97a11deb0ab7 | 12 | //DigitalOut led(LED1); |
Bhoney | 1:fd1e7e2d0780 | 13 | #endif |
Bhoney | 0:6f0f41537e2f | 14 | |
Bhoney | 3:cfb7cc54a3ba | 15 | Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control) |
Haeun | 5:fb5a46e37787 | 16 | |
Haeun | 5:fb5a46e37787 | 17 | #ifdef ToF |
Bhoney | 3:cfb7cc54a3ba | 18 | Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result) |
Bhoney | 0:6f0f41537e2f | 19 | |
Bhoney | 1:fd1e7e2d0780 | 20 | VL53L1X tof(ToF_I2C_SDA, ToF_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8) |
Bhoney | 0:6f0f41537e2f | 21 | |
Bhoney | 1:fd1e7e2d0780 | 22 | DigitalOut Xshut1(ToF_XShut_1); |
Bhoney | 1:fd1e7e2d0780 | 23 | DigitalOut Xshut2(ToF_XShut_2); |
Bhoney | 1:fd1e7e2d0780 | 24 | DigitalOut Xshut3(ToF_XShut_3); |
Bhoney | 1:fd1e7e2d0780 | 25 | #endif |
Bhoney | 0:6f0f41537e2f | 26 | |
Bhoney | 1:fd1e7e2d0780 | 27 | DigitalOut reverseR(MOTOR_REVERSE_R); |
Bhoney | 1:fd1e7e2d0780 | 28 | DigitalOut reverseL(MOTOR_REVERSE_L); |
Bhoney | 0:6f0f41537e2f | 29 | |
Bhoney | 1:fd1e7e2d0780 | 30 | AnalogOut throttleR(MOTOR_THROTTLE_R); |
Bhoney | 1:fd1e7e2d0780 | 31 | AnalogOut throttleL(MOTOR_THROTTLE_L); |
Haeun | 4:97a11deb0ab7 | 32 | |
Bhoney | 2:284491e0f6bf | 33 | Timer timer; |
Bhoney | 2:284491e0f6bf | 34 | #ifdef BT_MODE |
Bhoney | 1:fd1e7e2d0780 | 35 | char rxChar = 0; |
Bhoney | 1:fd1e7e2d0780 | 36 | bool rxFlag = false; |
Bhoney | 0:6f0f41537e2f | 37 | |
Bhoney | 1:fd1e7e2d0780 | 38 | void rxData(){ |
Bhoney | 3:cfb7cc54a3ba | 39 | rxChar = bt1.getc(); |
Bhoney | 3:cfb7cc54a3ba | 40 | bt1.putc(0); //convention |
Bhoney | 1:fd1e7e2d0780 | 41 | rxFlag = true; |
Bhoney | 1:fd1e7e2d0780 | 42 | } |
Haeun | 4:97a11deb0ab7 | 43 | //void rxData2(){ |
Haeun | 4:97a11deb0ab7 | 44 | // rxChar = bt2.getc(); |
Haeun | 4:97a11deb0ab7 | 45 | // bt2.putc(0); //convention |
Haeun | 4:97a11deb0ab7 | 46 | // rxFlag = true; |
Haeun | 4:97a11deb0ab7 | 47 | //} |
Bhoney | 2:284491e0f6bf | 48 | #endif |
Bhoney | 0:6f0f41537e2f | 49 | int main() |
Bhoney | 0:6f0f41537e2f | 50 | { |
Haeun | 4:97a11deb0ab7 | 51 | throttleL = 0; |
Haeun | 4:97a11deb0ab7 | 52 | throttleR = 0; |
Bhoney | 2:284491e0f6bf | 53 | #ifdef BT_MODE |
Bhoney | 3:cfb7cc54a3ba | 54 | bt1.baud(UART_BAUD); |
Haeun | 5:fb5a46e37787 | 55 | |
Haeun | 5:fb5a46e37787 | 56 | bt1.attach(&rxData, Serial::RxIrq); |
Haeun | 5:fb5a46e37787 | 57 | |
Haeun | 5:fb5a46e37787 | 58 | #endif |
Haeun | 5:fb5a46e37787 | 59 | #ifdef ToF_MODE |
Haeun | 4:97a11deb0ab7 | 60 | bt2.baud(UART_BAUD); |
Haeun | 4:97a11deb0ab7 | 61 | //bt2.attach(&rxData2, Serial::RxIrq); |
Bhoney | 2:284491e0f6bf | 62 | #endif |
Bhoney | 2:284491e0f6bf | 63 | int dis1, dis2, dis3; |
Bhoney | 2:284491e0f6bf | 64 | |
Bhoney | 2:284491e0f6bf | 65 | reverseR = 0; |
Bhoney | 2:284491e0f6bf | 66 | reverseL = 0; |
Bhoney | 3:cfb7cc54a3ba | 67 | |
Bhoney | 2:284491e0f6bf | 68 | bool wheelDirection = true; |
Haeun | 4:97a11deb0ab7 | 69 | |
Bhoney | 2:284491e0f6bf | 70 | while(1) { |
Bhoney | 2:284491e0f6bf | 71 | |
Bhoney | 3:cfb7cc54a3ba | 72 | #ifdef ToF_MODE |
Bhoney | 3:cfb7cc54a3ba | 73 | |
Bhoney | 3:cfb7cc54a3ba | 74 | ///// first ToF |
Bhoney | 2:284491e0f6bf | 75 | Xshut1 = 1; |
Bhoney | 2:284491e0f6bf | 76 | Xshut2 = 0; |
Bhoney | 2:284491e0f6bf | 77 | Xshut3 = 0; |
Bhoney | 2:284491e0f6bf | 78 | |
Bhoney | 2:284491e0f6bf | 79 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 80 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 81 | wait(ToF_SETUP_TIME); |
Bhoney | 2:284491e0f6bf | 82 | |
Bhoney | 2:284491e0f6bf | 83 | dis1 = tof.getDistance(); |
Bhoney | 3:cfb7cc54a3ba | 84 | |
Bhoney | 2:284491e0f6bf | 85 | |
Bhoney | 3:cfb7cc54a3ba | 86 | ///// second ToF |
Bhoney | 2:284491e0f6bf | 87 | Xshut1 = 0; |
Bhoney | 2:284491e0f6bf | 88 | Xshut2 = 1; |
Bhoney | 2:284491e0f6bf | 89 | Xshut3 = 0; |
Bhoney | 2:284491e0f6bf | 90 | |
Bhoney | 2:284491e0f6bf | 91 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 92 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 93 | wait(ToF_SETUP_TIME); |
Bhoney | 2:284491e0f6bf | 94 | dis2 = tof.getDistance(); |
Bhoney | 3:cfb7cc54a3ba | 95 | |
Bhoney | 2:284491e0f6bf | 96 | |
Bhoney | 3:cfb7cc54a3ba | 97 | ///// third ToF |
Bhoney | 2:284491e0f6bf | 98 | Xshut1 = 0; |
Bhoney | 2:284491e0f6bf | 99 | Xshut2 = 0; |
Bhoney | 2:284491e0f6bf | 100 | Xshut3 = 1; |
Bhoney | 2:284491e0f6bf | 101 | |
Bhoney | 2:284491e0f6bf | 102 | tof.begin(); |
Bhoney | 2:284491e0f6bf | 103 | tof.setDistanceMode(); |
Bhoney | 2:284491e0f6bf | 104 | wait(ToF_SETUP_TIME); |
Bhoney | 0:6f0f41537e2f | 105 | |
Bhoney | 3:cfb7cc54a3ba | 106 | dis3 = tof.getDistance(); //return INTEGER |
Bhoney | 3:cfb7cc54a3ba | 107 | //end of ToF com. |
Bhoney | 2:284491e0f6bf | 108 | |
Bhoney | 2:284491e0f6bf | 109 | |
Haeun | 4:97a11deb0ab7 | 110 | |
Haeun | 4:97a11deb0ab7 | 111 | //bt1.putc(0); |
Haeun | 4:97a11deb0ab7 | 112 | |
Bhoney | 2:284491e0f6bf | 113 | #endif |
Haeun | 4:97a11deb0ab7 | 114 | //bt2.printf("%d|%d|%d\n", dis1, dis2, dis3); |
Haeun | 4:97a11deb0ab7 | 115 | #ifdef BT_MODE |
Bhoney | 1:fd1e7e2d0780 | 116 | if(rxFlag) |
Bhoney | 1:fd1e7e2d0780 | 117 | { |
Bhoney | 1:fd1e7e2d0780 | 118 | #ifdef DEBUG |
Bhoney | 3:cfb7cc54a3ba | 119 | // bt1.puts("get\r\n"); |
Bhoney | 1:fd1e7e2d0780 | 120 | #endif |
Bhoney | 1:fd1e7e2d0780 | 121 | |
Haeun | 4:97a11deb0ab7 | 122 | // if(!(rxChar=='1'||rxChar=='2'||rxChar=='3'||rxChar=='4'||rxChar=='5'||rxChar=='6')) |
Haeun | 4:97a11deb0ab7 | 123 | // rxChar=rxChar_prev; |
Haeun | 4:97a11deb0ab7 | 124 | |
Bhoney | 1:fd1e7e2d0780 | 125 | switch(rxChar){ |
Bhoney | 3:cfb7cc54a3ba | 126 | case '1': // 전진 |
Haeun | 4:97a11deb0ab7 | 127 | if(reverseR==1&&reverseL==1){ |
Haeun | 4:97a11deb0ab7 | 128 | throttleR=0; |
Haeun | 4:97a11deb0ab7 | 129 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 130 | wait(0.1); |
Haeun | 4:97a11deb0ab7 | 131 | } |
Bhoney | 3:cfb7cc54a3ba | 132 | reverseR = 0; |
Bhoney | 3:cfb7cc54a3ba | 133 | reverseL = 0; |
Bhoney | 3:cfb7cc54a3ba | 134 | |
Bhoney | 3:cfb7cc54a3ba | 135 | //need to be delayed |
Bhoney | 3:cfb7cc54a3ba | 136 | |
Bhoney | 1:fd1e7e2d0780 | 137 | throttleR = throttleR + DAC_ONE_STEP; |
Bhoney | 3:cfb7cc54a3ba | 138 | throttleL = throttleL + DAC_ONE_STEP; |
Haeun | 4:97a11deb0ab7 | 139 | // throttleR = 1.0f; |
Haeun | 4:97a11deb0ab7 | 140 | // throttleL = 1.0f; |
Bhoney | 1:fd1e7e2d0780 | 141 | break; |
Bhoney | 1:fd1e7e2d0780 | 142 | |
Bhoney | 3:cfb7cc54a3ba | 143 | case '2': // 후진 |
Haeun | 4:97a11deb0ab7 | 144 | if(reverseR==0&&reverseL==0){ |
Haeun | 4:97a11deb0ab7 | 145 | throttleR=0; |
Haeun | 4:97a11deb0ab7 | 146 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 147 | wait(0.1); |
Haeun | 4:97a11deb0ab7 | 148 | } |
Haeun | 4:97a11deb0ab7 | 149 | |
Bhoney | 3:cfb7cc54a3ba | 150 | reverseR = 1; |
Bhoney | 3:cfb7cc54a3ba | 151 | reverseL = 1; |
Bhoney | 3:cfb7cc54a3ba | 152 | |
Bhoney | 3:cfb7cc54a3ba | 153 | //need to be delayed |
Bhoney | 3:cfb7cc54a3ba | 154 | |
Haeun | 4:97a11deb0ab7 | 155 | throttleR = throttleR + DAC_ONE_STEP; |
Bhoney | 3:cfb7cc54a3ba | 156 | throttleL = throttleL + DAC_ONE_STEP; |
Bhoney | 3:cfb7cc54a3ba | 157 | break; |
Bhoney | 3:cfb7cc54a3ba | 158 | |
Bhoney | 3:cfb7cc54a3ba | 159 | case '3': // 좌회전 |
Haeun | 4:97a11deb0ab7 | 160 | if(reverseR==1&&reverseL==1){ |
Haeun | 4:97a11deb0ab7 | 161 | throttleR=0; |
Haeun | 4:97a11deb0ab7 | 162 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 163 | wait(0.1); |
Haeun | 4:97a11deb0ab7 | 164 | reverseR = 0; |
Haeun | 4:97a11deb0ab7 | 165 | reverseL = 0; |
Haeun | 4:97a11deb0ab7 | 166 | } |
Haeun | 4:97a11deb0ab7 | 167 | |
Haeun | 4:97a11deb0ab7 | 168 | throttleR=0; |
Bhoney | 3:cfb7cc54a3ba | 169 | throttleL = throttleL + DAC_ONE_STEP; |
Bhoney | 3:cfb7cc54a3ba | 170 | break; |
Bhoney | 3:cfb7cc54a3ba | 171 | |
Haeun | 4:97a11deb0ab7 | 172 | case '4': // 우회전 |
Haeun | 4:97a11deb0ab7 | 173 | if(reverseR==1&&reverseL==1){ |
Haeun | 4:97a11deb0ab7 | 174 | throttleR=0; |
Haeun | 4:97a11deb0ab7 | 175 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 176 | wait(0.1); |
Haeun | 4:97a11deb0ab7 | 177 | reverseR = 0; |
Haeun | 4:97a11deb0ab7 | 178 | reverseL = 0; |
Haeun | 4:97a11deb0ab7 | 179 | } |
Haeun | 4:97a11deb0ab7 | 180 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 181 | throttleR = throttleR + DAC_ONE_STEP; |
Bhoney | 1:fd1e7e2d0780 | 182 | break; |
Bhoney | 1:fd1e7e2d0780 | 183 | |
Bhoney | 3:cfb7cc54a3ba | 184 | case '5': // 정지 |
Haeun | 4:97a11deb0ab7 | 185 | throttleR=0; |
Haeun | 4:97a11deb0ab7 | 186 | throttleL=0; |
Haeun | 4:97a11deb0ab7 | 187 | break; |
Bhoney | 1:fd1e7e2d0780 | 188 | |
Bhoney | 1:fd1e7e2d0780 | 189 | case '6': |
Bhoney | 3:cfb7cc54a3ba | 190 | // reverseL = !reverseL; |
Haeun | 4:97a11deb0ab7 | 191 | break; |
Bhoney | 1:fd1e7e2d0780 | 192 | |
Bhoney | 1:fd1e7e2d0780 | 193 | default: |
Haeun | 4:97a11deb0ab7 | 194 | // reverseR = 0; |
Haeun | 4:97a11deb0ab7 | 195 | // reverseL = 0; |
Haeun | 4:97a11deb0ab7 | 196 | // throttleL = 0; |
Haeun | 4:97a11deb0ab7 | 197 | // throttleR = 0; |
Haeun | 4:97a11deb0ab7 | 198 | |
Bhoney | 2:284491e0f6bf | 199 | break; |
Bhoney | 1:fd1e7e2d0780 | 200 | |
Bhoney | 1:fd1e7e2d0780 | 201 | } |
Bhoney | 1:fd1e7e2d0780 | 202 | rxFlag = false; |
Bhoney | 1:fd1e7e2d0780 | 203 | } |
Bhoney | 2:284491e0f6bf | 204 | #endif |
Bhoney | 0:6f0f41537e2f | 205 | } |
Haeun | 4:97a11deb0ab7 | 206 | //#endif |
Bhoney | 0:6f0f41537e2f | 207 | } |