File content as of revision 8:14e91fdf70e8:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

/*******************************************************************************
*   DEFENITIONS
*
*******************************************************************************/


#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; 
IR_sensors ir(p18, p19, p20, LED1, LED2, LED3); // controls three IR sensors
Car car(p21, p22, p23, p16, p15, p26, p25, p24, p13, p12);// controls two motors

float location = 0;



/*******************************************************************************
*   MOTOR FUNCTION
*
*******************************************************************************/

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);
}



/*******************************************************************************
*   STATE MACHINE
*
*******************************************************************************/

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;
                                if (DEBUG)
                                    printf("car_state::\t\t\tstate: Left\n\r");
                                break; 
           case centerleft :   turn = -SLOWTURNCW; // turns left slow
                                speed = SPEED / 2;
                                if (DEBUG)
                                    printf("car_state::\t\t\tstate: Center Left\n\r");
                                break;
            case center     :   turn = 0.0;         // doesn't turn
                                speed = SPEED;
                                if (DEBUG)
                                    printf("car_state::\t\t\tstate: Center\n\r");
                                break;
            case centerright:   turn =  SLOWTURNCW; // turns right slow
                                speed = SPEED / 2;
                                if (DEBUG)
                                    printf("car_state::\t\t\tstate: Center Right\n\r");
                                break;
            case right      :   turn =  FASTTURNCW; // turns right fast
                                speed = SPEED / 2;
                                if (DEBUG)
                                    printf("car_state::\t\t\tstate: Right\n\r");
                                break;
            default         :   turn =  0.0;        // MAX turn right
                                speed = 0.0;
                                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);
        prev_state = ir.state();
        wait(WAITSTATE * ADJ);
        if (switchstate != undef0)
            prev_state = ir.state();
    }
}


/*******************************************************************************
*   START THREADS
*
*******************************************************************************/


int main() {
    set_time(1256729737);
    time_t time_start = time(NULL);
    
    printf("\n\rPROGRAM STARTED\n\r");
    threadMotors.start(motors);
    threadState.start(car_state);
}