byeongjin kim
/
Hug2Go_ver_2
joystick_bjk
Diff: main.cpp
- Revision:
- 3:cfb7cc54a3ba
- Parent:
- 2:284491e0f6bf
- Child:
- 4:97a11deb0ab7
diff -r 284491e0f6bf -r cfb7cc54a3ba main.cpp --- a/main.cpp Thu Sep 05 10:02:40 2019 +0000 +++ b/main.cpp Thu Sep 05 10:24:20 2019 +0000 @@ -1,16 +1,19 @@ #include "mbed.h" #include "VL53L1X.h" #include "CONFIG.h" + #define DEBUG -//#define DEBUG_DAC #define BT_MODE + //#define ToF_MODE + + #ifdef DEBUG DigitalOut led(LED1); #endif -Serial bt(BLUETOOTH_TX, BLUETOOTH_RX); -//Serial pc(USBTX, USBRX); +Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control) +Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result) #ifdef ToF VL53L1X tof(ToF_I2C_SDA, ToF_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8) @@ -32,39 +35,31 @@ bool rxFlag = false; void rxData(){ - rxChar = bt.getc(); - bt.putc(0); + rxChar = bt1.getc(); + bt1.putc(0); //convention rxFlag = true; } #endif int main() { -#ifdef DEBUG_DAC - while(1){ - throttleR = 1; - throttleL = 1; - wait(2); - throttleR = 0; - throttleL = 0; - wait(2); - - } -#else + #ifdef BT_MODE - bt.baud(UART_BAUD); - bt.attach(&rxData, Serial::RxIrq); + bt1.baud(UART_BAUD); + bt1.attach(&rxData, Serial::RxIrq); #endif #ifdef ToF_MODE int dis1, dis2, dis3; reverseR = 0; reverseL = 0; - + bool wheelDirection = true; #endif while(1) { - #ifdef ToF_MODE + #ifdef ToF_MODE + + ///// first ToF Xshut1 = 1; Xshut2 = 0; Xshut3 = 0; @@ -74,8 +69,9 @@ wait(ToF_SETUP_TIME); dis1 = tof.getDistance(); -// bt.printf("dis1: %d\n", tof.getDistance()); + + ///// second ToF Xshut1 = 0; Xshut2 = 1; Xshut3 = 0; @@ -84,9 +80,9 @@ tof.setDistanceMode(); wait(ToF_SETUP_TIME); dis2 = tof.getDistance(); - bt.printf("dis2: %d\n", tof.getDistance() ); + - + ///// third ToF Xshut1 = 0; Xshut2 = 0; Xshut3 = 1; @@ -95,77 +91,92 @@ tof.setDistanceMode(); wait(ToF_SETUP_TIME); - dis3 = tof.getDistance(); -// bt.printf("dis3: %d\n", tof.getDistance() ); + dis3 = tof.getDistance(); //return INTEGER + //end of ToF com. - if( dis2 > HIGH_BOUND ){ - if(!wheelDirection){ - throttleL = 0; - throttleR = 0; - reverseR = 0; - reverseL = 0; - wait(1.2f); - wheelDirection = true; - - } - - throttleL = 0.9f; - throttleR = 0.9f; - } + bt2.printf("%d|%d|%d\n", dis1, dis2, dis3); - else if ( dis2 < LOW_BOUND ){ - if(wheelDirection){ - throttleL = 0; - throttleR = 0; - - reverseR = 1; - reverseL = 1; - wait(1.2f); - wheelDirection = false; - } - - wait(0.01f); - - throttleL = 0.9f; - throttleR = 0.9f; - } - else{ - throttleL = 0; - throttleR = 0; - } + // obstacle avoiding +// if( dis2 > HIGH_BOUND ){ +// if(!wheelDirection){ +// throttleL = 0; +// throttleR = 0; +// reverseR = 0; +// reverseL = 0; +// wait(1.2f); +// wheelDirection = true; +// +// } +// +// throttleL = 0.9f; +// throttleR = 0.9f; +// } +// +// else if ( dis2 < LOW_BOUND ){ +// if(wheelDirection){ +// throttleL = 0; +// throttleR = 0; +// +// reverseR = 1; +// reverseL = 1; +// wait(1.2f); +// wheelDirection = false; +// } +// +// wait(0.01f); +// +// throttleL = 0.9f; +// throttleR = 0.9f; +// } +// else{ +// throttleL = 0; +// throttleR = 0; +// } #endif #ifdef BT_MODE if(rxFlag) { #ifdef DEBUG - bt.puts("get\r\n"); +// bt1.puts("get\r\n"); #endif switch(rxChar){ - case '1': + case '1': // 전진 + reverseR = 0; + reverseL = 0; + + //need to be delayed + throttleR = throttleR + DAC_ONE_STEP; + throttleL = throttleL + DAC_ONE_STEP; break; - case '2': + case '2': // 후진 + reverseR = 1; + reverseL = 1; + + //need to be delayed + + throttleR = throttleR - DAC_ONE_STEP; + throttleL = throttleL + DAC_ONE_STEP; + break; + + case '3': // 좌회전 + throttleL = throttleL + DAC_ONE_STEP; + break; + + case '4': // 우회진 throttleR = throttleR - DAC_ONE_STEP; break; - case '3': - throttleL = throttleL + DAC_ONE_STEP; - break; - - case '4': - throttleL = throttleL - DAC_ONE_STEP; - break; - - case '5': - reverseR = !reverseR; - break; + case '5': // 정지 +// reverseR = !reverseR; +// break; case '6': - reverseL = !reverseL; - break; +// reverseL = !reverseL; +// break; default: reverseR = 0;