Diff: main.cpp

Revision:

16:c26d8e007df5

Parent:

15:151e59899221

Child:

17:043ed1d0196f

--- a/main.cpp	Sat May 20 05:17:47 2017 +0000
+++ b/main.cpp	Sat May 20 21:15:30 2017 +0000
@@ -1,34 +1,19 @@
 #include "drivecontrol.h"
-//#include "pin_assignment.h"
 #include "io_modules.h"
 #include "mbed.h"
-
-// Battery Debugger Interface
+//// PIN ASSIGNMENTS
 AnalogIn battery(PA_3);
-
-
-// Led Debugger Interface
 DigitalOut led_1(PB_12);
 DigitalOut led_2(PB_13);
 DigitalOut led_3(PB_14);
 DigitalOut led_4(PB_15);
-
-// System io
 Serial serial (PA_9, PA_10);
+// CONTROL CONSTANTS
+const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
+const int START_POS = 0, END_POS = 0;
+const int CONTROL = 1;
 
-// Define states for debugging the mouse hardware
-// const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
-// 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
-bool hasFoundCenter = false;
-
-// Battery Consumption Indicator
-void setup() {
+void check_battery () {
     // 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);
@@ -42,92 +27,150 @@
     }
 }
 
-// Performs the basic drive control of the mouse
+void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
+    led_1 = _led_1;
+    led_2 = _led_2;
+    led_3 = _led_3;
+    led_4 = _led_4;
+}
+
 int main() {
     DriveControl * driver = new DriveControl (START_POS, END_POS);
     driver->resetEncoders();
-    int state;
+    int state, count = 0;
     bool finished_traverse_one_cell = true;
-    while(1) {
-        setup();
-        // switch state
+    //wait(1.5);
+    while(CONTROL) {
+        check_battery();
         wait_ms(1);
+
+        //wait(2);
+        //driver->print_serial_ports();
+    
+        
         if (finished_traverse_one_cell == true) {
-            if (!driver->has_front_wall()) {
-                state = STRAIGHT;
-                driver->resetEncoders();
+            if (!driver->has_right_wall()) {
+                state = RIGHT;  
+                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;  
-                driver->resetEncoders();  
-            }
-            else {
+            else if(!driver->has_front_wall()){
                 state = STRAIGHT;
                 driver->resetEncoders();
             }
-            finished_traverse_one_cell = false;*/
+            else{
+                state = UTURN;
+                driver->resetEncoders();
+                count = 0;
+            }
+            finished_traverse_one_cell = false;
         }
         
+        
         if (state == STRAIGHT) {
-            if (!driver->has_front_wall()) {
+            /*
+            if (!driver->has_front_wall() && !driver->has_front_wall()) {
                 driver->drive_forward();
             }
             else {
                 driver->stop();
-                wait(0.5);
-                led_1 = 1;
-                led_2 = 1;
-                led_3 = 0;
-                    led_4 = 0;
-                    finished_traverse_one_cell = true;
-                    continue;
+                wait(0.25);
+                flash_led (1, 1, 0, 0);
+                finished_traverse_one_cell = true;
+                continue;
+            } 
+            */
+            if (!driver->should_finish_drive_forward() && !driver->has_front_wall()) {
+                driver->drive_forward();
+                flash_led(1, 0, 0, 0);
+            }
+            else {
+                driver->clear_pid();
+                driver->stop();
+                driver->resetEncoders();
+                wait(0.2);
+                flash_led (0, 0, 0, 0);
+                finished_traverse_one_cell = true;
+                continue;
             }
         }
         if (state == RIGHT) {
             if (!driver->should_finish_turn_right()) {
                 driver->turn_right();
+                flash_led (0, 1, 0, 0);
             }
             else {
                 driver->stop();
-                wait(0.5);
-                led_1 = 0;
-                led_2 = 1;
-                led_3 = 1;
-                led_4 = 0;
-                finished_traverse_one_cell = true;
+                wait(0.25);
+                flash_led (0, 0, 0, 0);
+                //finished_traverse_one_cell = true;
+                state = STRAIGHT;
+                driver->resetEncoders();
+                driver->clear_pid();
                 continue;
             }
         }
         if (state == LEFT) {
             if (!driver->should_finish_turn_left()) {
                 driver->turn_left();
+                flash_led (0, 0, 1, 0);
+            }
+            else {
+                driver->stop();
+                wait(0.25);
+                flash_led (0, 0, 0, 0);
+                //finished_traverse_one_cell = true;
+                state = STRAIGHT;
+                driver->resetEncoders();
+                driver->clear_pid();
+                continue;
+            }
+        }
+        
+        if (state == UTURN){
+            if (!driver->should_finish_turn_left()) {
+                driver->turn_left();
+                flash_led (0, 1, 1, 0);
             }
             else {
                 driver->stop();
-                wait(0.5);
-                led_1 = 0;
-                led_2 = 0;
-                led_3 = 1;
-                led_4 = 1;
+                driver->resetEncoders();
+                wait(0.25);
+                flash_led (0, 0, 0, 0);
+                count == 1 ? finished_traverse_one_cell = true: count++;
+                continue;
+            }
+        } 
+    }
+}
+
+
+/*
+
+    if (!driver->has_front_wall()) {
+            if (!driver->should_finish_drive_forward()) {
+                driver->drive_forward();
+                flash_led(0, 1, 1, 0);
+            }
+            else {
+                driver->stop();
+                wait(1);
+                flash_led (0,0,0,0);
+                driver->resetEncoders();
                 finished_traverse_one_cell = true;
                 continue;
             }
-        } 
-        
-    }
-}
\ No newline at end of file
+        }
+        else {
+            driver->stop();
+            driver->resetEncoders();
+            flash_led (1, 1, 1, 0);
+        }
+
+
+
+
+*/
\ No newline at end of file