Diff: main.cpp

Revision:

6:c6de60c953d2

Parent:

5:5380953108bb

Child:

7:f4814b9756fc

--- a/main.cpp	Fri Oct 16 17:41:00 2015 +0000
+++ b/main.cpp	Fri Oct 16 18:48:07 2015 +0000
@@ -11,6 +11,8 @@
 void get_period();
 void led_update();
 void update_q();
+void el_alg();  // need sorted list
+void bubbleSort();  // sorts list
 
 // Threads
 void bpc_func(void const *args);
@@ -46,6 +48,10 @@
 int test_floor = 0;
 int el_q [4];
 int el_q_size = 0;
+char cur_direction = 'S';   // S = stationary, U = up, D = down
+
+// mutex
+Mutex q_mutex; // protect el_q
 
 int main() {
    
@@ -85,8 +91,10 @@
         // execute elevator alg
         if (!el_q_size) {
 
+            q_mutex.lock();
             desired_floor = el_q[0];
             update_q();
+            q_mutex.unlock();
 
             // check if need to move
             if (cur_floor != desired_floor) {
@@ -95,9 +103,11 @@
                 if (cur_floor > desired_floor) {    // move down
                     in1 = 1;
                     in2 = 0;
+                    cur_direction = 'U';
                 } else {                            // move up
                     in1 = 0;
                     in2 = 1;
+                    cur_direction = 'D';
                 }
 
                 // start car
@@ -218,32 +228,29 @@
     while (1) {
         float ADC_val = Button.read(); 
         int val = ADC_val*10;
+        q_mutex.lock();
         if(val == 2) {
-            //desired_floor = 1;
-            //keyPressed = 1;
             el_q[el_q_size] = 1;
             el_q_size++;
+            bubbleSort();
         } else if(val == 4) {
-            //desired_floor = 2;
-            //keyPressed = 1;
             el_q[el_q_size] = 2;
             el_q_size++;
+            bubbleSort();
         } else if(val == 6) {
-            //desired_floor = 3;
-            //keyPressed = 1;
             el_q[el_q_size] = 3;
             el_q_size++;
+            bubbleSort();
         } else if(val == 8) {
-            //desired_floor = 4;
-            //keyPressed = 1;
             el_q[el_q_size] = 4;
             el_q_size++;
+            bubbleSort();
         } else if(val == 10) {
-            //desired_floor = 5;
-            //keyPressed = 1;
             el_q[el_q_size] = 5;
             el_q_size++;
+            bubbleSort();
         }
+        q_mutex.unlock();
         wait(0.1);
         while (el_q_size == 4) { // wait for queue to empty
             wait(0.2);
@@ -257,3 +264,55 @@
     }
     el_q_size--;    
 }
+
+void el_alg() {
+    int index = -1;
+    if (cur_direction == 'U') {
+        for (int i=0; i<el_q_size;i++) {
+            if (el_q[i] > cur_floor) { 
+                index = i;
+                break;
+            }
+        }
+        if (index != -1) {
+            for (int i=index; i<el_q_size; i++) {
+                int temp = el_q[i-index];
+                el_q[i-index] = el_q[i];
+                el_q[i] = temp;
+            }
+        }
+    }
+    else if (cur_direction == 'D') {
+        for (int i=0;i<el_q_size;i++) {
+            if (el_q[i] > cur_floor) { 
+                index = i;
+                break;
+            }
+        }
+        if (index != -1) {
+            for (int i=0; i<index; i--) {
+                int temp = el_q[i];
+                el_q[i] = el_q[index-1-i];
+                el_q[index-1-i] = temp;
+            }
+        }
+    }     
+}
+
+void bubbleSort() {
+    bool swapped = true;
+    int j = 0;
+    int tmp;
+    while (swapped) {
+        swapped = false;
+        j++;
+        for (int i = 0; i < el_q_size - j; i++) {
+            if (el_q[i] > el_q[i + 1]) {
+                tmp = el_q[i];
+                el_q[i] = el_q[i + 1];
+                el_q[i + 1] = tmp;
+                swapped = true;
+            }
+        }
+    }
+}