joystick_bjk

Dependencies:   mbed VL53L1X

Revision:
4:97a11deb0ab7
Parent:
3:cfb7cc54a3ba
Child:
5:fb5a46e37787
--- 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
 }