Diff: main.cpp

Revision:

2:284491e0f6bf

Parent:

1:fd1e7e2d0780

Child:

3:cfb7cc54a3ba

--- a/main.cpp	Tue Aug 27 04:52:07 2019 +0000
+++ b/main.cpp	Thu Sep 05 10:02:40 2019 +0000
@@ -2,7 +2,9 @@
 #include "VL53L1X.h"
 #include "CONFIG.h"
 #define DEBUG
-
+//#define DEBUG_DAC
+#define BT_MODE
+//#define ToF_MODE
 #ifdef DEBUG
 DigitalOut led(LED1);
 #endif
@@ -24,6 +26,8 @@
 AnalogOut throttleR(MOTOR_THROTTLE_R);
 AnalogOut throttleL(MOTOR_THROTTLE_L);
 
+Timer timer;
+#ifdef BT_MODE
 char rxChar = 0;
 bool rxFlag = false;
 
@@ -32,21 +36,106 @@
     bt.putc(0);
     rxFlag = true;
 }
-
+#endif
 int main()
 {
-    bt.baud(UART_BAUD);
-    bt.attach(&rxData, Serial::RxIrq);
-    int i = 1;
-//    Xshut1 = 1;
+#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);
+    #endif
+    #ifdef ToF_MODE
+    int dis1, dis2, dis3;
+    
+    reverseR = 0;
+    reverseL = 0;
+    
+    bool wheelDirection = true;
+    #endif
+    while(1) {
+ 
+    #ifdef ToF_MODE       
+        Xshut1 = 1;
+        Xshut2 = 0;
+        Xshut3 = 0;
+      
+        tof.begin();
+        tof.setDistanceMode();
+        wait(ToF_SETUP_TIME);
+        
+        dis1 = tof.getDistance();
+//        bt.printf("dis1: %d\n", tof.getDistance());
+        
+        Xshut1 = 0;
+        Xshut2 = 1;
+        Xshut3 = 0;
+    
+        tof.begin();
+        tof.setDistanceMode();
+        wait(ToF_SETUP_TIME);
+        dis2 = tof.getDistance();  
+        bt.printf("dis2: %d\n", tof.getDistance() );
+        
+    
+        Xshut1 = 0;
+        Xshut2 = 0;
+        Xshut3 = 1;
+        
+        tof.begin();
+        tof.setDistanceMode();
+        wait(ToF_SETUP_TIME);
 
-//    bt.printf("Hello World !\n");
+        dis3 = tof.getDistance();          
+//        bt.printf("dis3: %d\n", tof.getDistance() );
+        
+        if( dis2 > HIGH_BOUND ){
+            if(!wheelDirection){
+                throttleL = 0;
+                throttleR = 0;                
+            reverseR = 0;
+            reverseL = 0;
+                wait(1.2f);
+            wheelDirection = true;
+
+            }
 
-//    tof.begin();
-//    tof.setDistanceMode();
+            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
     
-    while(1) {
-        
+        #ifdef BT_MODE
         if(rxFlag)
         {
             #ifdef DEBUG
@@ -82,16 +171,13 @@
                     reverseR = 0;
                     reverseL = 0;
                     throttleL = 0;
-                    throttleR
-                    
-                    
-                    
-                     = 0;
-                    
+                    throttleR = 0;
+                    break;
                 
             }
             rxFlag = false;
         }
-
+        #endif
     }
+#endif
 }