Diff: main.cpp

Revision:

5:5380953108bb

Parent:

4:0b22363eb57c

--- a/main.cpp	Fri Oct 16 17:22:40 2015 +0000
+++ b/main.cpp	Fri Oct 16 17:41:00 2015 +0000
@@ -6,11 +6,11 @@
 Serial pc(USBTX,USBRX);
 
 // functions
-//void find_keys();
 void check_floor();
 void get_floor();   // uses floor period to set cur_floor
 void get_period();
 void led_update();
+void update_q();
 
 // Threads
 void bpc_func(void const *args);
@@ -25,15 +25,6 @@
 InterruptIn IRSensor(p10);  // read IR sensor
 AnalogIn Button(p15);       // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number
 
-// keyboard read
-/*
-DigitalIn col1(p20);
-DigitalIn col2(p19);
-DigitalIn col3(p18);
-DigitalOut row1(p30);
-DigitalOut row2(p27);
-*/
-
 // LEDs for testing
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -53,12 +44,17 @@
 int count = 0;
 int test_readings = 0;  // need 4 consecutive readings to declare cur_floor
 int test_floor = 0;
+int el_q [4];
+int el_q_size = 0;
 
 int main() {
    
     // init interrupt handler
     IRSensor.fall(&get_period);
    
+    // init queue
+    for (int i=0;i<4;i++) el_q[i] = 0;
+   
     // set period (constant)
     dc_motor.period(period);
     servo1.period(period);
@@ -87,12 +83,10 @@
         //find_keys();
 
         // execute elevator alg
-        if (keyPressed) {
+        if (!el_q_size) {
 
-            led1 = 1;
-            led2 = 1;
-            led3 = 1;
-            led4 = 1;
+            desired_floor = el_q[0];
+            update_q();
 
             // check if need to move
             if (cur_floor != desired_floor) {
@@ -123,45 +117,13 @@
             // open door
             servo1.write(dty_servo_open);
             servo2.write(dty_servo_open);
-            keyPressed = 0;
+            wait(2);
+            //keyPressed = 0;
         }
     }
 
 }
 
-/*
-void find_keys() {
-
-    // check row 1
-    row1 = 0;
-    row2 = 1;
-    if (!col1) {
-        desired_floor = 1;
-        keyPressed = 1;
-    }
-    if (!col2) {
-        desired_floor = 2;
-        keyPressed = 1;
-    }
-    if (!col3) {
-        desired_floor = 3;
-        keyPressed = 1;
-    }
-   
-    // check row 2
-    row1 = 1;
-    row2 = 0;
-    if (!col1) {
-        desired_floor = 4;
-        keyPressed = 1;
-    }
-    if (!col2) {
-        desired_floor = 5;
-        keyPressed = 1;
-    }
-}
-*/
-
 void get_floor() {
     if(floor_period > 9900 && floor_period < 10100) {
         if (test_floor == 1) { 
@@ -257,24 +219,41 @@
         float ADC_val = Button.read(); 
         int val = ADC_val*10;
         if(val == 2) {
-            desired_floor = 1;
-            keyPressed = 1;
+            //desired_floor = 1;
+            //keyPressed = 1;
+            el_q[el_q_size] = 1;
+            el_q_size++;
         } else if(val == 4) {
-            desired_floor = 2;
-            keyPressed = 1;
+            //desired_floor = 2;
+            //keyPressed = 1;
+            el_q[el_q_size] = 2;
+            el_q_size++;
         } else if(val == 6) {
-            desired_floor = 3;
-            keyPressed = 1;
+            //desired_floor = 3;
+            //keyPressed = 1;
+            el_q[el_q_size] = 3;
+            el_q_size++;
         } else if(val == 8) {
-            desired_floor = 4;
-            keyPressed = 1;
+            //desired_floor = 4;
+            //keyPressed = 1;
+            el_q[el_q_size] = 4;
+            el_q_size++;
         } else if(val == 10) {
-            desired_floor = 5;
-            keyPressed = 1;
+            //desired_floor = 5;
+            //keyPressed = 1;
+            el_q[el_q_size] = 5;
+            el_q_size++;
         }
         wait(0.1);
-        while (keyPressed) { // wait for elevator to arrive at desired floor
-            wait(.2);
+        while (el_q_size == 4) { // wait for queue to empty
+            wait(0.2);
         }
     }
-}
\ No newline at end of file
+}
+
+void update_q() {
+    for (int i=1;i<el_q_size;i++) {
+        el_q[i-1] = el_q[i];
+    }
+    el_q_size--;    
+}