Diff: main.cpp

Revision:

5:63f5caea39bd

Parent:

4:d8dc8544fe81

Child:

7:d16faa6d7713

--- a/main.cpp	Wed Mar 27 04:20:42 2019 +0000
+++ b/main.cpp	Thu Mar 28 16:10:50 2019 +0000
@@ -3,18 +3,21 @@
  * SPDX-License-Identifier: Apache-2.0
  */
 
-#include "mbed.h"           // mbed OS 5.11
-#include "Car.h"            // DC motor driver
-#include "IR_sensors.h"     // ir sensors to track the line
+#include "mbed.h"       // mbed OS 5.11
+#include "Car.h"        // DC motor driver
+#include "IR_sensors.h" // ir sensors to track the line
 
-#define ADJ (1.0/12)     //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
+#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
 
 
-#define SPEED         0.1    // car speed
-#define SLOWTURNCW    0.10    // speed to turn when the line still touching center
-#define FASTTURNCW    0.50    // speed to turn when the line only touches side
+#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 WAITMOTOR     0.01
 #define WAITSTATE     0.05
 
@@ -31,77 +34,137 @@
 float location = 0;
 
 void motors() {
-    car.speed(0.0);
-    float t = 0.0;
-    while (t < 3.0) {
-        t += WAITMOTOR;
-        //printf("motors::Checking Motors.. \t\tTime: %.2f \n\r", t);
+    //
+    // It checks the turn and speed queues and takes repective action.
+    //
+    car.speed(0.0); 
+    float total_time = 0.0;
+    float step_time = 0.0;  // this will be to control the DC motor step
+    int step_time_on = 0;
+    float current_speed = 0.0;
+    while (total_time < TOTALTIME) {
+        total_time += WAITMOTOR;
+        //if (DEBUG)
+        //    printf("motors::Checking Motors.. \t\tTime: %.2f \n\r", total_time);
         location += car.speed();
+        
+        // TURN ROUTINE
         if (not motor_turn_queue.empty()){
             osEvent evt = motor_turn_queue.get();
             if (evt.status == osEventMessage){
                 float *cmd = (float*)evt.value.p;
-                //printf("motors::Motor turning: %.2f \n\r", *cmd);
+                //if (DEBUG)
+                //    printf("motors::Motor turning: %.2f \n\r", *cmd);
                 car.turnCW(*cmd);
             }
         }
+        
+        // SPEED ROUTINE
         if (not motor_speed_queue.empty()){
             osEvent evt = motor_speed_queue.get();
+            step_time = total_time + STEP;
             if (evt.status == osEventMessage){
                 float *cmd = (float*)evt.value.p;
-                //printf("motors::Motor speeding: %.2f \n\r", *cmd);
+                //if (DEBUG)
+                //    printf("motors::Motor speeding: %.2f \n\r", *cmd);
+                current_speed = *cmd;
                 car.speed(*cmd);
             }
         }
+        
+        if (total_time > step_time) {
+            step_time = total_time + STEP;
+            step_time_on = ~ step_time_on;
+            //if (DEBUG)
+            //    printf("\n\rmotor stop\n\r");
+            if (step_time_on)
+                car.speed(current_speed);
+            else 
+                car.speed(0.0);
+        }
         wait(WAITMOTOR * ADJ);
     }
+    if (DEBUG)
         printf("motor is off");
-        wait(WAITMOTOR * ADJ);
-        car.speed(0.0);
+    wait(WAITMOTOR * ADJ);
+    car.speed(0.0);
 }
 
 void car_state() {
-    State prev_state = ir.state();
     float prev_speed = 0.0;
-    wait(3* ADJ);
+    float prev_turn = 0.0;
+    float speed = SPEED;
+    float turn;
+    int stateint = -1;
+    int prev_stateint = -1;
+    State prev_state = undef0;
+    wait(3* ADJ);   // wait before you move
     while (true) {
-        float turn;
-        float speed = SPEED;
-        switch (ir.state()) {
+        State switchstate = ir.state();
+        if ((switchstate == undef0) and (prev_state != undef0))
+            switchstate = prev_state;
+        switch (switchstate) {
             case left       :   turn = -FASTTURNCW;// turns left fast
-                                printf("car_state::\t\t\tstate: Left\n\r");
+                                speed = SPEED / 2;
+                                stateint = 0;
+                                if (DEBUG)
+                                    printf("car_state::\t\t\tstate: Left\n\r");
                                 break; 
-            case centerleft :   turn = -SLOWTURNCW; // turns left slow
-                                printf("car_state::\t\t\tstate: Center Left\n\r");
+           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
-                                printf("car_state::\t\t\tstate: Center\n\r");
+                                speed = SPEED;
+                                stateint = 2;
+                                if (DEBUG)
+                                    printf("car_state::\t\t\tstate: Center\n\r");
                                 break;
             case centerright:   turn =  SLOWTURNCW; // turns right slow
-                                printf("car_state::\t\t\tstate: Center Right\n\r");
+                                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
-                                printf("car_state::\t\t\tstate: Right\n\r");
+                                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;
-                                printf("car_state::\t\t\tstate: Default\n\r");
-                                printf("car_state::Motor STOP command\n\r");
+                                stateint = 5;
+                                if (DEBUG)
+                                    printf("car_state::\t\t\tstate: Default\n\r");
                                 break;
         }
-        if (prev_speed != speed) {
+        if ((prev_speed != speed) or (prev_turn != turn)) {
+            printf("car_state::\t\tchanging speed: Default\n\r");
+            float temp = 0.0;
+            motor_speed_queue.put(&temp);
+            wait(10 * WAITSTATE * ADJ);
+            printf("car_state::\t\tchanging speed: Default\n\r");
             motor_speed_queue.put(&speed);
             prev_speed = speed;
+            prev_turn = turn;
         }
         motor_turn_queue.put(&turn);
         //printf("car_state::Motor command: %.2f \n\r", turn);
-        prev_state = ir.state();
+        if ( (stateint >= 0) and (stateint <= 4)){
+            prev_stateint = stateint;
+            prev_state = ir.state();
+        }
+        //printf("previous state was %d" , prev_stateint);
         wait(WAITSTATE * ADJ);
     }
 }
- 
+
+
 int main() {
-    printf("PROGRAM STARTED");
+    printf("\n\rPROGRAM STARTED\n\r");
     threadMotors.start(motors);
     threadState.start(car_state);
 }