Diff: main.cpp

Revision:

3:cfb7cc54a3ba

Parent:

2:284491e0f6bf

Child:

4:97a11deb0ab7

--- a/main.cpp	Thu Sep 05 10:02:40 2019 +0000
+++ b/main.cpp	Thu Sep 05 10:24:20 2019 +0000
@@ -1,16 +1,19 @@
 #include "mbed.h"
 #include "VL53L1X.h"
 #include "CONFIG.h"
+
 #define DEBUG
-//#define DEBUG_DAC
 #define BT_MODE
+
 //#define ToF_MODE
+
+
 #ifdef DEBUG
 DigitalOut led(LED1);
 #endif
 
-Serial bt(BLUETOOTH_TX, BLUETOOTH_RX);
-//Serial pc(USBTX, USBRX);
+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)
@@ -32,39 +35,31 @@
 bool rxFlag = false;
 
 void rxData(){
-    rxChar = bt.getc();
-    bt.putc(0);
+    rxChar = bt1.getc();
+    bt1.putc(0);            //convention
     rxFlag = true;
 }
 #endif
 int main()
 {
-#ifdef DEBUG_DAC
-    while(1){
-        throttleR = 1;
-        throttleL = 1;
-        wait(2);
-        throttleR = 0;
-        throttleL = 0;
-        wait(2);
-        
-    }
-#else
+
     #ifdef BT_MODE
-        bt.baud(UART_BAUD);
-        bt.attach(&rxData, Serial::RxIrq);
+        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       
+    #ifdef ToF_MODE     
+        
+        ///// first ToF
         Xshut1 = 1;
         Xshut2 = 0;
         Xshut3 = 0;
@@ -74,8 +69,9 @@
         wait(ToF_SETUP_TIME);
         
         dis1 = tof.getDistance();
-//        bt.printf("dis1: %d\n", tof.getDistance());
+        
         
+        ///// second ToF
         Xshut1 = 0;
         Xshut2 = 1;
         Xshut3 = 0;
@@ -84,9 +80,9 @@
         tof.setDistanceMode();
         wait(ToF_SETUP_TIME);
         dis2 = tof.getDistance();  
-        bt.printf("dis2: %d\n", tof.getDistance() );
+        
         
-    
+        ///// third ToF
         Xshut1 = 0;
         Xshut2 = 0;
         Xshut3 = 1;
@@ -95,77 +91,92 @@
         tof.setDistanceMode();
         wait(ToF_SETUP_TIME);
 
-        dis3 = tof.getDistance();          
-//        bt.printf("dis3: %d\n", tof.getDistance() );
+        dis3 = tof.getDistance();                   //return INTEGER
+        //end of ToF com.
         
-        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;
-        }
+        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
         
-        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;            
-        }
+        // 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
-            bt.puts("get\r\n");
+//            bt1.puts("get\r\n");
             #endif
             
             switch(rxChar){
-                case '1':
+                case '1':                                   // 전진
+                    reverseR = 0;
+                    reverseL = 0;
+                    
+                    //need to be delayed
+
                     throttleR = throttleR + DAC_ONE_STEP;
+                    throttleL = throttleL + DAC_ONE_STEP;
                     break;
                     
-                case '2':
+                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 '3':
-                    throttleL = throttleL + DAC_ONE_STEP;
-                    break;
-                    
-                case '4':
-                    throttleL = throttleL - DAC_ONE_STEP;
-                    break;
-                    
-                case '5':
-                    reverseR = !reverseR;
-                    break;
+                case '5':                                   // 정지
+//                    reverseR = !reverseR;
+//                    break;
                     
                 case '6':
-                    reverseL = !reverseL;
-                    break;
+//                    reverseL = !reverseL;
+//                    break;
                     
                 default:    
                     reverseR = 0;