![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
joystick_bjk
main.cpp
- Committer:
- Haeun
- Date:
- 2019-09-19
- Revision:
- 8:27d6afa7cb7c
- Parent:
- 6:beddcb25ad4e
File content as of revision 8:27d6afa7cb7c:
#include "mbed.h" #include "VL53L1X.h" #include "CONFIG.h" #define DEBUG #define BT_MODE //#define BUTTON_MODE #define JOYSTICK_MODE #define ToF_MODE #define ToF #ifdef DEBUG //DigitalOut led(LED1); #endif //reserved pin //AnalogIn t1(SPI2_SCK); //AnalogIn t2(SPI2_NSS); //AnalogIn t3(SPI2_MISO); //AnalogIn t4(SPI2_MOSI); //AnalogIn t5(SPI3_SCK); //AnalogIn t6(SPI3_NSS); //AnalogIn t7(SPI3_MISO); //AnalogIn t8(SPI3_MOSI); //AnalogIn t9(USART3_TX); //AnalogIn t10(USART3_RX); //AnalogIn t11(ADC2_t); //AnalogIn t12(ADC3_1_t); //AnalogIn t13(ADC3_2_t); //AnalogIn t14(TIM1_t); //AnalogIn t15(TIM4_t); //AnalogIn t16(TIM9_t); //AnalogIn t17(TIM11_t); Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control) #ifdef ToF Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result) VL53L1X tof1(ToF1_I2C_SDA, ToF1_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8) VL53L1X tof2(ToF2_I2C_SDA, ToF2_I2C_SCL); VL53L1X tof3(ToF3_I2C_SDA, ToF3_I2C_SCL); //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; #endif #ifdef BUTTON_MODE void rxData(){ char temp = bt1.getc(); bt1.putc(0); //convention if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){ rxChar = temp; rxFlag = 1; } } #endif #ifdef JOYSTICK_MODE int x_position = 0; int y_position = 0; void rxData(){ char temp[5]; // int x_temp=0,y_temp=0; if('('==bt1.getc()){ for(int i=1;i<6&&temp[i-2]!=',';i++){ temp[i-1]=bt1.getc(); } x_position=atoi(temp); for(int i=1;i<6&&temp[i-2]!=')';i++){ temp[i-1]=bt1.getc(); } y_position=atoi(temp); // bt2.printf("X:%d,Y:%d\n",x_temp,y_temp); } } #endif int main() { throttleL = 0; throttleR = 0; #ifdef BT_MODE bt1.baud(UART_BAUD); bt1.attach(&rxData, Serial::RxIrq); #endif // #ifdef ToF_MODE bt2.baud(UART_BAUD); // bt2.attach(&rxData2, Serial::RxIrq); // #endif int dis1, dis2, dis3; reverseR = 0; reverseL = 0; // bool wheelDirection = true; #ifdef ToF_MODE ///// first ToF // tof1.begin(); tof1.setDistanceMode(2); while(!tof1.newDataReady()); ///// second ToF tof2.begin(); tof2.setDistanceMode(2); while(!tof2.newDataReady()); // // ///// third ToF tof3.begin(); tof3.setDistanceMode(2); while(!tof3.newDataReady()); #endif //return INTEGER float speed = MIN_SPEED; bool forward=1; float speed_joystick =0,direction=0; while(1) { #ifdef ToF_MODE ///// first ToF // tof1.startMeasurement(); while(!tof1.newDataReady()); dis1 = tof1.getDistance(); // // tof2.startMeasurement(); ///// second ToF while(!tof2.newDataReady()); dis2 = tof2.getDistance(); // // ///// third ToF // tof3.startMeasurement(); while(!tof3.newDataReady()); dis3 = tof3.getDistance(); //return INTEGER // //end of ToF com. //bt1.putc(0); #endif #ifdef JOYSTICK_MODE bt2.printf("%d|%d|%d\n", dis1, dis2, dis3); speed_joystick = (float)y_position/250; direction = (float)x_position/2000; //bt2.printf("X:%d,Y:%d\n",x_position,y_position); //bt2.printf("speed:%1.3f,dir:%1.3f\n",speed,direction); if(speed_joystick>0&&forward==0){ forward=1; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; wait(1.0); } else if(speed_joystick<0&&forward==1){ throttleR=0; throttleL=0; forward=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 1; reverseL = 1; wait(1.0); } if(direction<0){//left throttleR = abs(speed_joystick)+SPEED_CORRECTION+0.01f; throttleL = abs(speed_joystick)-SPEED_CORRECTION+direction; } else{//right throttleR = speed_joystick-direction; throttleL = speed_joystick+SPEED_CORRECTION; } #endif #ifdef BUTTON_MODE bt2.printf("%d|%d|%d\n", dis1, dis2, dis3); if(rxFlag) { rxFlag=0; switch(rxChar){ case '1': // 전진 if(reverseR==1&&reverseL==1){ speed=MIN_SPEED; throttleR=speed; throttleL=speed+SPEED_CORRECTION; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; } //need to be delayed if(speed <= MAX_SPEED-DAC_ONE_STEP) speed = speed + DAC_ONE_STEP; else speed = MAX_SPEED; throttleR = speed; throttleL = speed+SPEED_CORRECTION; // bt1.printf("Speed : %f\n",throttleR); // throttleR = 1.0f; // throttleL = 1.0f; break; case '2': // 후진 if(reverseR==0&&reverseL==0){ speed = MIN_SPEED; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 1; reverseL = 1; } //need to be delayed if(speed <= MAX_SPEED-DAC_ONE_STEP) speed = speed + DAC_ONE_STEP; else speed = MAX_SPEED; throttleR = speed; throttleL = speed+SPEED_CORRECTION; // throttleR = throttleR + DAC_ONE_STEP; // throttleL = throttleL + DAC_ONE_STEP; break; case '3': // 좌회전 if(reverseR==1&&reverseL==1){ speed = MIN_SPEED; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; } if(speed > MIN_SPEED+DAC_ONE_STEP) speed = speed - DAC_ONE_STEP; else speed = MIN_SPEED; throttleL=speed+SPEED_CORRECTION; // throttleR = speed+SPEED_CORRECTION; break; case '4': // 우회전 if(reverseR==1&&reverseL==1){ speed = MIN_SPEED; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; } if(speed > MIN_SPEED+DAC_ONE_STEP) speed = speed - DAC_ONE_STEP; else speed = MIN_SPEED; throttleR=speed; // throttleR=0; // throttleL = speed; break; case '5': // 정지 speed = MIN_SPEED; throttleR=0; throttleL=0; break; case '6': // reverseL = !reverseL; if(reverseR==1&&reverseL==1){ speed = MIN_SPEED; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; } if(speed <= MAX_SPEED-DAC_ONE_STEP) speed = speed + DAC_ONE_STEP; else speed = MAX_SPEED; // throttleL=speed+SPEED_CORRECTION; throttleR = speed; break; case '7': if(reverseR==1&&reverseL==1){ speed = MIN_SPEED; throttleR=0; throttleL=0; wait(TIME_DELAY_FOR_REVERSE); reverseR = 0; reverseL = 0; } if(speed <= MAX_SPEED-DAC_ONE_STEP) speed = speed + DAC_ONE_STEP; else speed = MAX_SPEED; // throttleR=speed; // throttleR=0; throttleL = speed; break; default: // reverseR = 0; // reverseL = 0; // throttleL = 0; // throttleR = 0; break; } } #endif } //#endif }