Diff: elevator.cpp

Revision:

8:dd34a528a47c

Parent:

7:9ff7df122ff6

Child:

9:95cbc05bd81d

--- a/elevator.cpp	Tue Oct 13 20:41:36 2015 +0000
+++ b/elevator.cpp	Mon Oct 19 19:42:36 2015 +0000
@@ -1,5 +1,8 @@
 #include "mbed.h"
+#include "LinkedList.h"
 
+int serving;
+LinkedList<node>list;
 Serial pc(USBTX,USBRX);
 PwmOut dc_motor(p23);
 PwmOut servo1(p21);
@@ -31,7 +34,29 @@
 int requested_floor1, requested_floor2, requested_floor;
 int busy = 0;
 int currentfloor;
+int globedir = 2;
 
+
+int inqueue(int request){
+    int i;
+    int exist = 0;
+    node *tmp;
+    for (i = 0; i<= list.length();i++){
+        tmp = list.pop(i);
+        if (request == (int) tmp->data)
+            exist = 1;
+            }
+    return exist;
+}
+
+int dir(int current, int next){
+    int direction;
+    if (current > next)
+        direction = 1;
+    if (current <= next)
+        direction = 0;
+return direction;
+}
 void serve(){
         
     if(busy == 1){ 
@@ -47,7 +72,7 @@
             once++;
             frequency = 1./time1.read();
 
-            if (once == 1000){
+            if (once == 50){
                 once = 0;
                 
                 if (frequency >= 900.0f && frequency <= 1100.0f){currentfloor = 5;}
@@ -56,7 +81,7 @@
                 else if (frequency >= 200.0f && frequency <= 300.0f){currentfloor = 2;}
                 else if (frequency >= 50.0f && frequency <= 150.0f){currentfloor = 1;}
         
-                if (currentfloor == requested_floor){
+                if (currentfloor == serving){
                     pc.printf("served:   %d floor frequency:  %f \n",currentfloor,frequency);
             
                     dc_motor.write(0.0f);
@@ -69,7 +94,7 @@
                     busy = 0;
                     servo1.write(open);
                     servo2.write(open2); 
-                    location = requested_floor;
+                    location = serving;
                     pc.printf("at:   %d \n",location);
                     sensor.fall(0);
                     }
@@ -81,14 +106,14 @@
     }
     
 void moveUp(){
-    dc_motor.write(1.0f);
+    dc_motor.write(0.5f);
     in1 = 1;
     in2 = 0;
     }
     
 void moveDown(){
     
-    dc_motor.write(1.0f);
+    dc_motor.write(0.5f);
     in1 = 0;
     in2 = 1;
     }
@@ -105,26 +130,69 @@
 }
 
 void call(int next) {
-    if (busy == 0 && next > 0.5){
-        requested_floor = next;
-        if (location > next){
-            pc.printf("serving:   %d \n",next);
+    node *tmp;
+    int i;
+    int assigned = 0;
+    if (list.length() < 4 && next > 0.5){
+        list.append((int *)next);
+        }
+        
+    if (busy == 0){
+        
+        if (globedir == 2){
+                    tmp = list.pop(0);
+                    serving = (int)tmp->data;
+                    list.remove(0);
+                    }
+                    
+        else if (globedir ==1){
+            for (i = 0; i<= list.length();i++){
+                tmp = list.pop(i);
+                if (dir(location,(int) tmp->data) == 1){
+                    serving = (int)tmp->data;
+                    list.remove(i);
+                    assigned = 1;
+                    break;
+                }
+                }
+            }
+            
+        else if (globedir == 0){
+            for (i = 0; i<= list.length();i++){
+                tmp = list.pop(i);
+            if (dir(location,(int) tmp->data) == 0){
+                serving = (int)tmp->data;
+                list.remove(i);
+                assigned = 1;
+                break;
+            }
+        }        
+            }
+        if (assigned != 1){
+            tmp = list.pop(0);
+            serving = (int)tmp->data;
+            }
+            
+            
+        if (location > serving){
+            pc.printf("serving:   %d \n",requested_floor);
             closeDoor();
             moveDown();
             busy = 1;
             sensor.fall(&serve);
             }
             
-        else if (location < next ){
-            pc.printf("serving:   %d \n",next);
+        else if (location < serving ){
+            pc.printf("serving:   %d \n",requested_floor);
             closeDoor();
             moveUp();
             busy = 1;
             sensor.fall(&serve);
             }
+            
     }
-}
-
+    }
+    
 int main() {
     servo1.period(0.02f);
     servo2.period(0.02f);
@@ -132,11 +200,11 @@
     in2.output();
     in2 = 0;
     in1 = 0; 
-    location = 1;
+    location = 2;
 
     while(1){
         
-        while(busy == 0){
+        while(list.length() < 4){
         
             if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor1 = 1;}      //1
             else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor1 = 2;}//2
@@ -154,8 +222,9 @@
             
             if (requested_floor1 != requested_floor2) {requested_floor = 0;}
             else{requested_floor = requested_floor1;}
-            
-            call(requested_floor); 
+            if (inqueue(requested_floor) == 0 && requested_floor != 0){
+                call(requested_floor); 
+                }
 }
 }
 }
\ No newline at end of file