byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@6:beddcb25ad4e, 2019-09-16 (annotated)

Committer:
Haeun
Date:
Mon Sep 16 12:06:13 2019 +0000
Revision:
6:beddcb25ad4e
Parent:
5:fb5a46e37787
Child:
7:1f4ef899e43e
Child:
8:27d6afa7cb7c
v1

Who changed what in which revision?

UserRevisionLine numberNew 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 6:beddcb25ad4e 8 //#define BUTTON_MODE
Haeun 6:beddcb25ad4e 9 #define JOYSTICK_MODE
Bhoney 3:cfb7cc54a3ba 10
Bhoney 3:cfb7cc54a3ba 11
Haeun 6:beddcb25ad4e 12 #define ToF_MODE
Haeun 6:beddcb25ad4e 13 #define ToF
Haeun 6:beddcb25ad4e 14
Bhoney 1:fd1e7e2d0780 15 #ifdef DEBUG
Haeun 4:97a11deb0ab7 16 //DigitalOut led(LED1);
Bhoney 1:fd1e7e2d0780 17 #endif
Bhoney 0:6f0f41537e2f 18
Haeun 6:beddcb25ad4e 19 //reserved pin
Haeun 6:beddcb25ad4e 20 //AnalogIn t1(SPI2_SCK);
Haeun 6:beddcb25ad4e 21 //AnalogIn t2(SPI2_NSS);
Haeun 6:beddcb25ad4e 22 //AnalogIn t3(SPI2_MISO);
Haeun 6:beddcb25ad4e 23 //AnalogIn t4(SPI2_MOSI);
Haeun 6:beddcb25ad4e 24 //AnalogIn t5(SPI3_SCK);
Haeun 6:beddcb25ad4e 25 //AnalogIn t6(SPI3_NSS);
Haeun 6:beddcb25ad4e 26 //AnalogIn t7(SPI3_MISO);
Haeun 6:beddcb25ad4e 27 //AnalogIn t8(SPI3_MOSI);
Haeun 6:beddcb25ad4e 28 //AnalogIn t9(USART3_TX);
Haeun 6:beddcb25ad4e 29 //AnalogIn t10(USART3_RX);
Haeun 6:beddcb25ad4e 30 //AnalogIn t11(ADC2_t);
Haeun 6:beddcb25ad4e 31 //AnalogIn t12(ADC3_1_t);
Haeun 6:beddcb25ad4e 32 //AnalogIn t13(ADC3_2_t);
Haeun 6:beddcb25ad4e 33 //AnalogIn t14(TIM1_t);
Haeun 6:beddcb25ad4e 34 //AnalogIn t15(TIM4_t);
Haeun 6:beddcb25ad4e 35 //AnalogIn t16(TIM9_t);
Haeun 6:beddcb25ad4e 36 //AnalogIn t17(TIM11_t);
Haeun 6:beddcb25ad4e 37
Haeun 6:beddcb25ad4e 38
Haeun 6:beddcb25ad4e 39
Haeun 6:beddcb25ad4e 40
Haeun 6:beddcb25ad4e 41
Haeun 6:beddcb25ad4e 42
Bhoney 3:cfb7cc54a3ba 43 Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control)
Haeun 5:fb5a46e37787 44
Haeun 5:fb5a46e37787 45 #ifdef ToF
Bhoney 3:cfb7cc54a3ba 46 Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result)
Bhoney 0:6f0f41537e2f 47
Haeun 6:beddcb25ad4e 48 VL53L1X tof1(ToF1_I2C_SDA, ToF1_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)
Haeun 6:beddcb25ad4e 49 VL53L1X tof2(ToF2_I2C_SDA, ToF2_I2C_SCL);
Haeun 6:beddcb25ad4e 50 VL53L1X tof3(ToF3_I2C_SDA, ToF3_I2C_SCL);
Bhoney 0:6f0f41537e2f 51
Haeun 6:beddcb25ad4e 52 //DigitalOut Xshut1(ToF_XShut_1);
Haeun 6:beddcb25ad4e 53 //DigitalOut Xshut2(ToF_XShut_2);
Haeun 6:beddcb25ad4e 54 //DigitalOut Xshut3(ToF_XShut_3);
Bhoney 1:fd1e7e2d0780 55 #endif
Bhoney 0:6f0f41537e2f 56
Bhoney 1:fd1e7e2d0780 57 DigitalOut reverseR(MOTOR_REVERSE_R);
Bhoney 1:fd1e7e2d0780 58 DigitalOut reverseL(MOTOR_REVERSE_L);
Bhoney 0:6f0f41537e2f 59
Bhoney 1:fd1e7e2d0780 60 AnalogOut throttleR(MOTOR_THROTTLE_R);
Bhoney 1:fd1e7e2d0780 61 AnalogOut throttleL(MOTOR_THROTTLE_L);
Haeun 4:97a11deb0ab7 62
Bhoney 2:284491e0f6bf 63 Timer timer;
Bhoney 2:284491e0f6bf 64 #ifdef BT_MODE
Bhoney 1:fd1e7e2d0780 65 char rxChar = 0;
Bhoney 1:fd1e7e2d0780 66 bool rxFlag = false;
Haeun 6:beddcb25ad4e 67 #endif
Haeun 6:beddcb25ad4e 68
Haeun 6:beddcb25ad4e 69 #ifdef BUTTON_MODE
Bhoney 0:6f0f41537e2f 70
Bhoney 1:fd1e7e2d0780 71 void rxData(){
Haeun 6:beddcb25ad4e 72 char temp = bt1.getc();
Bhoney 3:cfb7cc54a3ba 73 bt1.putc(0); //convention
Haeun 6:beddcb25ad4e 74 if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
Haeun 6:beddcb25ad4e 75 rxChar = temp;
Haeun 6:beddcb25ad4e 76 rxFlag = 1;
Haeun 6:beddcb25ad4e 77 }
Bhoney 1:fd1e7e2d0780 78 }
Haeun 6:beddcb25ad4e 79
Haeun 6:beddcb25ad4e 80 #endif
Haeun 6:beddcb25ad4e 81
Haeun 6:beddcb25ad4e 82 #ifdef JOYSTICK_MODE
Haeun 6:beddcb25ad4e 83 int X_position = 0;
Haeun 6:beddcb25ad4e 84 int Y_position = 0;
Haeun 6:beddcb25ad4e 85
Haeun 6:beddcb25ad4e 86 void rxData(){
Haeun 6:beddcb25ad4e 87 char temp[20];
Haeun 6:beddcb25ad4e 88 temp = bt1.gets();
Haeun 6:beddcb25ad4e 89 bt1.printf(0); //convention
Haeun 6:beddcb25ad4e 90 // if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
Haeun 6:beddcb25ad4e 91 // rxChar = temp;
Haeun 6:beddcb25ad4e 92 // rxFlag = 1;
Haeun 6:beddcb25ad4e 93 // }
Haeun 6:beddcb25ad4e 94 }
Bhoney 2:284491e0f6bf 95 #endif
Bhoney 0:6f0f41537e2f 96 int main()
Bhoney 0:6f0f41537e2f 97 {
Haeun 4:97a11deb0ab7 98 throttleL = 0;
Haeun 4:97a11deb0ab7 99 throttleR = 0;
Bhoney 2:284491e0f6bf 100 #ifdef BT_MODE
Bhoney 3:cfb7cc54a3ba 101 bt1.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 102 bt1.attach(&rxData, Serial::RxIrq);
Haeun 5:fb5a46e37787 103 #endif
Haeun 6:beddcb25ad4e 104 // #ifdef ToF_MODE
Haeun 4:97a11deb0ab7 105 bt2.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 106 // bt2.attach(&rxData2, Serial::RxIrq);
Haeun 6:beddcb25ad4e 107 // #endif
Bhoney 2:284491e0f6bf 108 int dis1, dis2, dis3;
Bhoney 2:284491e0f6bf 109
Bhoney 2:284491e0f6bf 110 reverseR = 0;
Bhoney 2:284491e0f6bf 111 reverseL = 0;
Bhoney 3:cfb7cc54a3ba 112
Haeun 6:beddcb25ad4e 113 // bool wheelDirection = true;
Haeun 6:beddcb25ad4e 114
Haeun 4:97a11deb0ab7 115
Haeun 6:beddcb25ad4e 116 #ifdef ToF_MODE
Haeun 6:beddcb25ad4e 117 ///// first ToF
Haeun 6:beddcb25ad4e 118 //
Haeun 6:beddcb25ad4e 119 tof1.begin();
Haeun 6:beddcb25ad4e 120 tof1.setDistanceMode(2);
Haeun 6:beddcb25ad4e 121 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 122
Haeun 6:beddcb25ad4e 123
Haeun 6:beddcb25ad4e 124 ///// second ToF
Haeun 6:beddcb25ad4e 125
Haeun 6:beddcb25ad4e 126 tof2.begin();
Haeun 6:beddcb25ad4e 127 tof2.setDistanceMode(2);
Haeun 6:beddcb25ad4e 128 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 129 //
Haeun 6:beddcb25ad4e 130 // ///// third ToF
Haeun 6:beddcb25ad4e 131 tof3.begin();
Haeun 6:beddcb25ad4e 132 tof3.setDistanceMode(2);
Haeun 6:beddcb25ad4e 133 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 134
Haeun 6:beddcb25ad4e 135 #endif
Haeun 6:beddcb25ad4e 136 //return INTEGER
Haeun 6:beddcb25ad4e 137 float speed = MIN_SPEED;
Bhoney 2:284491e0f6bf 138 while(1) {
Bhoney 2:284491e0f6bf 139
Bhoney 3:cfb7cc54a3ba 140 #ifdef ToF_MODE
Bhoney 3:cfb7cc54a3ba 141
Bhoney 3:cfb7cc54a3ba 142 ///// first ToF
Haeun 6:beddcb25ad4e 143 // tof1.startMeasurement();
Haeun 6:beddcb25ad4e 144 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 145 dis1 = tof1.getDistance();
Haeun 6:beddcb25ad4e 146 //
Haeun 6:beddcb25ad4e 147 // tof2.startMeasurement();
Bhoney 3:cfb7cc54a3ba 148 ///// second ToF
Haeun 6:beddcb25ad4e 149 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 150 dis2 = tof2.getDistance();
Bhoney 2:284491e0f6bf 151
Haeun 6:beddcb25ad4e 152 //
Haeun 6:beddcb25ad4e 153 // ///// third ToF
Haeun 6:beddcb25ad4e 154 // tof3.startMeasurement();
Haeun 6:beddcb25ad4e 155 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 156 dis3 = tof3.getDistance(); //return INTEGER
Haeun 6:beddcb25ad4e 157 // //end of ToF com.
Bhoney 2:284491e0f6bf 158
Bhoney 2:284491e0f6bf 159
Haeun 4:97a11deb0ab7 160
Haeun 4:97a11deb0ab7 161 //bt1.putc(0);
Haeun 4:97a11deb0ab7 162
Bhoney 2:284491e0f6bf 163 #endif
Haeun 6:beddcb25ad4e 164 bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
Haeun 6:beddcb25ad4e 165
Haeun 6:beddcb25ad4e 166 #ifdef JOYSTICK_MODE
Haeun 6:beddcb25ad4e 167
Haeun 6:beddcb25ad4e 168
Haeun 6:beddcb25ad4e 169
Haeun 6:beddcb25ad4e 170 #endif
Haeun 6:beddcb25ad4e 171
Haeun 6:beddcb25ad4e 172 #ifdef BUTTON_MODE
Haeun 6:beddcb25ad4e 173
Bhoney 1:fd1e7e2d0780 174 if(rxFlag)
Bhoney 1:fd1e7e2d0780 175 {
Haeun 6:beddcb25ad4e 176 rxFlag=0;
Haeun 6:beddcb25ad4e 177
Bhoney 1:fd1e7e2d0780 178 switch(rxChar){
Bhoney 3:cfb7cc54a3ba 179 case '1': // 전진
Haeun 4:97a11deb0ab7 180 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 181 speed=MIN_SPEED;
Haeun 6:beddcb25ad4e 182 throttleR=speed;
Haeun 6:beddcb25ad4e 183 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 184 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 185 reverseR = 0;
Haeun 6:beddcb25ad4e 186 reverseL = 0;
Haeun 4:97a11deb0ab7 187 }
Haeun 6:beddcb25ad4e 188
Bhoney 3:cfb7cc54a3ba 189 //need to be delayed
Haeun 6:beddcb25ad4e 190 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 191 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 192 else
Haeun 6:beddcb25ad4e 193 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 194 throttleR = speed;
Haeun 6:beddcb25ad4e 195 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 196 // bt1.printf("Speed : %f\n",throttleR);
Haeun 4:97a11deb0ab7 197 // throttleR = 1.0f;
Haeun 4:97a11deb0ab7 198 // throttleL = 1.0f;
Bhoney 1:fd1e7e2d0780 199 break;
Bhoney 1:fd1e7e2d0780 200
Bhoney 3:cfb7cc54a3ba 201 case '2': // 후진
Haeun 4:97a11deb0ab7 202 if(reverseR==0&&reverseL==0){
Haeun 6:beddcb25ad4e 203 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 204 throttleR=0;
Haeun 4:97a11deb0ab7 205 throttleL=0;
Haeun 6:beddcb25ad4e 206 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 207 reverseR = 1;
Haeun 6:beddcb25ad4e 208 reverseL = 1;
Haeun 4:97a11deb0ab7 209 }
Haeun 6:beddcb25ad4e 210
Bhoney 3:cfb7cc54a3ba 211 //need to be delayed
Haeun 6:beddcb25ad4e 212 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 213 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 214 else
Haeun 6:beddcb25ad4e 215 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 216 throttleR = speed;
Haeun 6:beddcb25ad4e 217 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 218 // throttleR = throttleR + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 219 // throttleL = throttleL + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 220 break;
Bhoney 3:cfb7cc54a3ba 221
Bhoney 3:cfb7cc54a3ba 222 case '3': // 좌회전
Haeun 4:97a11deb0ab7 223 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 224 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 225 throttleR=0;
Haeun 4:97a11deb0ab7 226 throttleL=0;
Haeun 6:beddcb25ad4e 227 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 228 reverseR = 0;
Haeun 4:97a11deb0ab7 229 reverseL = 0;
Haeun 4:97a11deb0ab7 230 }
Haeun 6:beddcb25ad4e 231 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 232 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 233 else
Haeun 6:beddcb25ad4e 234 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 235 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 236 // throttleR = speed+SPEED_CORRECTION;
Bhoney 3:cfb7cc54a3ba 237 break;
Bhoney 3:cfb7cc54a3ba 238
Haeun 4:97a11deb0ab7 239 case '4': // 우회전
Haeun 4:97a11deb0ab7 240 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 241 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 242 throttleR=0;
Haeun 4:97a11deb0ab7 243 throttleL=0;
Haeun 6:beddcb25ad4e 244 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 245 reverseR = 0;
Haeun 4:97a11deb0ab7 246 reverseL = 0;
Haeun 4:97a11deb0ab7 247 }
Haeun 6:beddcb25ad4e 248 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 249 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 250 else
Haeun 6:beddcb25ad4e 251 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 252 throttleR=speed;
Haeun 6:beddcb25ad4e 253 // throttleR=0;
Haeun 6:beddcb25ad4e 254 // throttleL = speed;
Bhoney 1:fd1e7e2d0780 255 break;
Bhoney 1:fd1e7e2d0780 256
Bhoney 3:cfb7cc54a3ba 257 case '5': // 정지
Haeun 6:beddcb25ad4e 258 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 259 throttleR=0;
Haeun 4:97a11deb0ab7 260 throttleL=0;
Haeun 4:97a11deb0ab7 261 break;
Bhoney 1:fd1e7e2d0780 262
Bhoney 1:fd1e7e2d0780 263 case '6':
Bhoney 3:cfb7cc54a3ba 264 // reverseL = !reverseL;
Haeun 6:beddcb25ad4e 265 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 266 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 267 throttleR=0;
Haeun 6:beddcb25ad4e 268 throttleL=0;
Haeun 6:beddcb25ad4e 269 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 270 reverseR = 0;
Haeun 6:beddcb25ad4e 271 reverseL = 0;
Haeun 6:beddcb25ad4e 272 }
Haeun 6:beddcb25ad4e 273 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 274 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 275 else
Haeun 6:beddcb25ad4e 276 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 277 // throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 278 throttleR = speed;
Haeun 4:97a11deb0ab7 279 break;
Haeun 6:beddcb25ad4e 280 case '7':
Haeun 6:beddcb25ad4e 281 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 282 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 283 throttleR=0;
Haeun 6:beddcb25ad4e 284 throttleL=0;
Haeun 6:beddcb25ad4e 285 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 286 reverseR = 0;
Haeun 6:beddcb25ad4e 287 reverseL = 0;
Haeun 6:beddcb25ad4e 288 }
Haeun 6:beddcb25ad4e 289 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 290 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 291 else
Haeun 6:beddcb25ad4e 292 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 293 // throttleR=speed;
Haeun 6:beddcb25ad4e 294 // throttleR=0;
Haeun 6:beddcb25ad4e 295 throttleL = speed;
Haeun 6:beddcb25ad4e 296 break;
Bhoney 1:fd1e7e2d0780 297 default:
Haeun 4:97a11deb0ab7 298 // reverseR = 0;
Haeun 4:97a11deb0ab7 299 // reverseL = 0;
Haeun 4:97a11deb0ab7 300 // throttleL = 0;
Haeun 4:97a11deb0ab7 301 // throttleR = 0;
Haeun 4:97a11deb0ab7 302
Bhoney 2:284491e0f6bf 303 break;
Bhoney 1:fd1e7e2d0780 304
Bhoney 1:fd1e7e2d0780 305 }
Haeun 6:beddcb25ad4e 306
Bhoney 1:fd1e7e2d0780 307 }
Bhoney 2:284491e0f6bf 308 #endif
Bhoney 0:6f0f41537e2f 309 }
Haeun 4:97a11deb0ab7 310 //#endif
Bhoney 0:6f0f41537e2f 311 }