Diff: main.cpp

Revision:

15:151e59899221

Parent:

14:a646667ac9ea

Child:

16:c26d8e007df5

--- a/main.cpp	Fri May 19 16:46:21 2017 +0000
+++ b/main.cpp	Sat May 20 05:17:47 2017 +0000
@@ -31,91 +31,103 @@
 void setup() {
     // pc.baud(9600);
     // using the serial functions will have an impact on timing.
-//    serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
+    // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
     // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
-    if (battery.read() < 0.67f){
+    if (battery.read() < 0.70f){
         // flash led
         led_1 = 1;
         led_2 = 1;
         led_3 = 1;
         led_4 = 1;
-    }  
+    }
 }
 
 // Performs the basic drive control of the mouse
 int main() {
-    
     DriveControl * driver = new DriveControl (START_POS, END_POS);
     driver->resetEncoders();
     int state;
     bool finished_traverse_one_cell = true;
     while(1) {
         setup();
-        led_2 = 0;
-        led_3 = 0;
-        driver->getEncoder();
-        
-        
-        wait(2);
-        /*
-        
         // switch state
+        wait_ms(1);
         if (finished_traverse_one_cell == true) {
             if (!driver->has_front_wall()) {
                 state = STRAIGHT;
+                driver->resetEncoders();
+            }
+            else if (!driver->has_left_wall()) {
+                state = LEFT;  
+                driver->resetEncoders();  
+            }
+            else {
+                state = RIGHT;
+                driver->resetEncoders();
+            }
+            finished_traverse_one_cell = false;
+         
+        /*
+            if (!driver->has_left_wall()) {
+                state = LEFT;  
+                driver->resetEncoders();  
             }
             else if (!driver->has_right_wall()) {
-                state = RIGHT;    
+                state = RIGHT;  
+                driver->resetEncoders();  
             }
             else {
-                state = LEFT;
+                state = STRAIGHT;
+                driver->resetEncoders();
             }
-            finished_traverse_one_cell = false;
+            finished_traverse_one_cell = false;*/
         }
         
         if (state == STRAIGHT) {
-            if (!driver->should_stop_drive_forward()) {
+            if (!driver->has_front_wall()) {
                 driver->drive_forward();
             }
             else {
-                    driver->stop();
-                    driver->resetEncoders();
-                    led_2 = 1;
-                    led_3 = 0;
+                driver->stop();
+                wait(0.5);
+                led_1 = 1;
+                led_2 = 1;
+                led_3 = 0;
+                    led_4 = 0;
                     finished_traverse_one_cell = true;
                     continue;
             }
         }
-        
         if (state == RIGHT) {
             if (!driver->should_finish_turn_right()) {
                 driver->turn_right();
             }
             else {
                 driver->stop();
-                driver->resetEncoders();
-                led_2 = 0;
+                wait(0.5);
+                led_1 = 0;
+                led_2 = 1;
                 led_3 = 1;
+                led_4 = 0;
                 finished_traverse_one_cell = true;
                 continue;
             }
         }
-        
         if (state == LEFT) {
             if (!driver->should_finish_turn_left()) {
                 driver->turn_left();
             }
             else {
                 driver->stop();
-                driver->resetEncoders();
-                led_1 = 1;
+                wait(0.5);
+                led_1 = 0;
+                led_2 = 0;
+                led_3 = 1;
                 led_4 = 1;
-                led_2 = 0;
-                led_3 = 0;
                 finished_traverse_one_cell = true;
                 continue;
             }
-        }
-        wait_ms(1); */
+        } 
+        
     }
 }
\ No newline at end of file