Project

Session6/Project/mbed_app.json

@@ -0,0 +1,8 @@
+{
+        "target_overrides": {
+            "*": {
+                "target.printf_lib": "std"
+            }
+        }
+
+}

Session6/Project/src/main.cpp

@@ -1,13 +1,15 @@
 #include "mbed.h"
 
-int MAX_SPEED = 120;
+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{
@@ -33,6 +35,7 @@ Thread sensing_thread;
 // Thread for Conrol
 Thread control_thread;
 
+
 void driver_led_feedback(int bitmask){
   driver_led = bitmask;
 }
@@ -54,13 +57,16 @@ void handle_sensing(){
     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 = 0.0;
+      sensor_msg->distance = the_distance;
 
     //And Send it
     message_queue.put(sensor_msg);
@@ -70,20 +76,64 @@ void handle_sensing(){
   }
 }
 
+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\n", the_msg->speed, the_msg->user_speed);
+
+    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)
@@ -94,17 +144,31 @@ void handle_control(){
     }
     else { //Speed OK
       queue.call(driver_led_feedback, 2);
-      int foo=0;
+    }
+
+    // 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);
+  }
 
-    //Release the queue
-
+  //And we are stopped, so Exit
+  queue.call(printf,"----- Break Success------\n");
+  queue.call(driver_led_feedback, 0);
 
-  }
+  //Reset Priority
 }
 
 
@@ -114,6 +178,7 @@ int main() {
 
   // 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);