File content as of revision 5:63f5caea39bd:

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

// 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() {
    //
    // 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;
                //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;
                //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);
}

void car_state() {
    float prev_speed = 0.0;
    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) {
        State switchstate = ir.state();
        if ((switchstate == undef0) and (prev_state != undef0))
            switchstate = prev_state;
        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;
        }
        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);
        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("\n\rPROGRAM STARTED\n\r");
    threadMotors.start(motors);
    threadState.start(car_state);
}