byeongjin kim
/
Hug2Go_ver_2
joystick_bjk
main.cpp
- Committer:
- Bhoney
- Date:
- 2019-09-05
- Revision:
- 3:cfb7cc54a3ba
- Parent:
- 2:284491e0f6bf
- Child:
- 4:97a11deb0ab7
File content as of revision 3:cfb7cc54a3ba:
#include "mbed.h" #include "VL53L1X.h" #include "CONFIG.h" #define DEBUG #define BT_MODE //#define ToF_MODE #ifdef DEBUG DigitalOut led(LED1); #endif 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) DigitalOut Xshut1(ToF_XShut_1); DigitalOut Xshut2(ToF_XShut_2); DigitalOut Xshut3(ToF_XShut_3); #endif DigitalOut reverseR(MOTOR_REVERSE_R); DigitalOut reverseL(MOTOR_REVERSE_L); AnalogOut throttleR(MOTOR_THROTTLE_R); AnalogOut throttleL(MOTOR_THROTTLE_L); Timer timer; #ifdef BT_MODE char rxChar = 0; bool rxFlag = false; void rxData(){ rxChar = bt1.getc(); bt1.putc(0); //convention rxFlag = true; } #endif int main() { #ifdef BT_MODE 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 ///// first ToF Xshut1 = 1; Xshut2 = 0; Xshut3 = 0; tof.begin(); tof.setDistanceMode(); wait(ToF_SETUP_TIME); dis1 = tof.getDistance(); ///// second ToF Xshut1 = 0; Xshut2 = 1; Xshut3 = 0; tof.begin(); tof.setDistanceMode(); wait(ToF_SETUP_TIME); dis2 = tof.getDistance(); ///// third ToF Xshut1 = 0; Xshut2 = 0; Xshut3 = 1; tof.begin(); tof.setDistanceMode(); wait(ToF_SETUP_TIME); dis3 = tof.getDistance(); //return INTEGER //end of ToF com. bt2.printf("%d|%d|%d\n", dis1, dis2, dis3); // 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 // bt1.puts("get\r\n"); #endif switch(rxChar){ case '1': // 전진 reverseR = 0; reverseL = 0; //need to be delayed throttleR = throttleR + DAC_ONE_STEP; throttleL = throttleL + DAC_ONE_STEP; break; 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 '5': // 정지 // reverseR = !reverseR; // break; case '6': // reverseL = !reverseL; // break; default: reverseR = 0; reverseL = 0; throttleL = 0; throttleR = 0; break; } rxFlag = false; } #endif } #endif }