main.cpp

#include "mbed.h"

int MAX_SPEED = 120;

//Lets have LED blink to show its working
DigitalOut statusLED(LED1);

// Use Analog 0 to represent speed
AnalogIn speedSensor(A0);
AnalogIn selectSensor(A1);

// Struct to hold messages
typedef struct{
    int speed;
    int user_speed;
    float distance;
    } sensing_t;

// Our Message Box
Mail<sensing_t, 4> message_queue;

// For Speed Output
BusOut driver_led(D5,D7,D10);


// Create a queue that can hold a maximum of 32 events
EventQueue queue(32 * EVENTS_EVENT_SIZE);

// Threads
Thread event_thread;  //Needs to be global so all elements can post to it
//Thread for sensing
Thread sensing_thread; 
// Thread for Conrol
Thread control_thread;

void driver_led_feedback(int bitmask){
  driver_led = bitmask;
}


void handle_sensing(){
  int speed = 0;
  int selected_speed = 0;

  while (true){
    //Read from our speed potentiometer Remeber Value here is 0-1
    float speed_pot = speedSensor.read();
    // Convert to speed range
    int speed = round(speed_pot * MAX_SPEED);

    float select_pot = selectSensor.read();
    int selected_speed = round(select_pot * MAX_SPEED);
    //And Print It for the moment
    queue.call(printf, "Current Speed is %d\n", speed);
    queue.call(printf, "Selected Speed is %d\n", selected_speed);

    // Ceate a sensor message
    if (!message_queue.full()){
      //Block if there is no space
      sensing_t *sensor_msg = message_queue.try_alloc_for(Kernel::wait_for_u32_forever);
      sensor_msg->speed = speed;
      sensor_msg->user_speed = selected_speed;
      sensor_msg->distance = 0.0;

    //And Send it
    message_queue.put(sensor_msg);
    }

    ThisThread::sleep_for(1000ms);
  }
}

void handle_control(){
  int speed_delta;

  while (true){
    // Get the message in blocking mode
    queue.call(printf, "Read from Queue\n");
    sensing_t *the_msg = message_queue.try_get_for(Kernel::wait_for_u32_forever);
    queue.call(printf, "Read from Queue Complete\n");  

    queue.call(printf, "HANDLE:  Speed %d User %d\n", the_msg->speed, the_msg->user_speed);
    // Do the processing
    speed_delta = the_msg->user_speed - the_msg->speed;
    queue.call(printf,"--> Speed Delta is %d\n", speed_delta);

    if (speed_delta > 5){  //Low Speed
      queue.call(printf,"--> speed INCREASE needed\n");
      queue.call(driver_led_feedback, 1); //Bitmask 001 (But Endianess)
    }
    else if (speed_delta < -5){ //High Speed
      queue.call(printf,"--> speed DECREASE needed\n");
      queue.call(driver_led_feedback, 4); 
    }
    else { //Speed OK
      queue.call(driver_led_feedback, 2);
      int foo=0;
    }

    message_queue.free(the_msg); //Important, Free space in the queue
    // For the moment have a break to avoid thrashig processor
    ThisThread::sleep_for(1000ms);

    //Release the queue
    

  }
}


int main() {

  // put your setup code here, to run once:

  // Start the Event Thread
  event_thread.start(callback(&queue, &EventQueue::dispatch_forever));
  sensing_thread.start(handle_sensing);
  control_thread.start(handle_control);

  while(1) {
    // put your main code here, to run repeatedly:


    // Blink to let us know things are working
    statusLED = !statusLED;

    // Temporary Print Callbak so we we know event queue is working
    queue.call(printf, "Tick\n"); 
    ThisThread::sleep_for(5000ms);
  }
}