Diff: main.cpp

Revision:

2:d7103c7e0671

Parent:

1:e79ac0826624

--- a/main.cpp	Fri Oct 16 16:41:38 2015 +0000
+++ b/main.cpp	Fri Oct 16 16:54:34 2015 +0000
@@ -1,6 +1,7 @@
 // ESE 519 Lab 3 Code //
 
 #include "mbed.h"
+#include "rtos.h"
 
 Serial pc(USBTX,USBRX);
 
@@ -79,7 +80,7 @@
         while (!cur_floor) { // wait for a floor to be detected
             wait(0.25);
         }
-        desired_floor = cur_floor;
+        if (!desired_floor) desired_floor = cur_floor;
         led_update();
        
         // check for key press
@@ -88,6 +89,11 @@
         // execute elevator alg
         if (keyPressed) {
 
+            led1 = 1;
+            led2 = 1;
+            led3 = 1;
+            led4 = 1;
+
             // check if need to move
             if (cur_floor != desired_floor) {
 
@@ -265,6 +271,8 @@
             keyPressed = 1;
         }
         wait(0.1);
-        while (keyPressed); // wait for elevator to arrive at desired floor
+        while (keyPressed) { // wait for elevator to arrive at desired floor
+            wait(.2);
+        }
     }
 }
\ No newline at end of file