File content as of revision 4:d8dc8544fe81:

/* 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/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 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 WAITMOTOR     0.01
#define WAITSTATE     0.05

// 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_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

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);
        location += car.speed();
        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);
                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(WAITMOTOR * ADJ);
    }
        printf("motor is off");
        wait(WAITMOTOR * ADJ);
        car.speed(0.0);
}

void car_state() {
    State prev_state = ir.state();
    float prev_speed = 0.0;
    wait(3* ADJ);
    while (true) {
        float turn;
        float speed = SPEED;
        switch (ir.state()) {
            case left       :   turn = -FASTTURNCW;// turns left fast
                                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");
                                break;
            case center     :   turn = 0.0;         // doesn't turn
                                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");
                                break;
            case right      :   turn =  FASTTURNCW; // turns right fast
                                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");
                                break;
        }
        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(WAITSTATE * ADJ);
    }
}
 
int main() {
    printf("PROGRAM STARTED");
    threadMotors.start(motors);
    threadState.start(car_state);
}