Diff: main.cpp

Revision:

0:7676da98b5c1

Child:

1:2ceb12f88b5e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 26 03:27:14 2019 +0000
@@ -0,0 +1,81 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * 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
+
+//#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.001    // 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
+
+// the motors have the highest priority; 
+// this will be use to "track" its position
+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; 
+Car car(p21, p22, p23,p26, p25, p24);           // controls two motors
+IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
+
+float location = 0;
+
+void motors() {
+    while (true) {
+        printf("Checking Motors.. \n\r");
+        location += car.speed();
+        car.speed(SPEED);
+        if (not motor_queue.empty()){
+            osEvent evt = motor_queue.get();
+            if (evt.status == osEventMessage){
+                float *cmd = (float*)evt.value.p;
+                printf("Motor turning: %.2f \n\r", *cmd);
+                car.turnCW(*cmd);
+            }
+        }
+        wait(0.20 * ADJ);
+    }
+}
+
+void car_state() {
+    State prevState = ir.state();
+    while (true) {
+        float turn;
+        switch (ir.state()) {
+            case left       :   turn = -FASTTURNCW;// turns left fast
+                                printf("\t\t\tstate: Left\n\r");
+                                break; 
+            case centerleft :   turn = -SLOWTURNCW; // turns left slow
+                                printf("\t\t\tstate: Center Left\n\r");
+                                break;
+            case center     :   turn = 0.0;         // doesn't turn
+                                printf("\t\t\tstate: Center\n\r");
+                                break;
+            case centerright:   turn =  SLOWTURNCW; // turns right slow
+                                printf("\t\t\tstate: Center Right\n\r");
+                                break;
+            case right      :   turn =  FASTTURNCW; // turns right fast
+                                printf("\t\t\tstate: Right\n\r");
+                                break;
+            default         :   turn =  0.0;        // MAX turn right
+                                printf("\t\t\tstate: Default\n\r");
+                                break;
+        }
+        motor_queue.put(&turn);
+        printf("Motor command: %.2f \n\r", turn);
+        prevState = ir.state();
+        wait(1.0 * ADJ);
+    }
+}
+ 
+int main() {
+printf("PROGRAM STARTED");
+   threadMotors.start(motors);
+   threadState.start(car_state);
+}