Diff: main.cpp

Revision:

1:2ceb12f88b5e

Parent:

0:7676da98b5c1

Child:

2:49fbdf1e7fb0

--- a/main.cpp	Tue Mar 26 03:27:14 2019 +0000
+++ b/main.cpp	Tue Mar 26 03:58:41 2019 +0000
@@ -11,7 +11,7 @@
 #define ADJ (1.0)     //this is a time adjustment for bug in mbed OS 5.11
 
 
-#define SPEED         0.001    // car speed
+#define SPEED         0.01    // 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
 
@@ -20,7 +20,8 @@
 Thread threadMotors = Thread(osPriorityRealtime4, OS_STACK_SIZE, NULL, NULL);
 Thread threadState = Thread(osPriorityRealtime1, OS_STACK_SIZE, NULL, NULL);
 
-Queue<float,4> motor_queue;  // queue for motor turn commands; 
+Queue<float,4> motor_turn_queue;  // queue for motor turn commands; 
+Queue<float,4> motor_speed_queue;  // queue for motor turn commands; 
 Car car(p21, p22, p23,p26, p25, p24);           // controls two motors
 IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
 
@@ -28,48 +29,64 @@
 
 void motors() {
     while (true) {
-        printf("Checking Motors.. \n\r");
+        printf("motors::Checking Motors.. \n\r");
         location += car.speed();
         car.speed(SPEED);
-        if (not motor_queue.empty()){
-            osEvent evt = motor_queue.get();
+        if (not motor_turn_queue.empty()){
+            osEvent evt = motor_turn_queue.get();
             if (evt.status == osEventMessage){
                 float *cmd = (float*)evt.value.p;
-                printf("Motor turning: %.2f \n\r", *cmd);
+                printf("motors::Motor turning: %.2f \n\r", *cmd);
                 car.turnCW(*cmd);
             }
         }
+        if (not motor_speed_queue.empty()){
+            osEvent evt = motor_speed_queue.get();
+            if (evt.status == osEventMessage){
+                float *cmd = (float*)evt.value.p;
+                printf("motors::Motor speeding: %.2f \n\r", *cmd);
+                car.speed(*cmd);
+            }
+        }
         wait(0.20 * ADJ);
     }
 }
 
 void car_state() {
-    State prevState = ir.state();
+    State prev_state = ir.state();
+    float prev_speed = 0.0;
     while (true) {
         float turn;
+        float speed = SPEED;
         switch (ir.state()) {
             case left       :   turn = -FASTTURNCW;// turns left fast
-                                printf("\t\t\tstate: Left\n\r");
+                                printf("car_state::\t\t\tstate: Left\n\r");
                                 break; 
             case centerleft :   turn = -SLOWTURNCW; // turns left slow
-                                printf("\t\t\tstate: Center Left\n\r");
+                                printf("car_state::\t\t\tstate: Center Left\n\r");
                                 break;
             case center     :   turn = 0.0;         // doesn't turn
-                                printf("\t\t\tstate: Center\n\r");
+                                printf("car_state::\t\t\tstate: Center\n\r");
                                 break;
             case centerright:   turn =  SLOWTURNCW; // turns right slow
-                                printf("\t\t\tstate: Center Right\n\r");
+                                printf("car_state::\t\t\tstate: Center Right\n\r");
                                 break;
             case right      :   turn =  FASTTURNCW; // turns right fast
-                                printf("\t\t\tstate: Right\n\r");
+                                printf("car_state::\t\t\tstate: Right\n\r");
                                 break;
             default         :   turn =  0.0;        // MAX turn right
-                                printf("\t\t\tstate: Default\n\r");
+                                speed = 0.0;
+                                printf("car_state::\t\t\tstate: Default\n\r");
+                                printf("car_state::Motor STOP command\n\r");
                                 break;
         }
-        motor_queue.put(&turn);
-        printf("Motor command: %.2f \n\r", turn);
-        prevState = ir.state();
+        if (prev_speed != speed) {
+            motor_speed_queue.put(&speed);
+            prev_speed = speed;
+        }
+        motor_turn_queue.put(&turn);
+        printf("car_state::Motor command: %.2f \n\r", turn);
+        prev_state = ir.state();
         wait(1.0 * ADJ);
     }
 }