main.cpp

#include "mbed.h"

int MAX_SPEED = 200;
int MAX_DISTANCE = 100;

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

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

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

    float distance_pot = distanceSensor.read();
    float the_distance = distance_pot * MAX_DISTANCE;

    // 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 = the_distance;

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

    ThisThread::sleep_for(1000ms);
  }
}

void apply_brakes(){
  /* Fake brake applicaton*/

  // Get Current Speed (it would be better to get this from elsewhre)
  queue.call(printf,"----- EMERGENY BRAKE APPLIED ------\n");
  float speed_pot = speedSensor.read();
  // Convert to speed
  int current_speed = speed_pot * MAX_SPEED;
  int ledState = 0; // Helper for LEDS
  // While we are still moving
  while(current_speed > 0){
    //Simulate breaking 
    current_speed -= 1;

    // Blink LEDS
    if (ledState == 0){
      queue.call(driver_led_feedback, 7);
      ledState = 1;
    }
    else {
      queue.call(driver_led_feedback, 0);
      ledState = 0;
    }

    ThisThread::sleep_for(100ms); //Sleep for a Moment
    
  }
}

void handle_control(){
  int speed_delta;
  float last_distance = -1;
  float distance_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 Distance %0.2f\n", the_msg->speed, the_msg->user_speed, the_msg->distance);

    if(last_distance == -1){
      last_distance= the_msg->distance;
    }

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

    // Calculate distance delta
    distance_delta = the_msg->distance - last_distance;
    // Updte last istance
    last_distance = the_msg->distance;

    queue.call(printf,"--> Distance Delta is %f\n", distance_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);
    }

    // TRivial Call for Distance
    if (the_msg->distance < 50){
        //Chek if we are already breaking
          queue.call(printf,"--> Contoller Apply Brakes: Start\n");
          // Thread for Brake
          Thread braking_thread;
          braking_thread.start(apply_brakes);
          braking_thread.set_priority(osPriorityHigh);
          braking_thread.join(); //Wait for breaking to stop
          queue.call(printf,"--> Contoller Apply Brakes: Done\n");
        
    }

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

  //And we are stopped, so Exit
  queue.call(printf,"----- Break Success------\n");
  queue.call(driver_led_feedback, 0);

  //Reset Priority
}


int main() {

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

  // Start the Event Thread
  event_thread.start(callback(&queue, &EventQueue::dispatch_forever));
  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);
  }
}