Diff: main.cpp

Revision:

7:f4814b9756fc

Parent:

6:c6de60c953d2

Child:

8:4b0f6f68db12

--- a/main.cpp	Fri Oct 16 18:48:07 2015 +0000
+++ b/main.cpp	Fri Oct 16 19:39:08 2015 +0000
@@ -12,7 +12,7 @@
 void led_update();
 void update_q();
 void el_alg();  // need sorted list
-void bubbleSort();  // sorts list
+void bubble_sort();
 
 // Threads
 void bpc_func(void const *args);
@@ -48,7 +48,7 @@
 int test_floor = 0;
 int el_q [4];
 int el_q_size = 0;
-char cur_direction = 'S';   // S = stationary, U = up, D = down
+char cur_dir = 'S';   // S = stationary, U = up, D = down
 
 // mutex
 Mutex q_mutex; // protect el_q
@@ -91,10 +91,12 @@
         // 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) {
@@ -103,11 +105,11 @@
                 if (cur_floor > desired_floor) {    // move down
                     in1 = 1;
                     in2 = 0;
-                    cur_direction = 'U';
+                    cur_dir = 'U';
                 } else {                            // move up
                     in1 = 0;
                     in2 = 1;
-                    cur_direction = 'D';
+                    cur_dir = 'D';
                 }
 
                 // start car
@@ -127,7 +129,12 @@
             // open door
             servo1.write(dty_servo_open);
             servo2.write(dty_servo_open);
-            wait(2);
+            
+            // update queue
+            q_mutex.lock();
+            update_q();
+            q_mutex.unlock();
+            wait(1);
             //keyPressed = 0;
         }
     }
@@ -232,23 +239,23 @@
         if(val == 2) {
             el_q[el_q_size] = 1;
             el_q_size++;
-            bubbleSort();
+            bubble_sort();
         } else if(val == 4) {
             el_q[el_q_size] = 2;
             el_q_size++;
-            bubbleSort();
+            bubble_sort();
         } else if(val == 6) {
             el_q[el_q_size] = 3;
             el_q_size++;
-            bubbleSort();
+            bubble_sort();
         } else if(val == 8) {
             el_q[el_q_size] = 4;
             el_q_size++;
-            bubbleSort();
+            bubble_sort();
         } else if(val == 10) {
             el_q[el_q_size] = 5;
             el_q_size++;
-            bubbleSort();
+            bubble_sort();
         }
         q_mutex.unlock();
         wait(0.1);
@@ -266,53 +273,53 @@
 }
 
 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) { 
+    int n = el_q_size;
+    if (n>1) {
+        int index;
+        int temp;
+        for (int i=0;i<n;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;
+        if (cur_dir == 'U') {
+            for (int i=index;i<n;i++) {
+                temp = el_q[i-index];
+                el_q[i-index] = el_q[i];
+                el_q[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;
+        if (cur_dir == 'D') {
+            for (int i=0;i<index-1;i++) {
+                temp = el_q[i];
+                el_q[i] = el_q[index-i-1];
+                el_q[index-i-1] = temp;
             }
         }
     }
+    desired_floor = el_q[0];
 }
+
+
+void bubble_sort() {
+    int n = el_q_size;
+    bool sorted = false;
+    while (!sorted) {
+        sorted = true;
+        for (int i=0;i<n-1;i++) {
+            for (int j=i;j<n;j++) {
+                if (el_q[i] > el_q[j]) {
+                    int temp = el_q[j];
+                    el_q[j] = el_q[i];
+                    el_q[i] = temp;
+                    sorted = false;
+                }
+            }
+        }
+    }
+    el_alg();
+}
+
+
+