Diff: main.cpp

Revision:

14:a646667ac9ea

Parent:

9:65a9aad9a5b5

Child:

15:151e59899221

--- a/main.cpp	Wed May 17 07:27:46 2017 +0000
+++ b/main.cpp	Fri May 19 16:46:21 2017 +0000
@@ -21,6 +21,7 @@
 // Direction of which to turn
 // const int LEFT = 0, RIGHT = 1;
 // Start and End Pos
+const int STRAIGHT = 0, LEFT = 1, RIGHT = 2;
 const int START_POS = 0, END_POS = 0;
 
 // Terminating condition for the main control loop
@@ -43,32 +44,78 @@
 
 // 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_1 = 1;
+        led_2 = 0;
+        led_3 = 0;
         driver->getEncoder();
-        //wait(1);
-        if (!driver->has_front_wall()) {
-            driver->drive_forward();
-        }
-        else{
-            driver->stop();
-        }
-        //wait_ms(1);
+        
+        
+        wait(2);
         /*
-        else if (!driver->has_right_wall) {
-            // driver->turn_right();
-            // turn_right() in drivecontrol will call pid_controller::turn(RIGHT)   
+        
+        // switch state
+        if (finished_traverse_one_cell == true) {
+            if (!driver->has_front_wall()) {
+                state = STRAIGHT;
+            }
+            else if (!driver->has_right_wall()) {
+                state = RIGHT;    
+            }
+            else {
+                state = LEFT;
+            }
+            finished_traverse_one_cell = false;
         }
-        else if (!driver->has_left_wall) {
-            // driver->turn_left()
+        
+        if (state == STRAIGHT) {
+            if (!driver->should_stop_drive_forward()) {
+                driver->drive_forward();
+            }
+            else {
+                    driver->stop();
+                    driver->resetEncoders();
+                    led_2 = 1;
+                    led_3 = 0;
+                    finished_traverse_one_cell = true;
+                    continue;
+            }
         }
-        else { // has all three walls
-            // driver->turn_around();
-        }*/
+        
+        if (state == RIGHT) {
+            if (!driver->should_finish_turn_right()) {
+                driver->turn_right();
+            }
+            else {
+                driver->stop();
+                driver->resetEncoders();
+                led_2 = 0;
+                led_3 = 1;
+                finished_traverse_one_cell = true;
+                continue;
+            }
+        }
         
-     
-//        driver->stop();
+        if (state == LEFT) {
+            if (!driver->should_finish_turn_left()) {
+                driver->turn_left();
+            }
+            else {
+                driver->stop();
+                driver->resetEncoders();
+                led_1 = 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