Diff: main.cpp

Revision:

7:d16faa6d7713

Parent:

5:63f5caea39bd

Child:

8:14e91fdf70e8

--- a/main.cpp	Mon Apr 08 03:37:54 2019 +0000
+++ b/main.cpp	Wed Apr 10 02:16:32 2019 +0000
@@ -3,12 +3,18 @@
  * SPDX-License-Identifier: Apache-2.0
  */
 
+/*******************************************************************************
+*   DEFENITIONS
+*
+*******************************************************************************/
+
+
 #include "mbed.h"       // mbed OS 5.11
 #include "Car.h"        // DC motor driver
 #include "IR_sensors.h" // ir sensors to track the line
 
-#define DEBUG 0     // enables the print statements
-#define ADJ (0.1)    // this is a time adjustment for bug in mbed OS 5.11
+#define DEBUG 0         // enables the print statements
+#define ADJ (0.1)       // this is a time adjustment for bug in mbed OS 5.11
 //#define ADJ (1.0/6)   // this is a time adjustment for bug in mbed OS 5.11
 //#define ADJ (1.0)     // this is a time adjustment for bug in mbed OS 5.11
 
@@ -16,8 +22,8 @@
 #define TOTALTIME     100.0
 #define STEP          0.1   // each motor step is half a second
 #define SPEED         1.0   // car speed
-#define SLOWTURNCW    -0.90  // speed to turn when the line still touching center
-#define FASTTURNCW    -1.00  // speed to turn when the line only touches side
+#define SLOWTURNCW    -0.90 // speed to turn when the line still touching center
+#define FASTTURNCW    -1.00 // speed to turn when the line only touches side
 #define WAITMOTOR     0.01
 #define WAITSTATE     0.05
 
@@ -33,6 +39,13 @@
 
 float location = 0;
 
+
+
+/*******************************************************************************
+*   MOTOR FUNCTION
+*
+*******************************************************************************/
+
 void motors() {
     //
     // It checks the turn and speed queues and takes repective action.
@@ -90,6 +103,13 @@
     car.speed(0.0);
 }
 
+
+
+/*******************************************************************************
+*   STATE MACHINE
+*
+*******************************************************************************/
+
 void car_state() {
     float prev_speed = 0.0;
     float prev_turn = 0.0;
@@ -106,37 +126,31 @@
         switch (switchstate) {
             case left       :   turn = -FASTTURNCW;// turns left fast
                                 speed = SPEED / 2;
-                                stateint = 0;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Left\n\r");
                                 break; 
            case centerleft :   turn = -SLOWTURNCW; // turns left slow
                                 speed = SPEED / 2;
-                                stateint = 1;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Center Left\n\r");
                                 break;
             case center     :   turn = 0.0;         // doesn't turn
                                 speed = SPEED;
-                                stateint = 2;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Center\n\r");
                                 break;
             case centerright:   turn =  SLOWTURNCW; // turns right slow
                                 speed = SPEED / 2;
-                                stateint = 3;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Center Right\n\r");
                                 break;
             case right      :   turn =  FASTTURNCW; // turns right fast
                                 speed = SPEED / 2;
-                                stateint = 4;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Right\n\r");
                                 break;
             default         :   turn =  0.0;        // MAX turn right
                                 speed = 0.0;
-                                stateint = 5;
                                 if (DEBUG)
                                     printf("car_state::\t\t\tstate: Default\n\r");
                                 break;
@@ -152,17 +166,20 @@
             prev_turn = turn;
         }
         motor_turn_queue.put(&turn);
-        //printf("car_state::Motor command: %.2f \n\r", turn);
-        if ( (stateint >= 0) and (stateint <= 4)){
-            prev_stateint = stateint;
+        prev_state = ir.state();
+        wait(WAITSTATE * ADJ);
+        if (switchstate != undef0)
             prev_state = ir.state();
-        }
-        //printf("previous state was %d" , prev_stateint);
-        wait(WAITSTATE * ADJ);
     }
 }
 
 
+/*******************************************************************************
+*   START THREADS
+*
+*******************************************************************************/
+
+
 int main() {
     printf("\n\rPROGRAM STARTED\n\r");
     threadMotors.start(motors);