Diff: main.cpp

Revision:

18:85196734207e

Parent:

17:043ed1d0196f

--- a/main.cpp	Sat May 20 21:34:06 2017 +0000
+++ b/main.cpp	Sat May 20 22:37:18 2017 +0000
@@ -9,10 +9,15 @@
 DigitalOut led_4(PB_15);
 Serial serial (PA_9, PA_10);
 // CONTROL CONSTANTS
-const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
+const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3, REVERSE  = 4;
 const int START_POS = 0, END_POS = 0;
 const int CONTROL = 1;
 
+int state, count = 0;
+DriveControl * driver;
+bool finished_traverse_one_cell = true;
+Timer t;
+
 void check_battery () {
     // pc.baud(9600);
     // using the serial functions will have an impact on timing.
@@ -34,55 +39,69 @@
     led_4 = _led_4;
 }
 
-int main() {
-    DriveControl * driver = new DriveControl (START_POS, END_POS);
-    driver->resetEncoders();
-    int state, count = 0;
-    bool finished_traverse_one_cell = true;
-    //wait(1.5);
-    while(CONTROL) {
-        check_battery();
-        wait_ms(1);
-
-        //wait(2);
-        //driver->print_serial_ports();
-    
-        
-        if (finished_traverse_one_cell == true) {
+int get_next_state() {
+    if (finished_traverse_one_cell == true) {
+            t.reset();
             if (!driver->has_right_wall()) {
-                state = RIGHT;  
-                driver->resetEncoders();  
+                state = RIGHT;
+                driver->resetEncoders();
+                wait(1);  
+                t.start();
             }
             else if (!driver->has_left_wall()) {
-                state = LEFT;  
-                driver->resetEncoders();  
+                state = LEFT;
+                driver->resetEncoders();   
+                wait(1);
+                t.start();
             }
             else if(!driver->has_front_wall()){
                 state = STRAIGHT;
                 driver->resetEncoders();
+                t.start();
             }
             else{
                 state = UTURN;
                 driver->resetEncoders();
                 count = 0;
+                wait(1);
+                t.start();
             }
             finished_traverse_one_cell = false;
         }
-        
-        
-        if (state == STRAIGHT) {
-            /*
-            if (!driver->has_front_wall() && !driver->has_front_wall()) {
-                driver->drive_forward();
+    return state;
+}
+
+void print() {
+    wait(2);
+    driver->print_serial_ports();
+}
+
+
+int main() {
+    driver = new DriveControl (START_POS, END_POS);
+    driver->resetEncoders();
+    //wait(1.5);
+    while(CONTROL) {
+        check_battery();
+        wait_ms(1);
+        // Check if the mouse has been stucked on the wall
+        float curr_time = t.read();
+        /*
+        if (curr_time > 5 && driver->should_reverse()) {
+            if (curr_time < 1) {
+                state = REVERSE;
+                driver->reverse();
             }
             else {
-                driver->stop();
-                wait(0.25);
-                flash_led (1, 1, 0, 0);
-                finished_traverse_one_cell = true;
-                continue;
-            } 
-            */
+                state = STRAIGHT;
+            }
+        } 
+        if (state != REVERSE) {
+            state = get_next_state();
+        } */
+        
+        state = get_next_state();
+        if (state == STRAIGHT) {
             if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
                 driver->drive_forward();
                 flash_led(1, 0, 0, 0);
@@ -91,7 +110,7 @@
                 driver->clear_pid();
                 driver->stop();
                 driver->resetEncoders();
-                wait(0.2);
+                // wait(0.2);
                 flash_led (0, 0, 0, 0);
                 finished_traverse_one_cell = true;
                 continue;
@@ -104,7 +123,7 @@
             }
             else {
                 driver->stop();
-                wait(0.25);
+                wait(0.2);
                 flash_led (0, 0, 0, 0);
                 //finished_traverse_one_cell = true;
                 state = STRAIGHT;
@@ -120,7 +139,7 @@
             }
             else {
                 driver->stop();
-                wait(0.25);
+                wait(0.2);
                 flash_led (0, 0, 0, 0);
                 //finished_traverse_one_cell = true;
                 state = STRAIGHT;
@@ -138,7 +157,7 @@
             else {
                 driver->stop();
                 driver->resetEncoders();
-                wait(0.25);
+                wait(0.2);
                 flash_led (0, 0, 0, 0);
                 count == 1 ? finished_traverse_one_cell = true: count++;
                 continue;