byeongjin kim
joystick_bjk
Diff: main.cpp
- Revision:
- 6:beddcb25ad4e
- Parent:
- 5:fb5a46e37787
- Child:
- 7:1f4ef899e43e
- Child:
- 8:27d6afa7cb7c
--- a/main.cpp Mon Sep 09 07:53:29 2019 +0000
+++ b/main.cpp Mon Sep 16 12:06:13 2019 +0000
@@ -5,23 +5,53 @@
#define DEBUG
#define BT_MODE
-//#define ToF_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 tof(ToF_I2C_SDA, ToF_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)
+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);
+//DigitalOut Xshut1(ToF_XShut_1);
+//DigitalOut Xshut2(ToF_XShut_2);
+//DigitalOut Xshut3(ToF_XShut_3);
#endif
DigitalOut reverseR(MOTOR_REVERSE_R);
@@ -34,17 +64,34 @@
#ifdef BT_MODE
char rxChar = 0;
bool rxFlag = false;
+#endif
+
+#ifdef BUTTON_MODE
void rxData(){
- rxChar = bt1.getc();
+ char temp = bt1.getc();
bt1.putc(0); //convention
- rxFlag = true;
+ if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
+ rxChar = temp;
+ rxFlag = 1;
+ }
}
-//void rxData2(){
-// rxChar = bt2.getc();
-// bt2.putc(0); //convention
-// rxFlag = true;
-//}
+
+#endif
+
+#ifdef JOYSTICK_MODE
+int X_position = 0;
+int Y_position = 0;
+
+void rxData(){
+ char temp[20];
+ temp = bt1.gets();
+ bt1.printf(0); //convention
+// if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
+// rxChar = temp;
+// rxFlag = 1;
+// }
+}
#endif
int main()
{
@@ -52,144 +99,201 @@
throttleR = 0;
#ifdef BT_MODE
bt1.baud(UART_BAUD);
-
- bt1.attach(&rxData, Serial::RxIrq);
-
+ bt1.attach(&rxData, Serial::RxIrq);
#endif
- #ifdef ToF_MODE
+// #ifdef ToF_MODE
bt2.baud(UART_BAUD);
- //bt2.attach(&rxData2, Serial::RxIrq);
- #endif
+// bt2.attach(&rxData2, Serial::RxIrq);
+// #endif
int dis1, dis2, dis3;
reverseR = 0;
reverseL = 0;
- bool wheelDirection = true;
+// 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;
while(1) {
#ifdef ToF_MODE
///// first ToF
- Xshut1 = 1;
- Xshut2 = 0;
- Xshut3 = 0;
-
- tof.begin();
- tof.setDistanceMode();
- wait(ToF_SETUP_TIME);
-
- dis1 = tof.getDistance();
-
-
+// tof1.startMeasurement();
+ while(!tof1.newDataReady());
+ dis1 = tof1.getDistance();
+//
+// tof2.startMeasurement();
///// second ToF
- Xshut1 = 0;
- Xshut2 = 1;
- Xshut3 = 0;
-
- tof.begin();
- tof.setDistanceMode();
- wait(ToF_SETUP_TIME);
- dis2 = tof.getDistance();
-
+ while(!tof2.newDataReady());
+ dis2 = tof2.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.
+//
+// ///// third ToF
+// tof3.startMeasurement();
+ while(!tof3.newDataReady());
+ dis3 = tof3.getDistance(); //return INTEGER
+// //end of ToF com.
//bt1.putc(0);
#endif
- //bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
- #ifdef BT_MODE
+ bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
+
+ #ifdef JOYSTICK_MODE
+
+
+
+ #endif
+
+ #ifdef BUTTON_MODE
+
if(rxFlag)
{
- #ifdef DEBUG
-// bt1.puts("get\r\n");
- #endif
-
-// if(!(rxChar=='1'||rxChar=='2'||rxChar=='3'||rxChar=='4'||rxChar=='5'||rxChar=='6'))
-// rxChar=rxChar_prev;
-
+ rxFlag=0;
+
switch(rxChar){
case '1': // 전진
if(reverseR==1&&reverseL==1){
- throttleR=0;
- throttleL=0;
- wait(0.1);
+ speed=MIN_SPEED;
+ throttleR=speed;
+ throttleL=speed+SPEED_CORRECTION;
+ wait(TIME_DELAY_FOR_REVERSE);
+ reverseR = 0;
+ reverseL = 0;
}
- reverseR = 0;
- reverseL = 0;
-
+
//need to be delayed
-
- throttleR = throttleR + DAC_ONE_STEP;
- throttleL = throttleL + DAC_ONE_STEP;
+ 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(0.1);
+ wait(TIME_DELAY_FOR_REVERSE);
+ reverseR = 1;
+ reverseL = 1;
}
-
- reverseR = 1;
- reverseL = 1;
-
+
//need to be delayed
-
- throttleR = throttleR + DAC_ONE_STEP;
- throttleL = throttleL + DAC_ONE_STEP;
+ 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(0.1);
+ wait(TIME_DELAY_FOR_REVERSE);
reverseR = 0;
reverseL = 0;
}
-
- throttleR=0;
- throttleL = throttleL + DAC_ONE_STEP;
+ 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(0.1);
+ wait(TIME_DELAY_FOR_REVERSE);
reverseR = 0;
reverseL = 0;
}
- throttleL=0;
- throttleR = throttleR + DAC_ONE_STEP;
+ 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;
@@ -199,7 +303,7 @@
break;
}
- rxFlag = false;
+
}
#endif
}