byeongjin kim
joystick_bjk
Diff: main.cpp
- Revision:
- 4:97a11deb0ab7
- Parent:
- 3:cfb7cc54a3ba
- Child:
- 5:fb5a46e37787
diff -r cfb7cc54a3ba -r 97a11deb0ab7 main.cpp
--- a/main.cpp Thu Sep 05 10:24:20 2019 +0000
+++ b/main.cpp Fri Sep 06 01:39:40 2019 +0000
@@ -5,11 +5,11 @@
#define DEBUG
#define BT_MODE
-//#define ToF_MODE
+#define ToF_MODE
#ifdef DEBUG
-DigitalOut led(LED1);
+//DigitalOut led(LED1);
#endif
Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control)
@@ -28,7 +28,7 @@
AnalogOut throttleR(MOTOR_THROTTLE_R);
AnalogOut throttleL(MOTOR_THROTTLE_L);
-
+
Timer timer;
#ifdef BT_MODE
char rxChar = 0;
@@ -39,22 +39,30 @@
bt1.putc(0); //convention
rxFlag = true;
}
+//void rxData2(){
+// rxChar = bt2.getc();
+// bt2.putc(0); //convention
+// rxFlag = true;
+//}
#endif
int main()
{
-
+ throttleL = 0;
+ throttleR = 0;
#ifdef BT_MODE
bt1.baud(UART_BAUD);
+ bt2.baud(UART_BAUD);
bt1.attach(&rxData, Serial::RxIrq);
+ //bt2.attach(&rxData2, Serial::RxIrq);
#endif
- #ifdef ToF_MODE
+
int dis1, dis2, dis3;
reverseR = 0;
reverseL = 0;
bool wheelDirection = true;
- #endif
+
while(1) {
#ifdef ToF_MODE
@@ -94,55 +102,29 @@
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;
-// }
+
+ //bt1.putc(0);
+
#endif
-
- #ifdef BT_MODE
+ //bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
+ #ifdef BT_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;
+
switch(rxChar){
case '1': // 전진
+ if(reverseR==1&&reverseL==1){
+ throttleR=0;
+ throttleL=0;
+ wait(0.1);
+ }
reverseR = 0;
reverseL = 0;
@@ -150,39 +132,66 @@
throttleR = throttleR + DAC_ONE_STEP;
throttleL = throttleL + DAC_ONE_STEP;
+// throttleR = 1.0f;
+// throttleL = 1.0f;
break;
case '2': // 후진
+ if(reverseR==0&&reverseL==0){
+ throttleR=0;
+ throttleL=0;
+ wait(0.1);
+ }
+
reverseR = 1;
reverseL = 1;
//need to be delayed
- throttleR = throttleR - DAC_ONE_STEP;
+ throttleR = throttleR + DAC_ONE_STEP;
throttleL = throttleL + DAC_ONE_STEP;
break;
case '3': // 좌회전
+ if(reverseR==1&&reverseL==1){
+ throttleR=0;
+ throttleL=0;
+ wait(0.1);
+ reverseR = 0;
+ reverseL = 0;
+ }
+
+ throttleR=0;
throttleL = throttleL + DAC_ONE_STEP;
break;
- case '4': // 우회진
- throttleR = throttleR - DAC_ONE_STEP;
+ case '4': // 우회전
+ if(reverseR==1&&reverseL==1){
+ throttleR=0;
+ throttleL=0;
+ wait(0.1);
+ reverseR = 0;
+ reverseL = 0;
+ }
+ throttleL=0;
+ throttleR = throttleR + DAC_ONE_STEP;
break;
case '5': // 정지
-// reverseR = !reverseR;
-// break;
+ throttleR=0;
+ throttleL=0;
+ break;
case '6':
// reverseL = !reverseL;
-// break;
+ break;
default:
- reverseR = 0;
- reverseL = 0;
- throttleL = 0;
- throttleR = 0;
+// reverseR = 0;
+// reverseL = 0;
+// throttleL = 0;
+// throttleR = 0;
+
break;
}
@@ -190,5 +199,5 @@
}
#endif
}
-#endif
+//#endif
}