Diff: elevator.cpp

Revision:

9:95cbc05bd81d

Parent:

8:dd34a528a47c

--- a/elevator.cpp	Mon Oct 19 19:42:36 2015 +0000
+++ b/elevator.cpp	Tue Oct 20 19:29:47 2015 +0000
@@ -41,7 +41,7 @@
     int i;
     int exist = 0;
     node *tmp;
-    for (i = 0; i<= list.length();i++){
+    for (i = 1; i<= list.length();i++){
         tmp = list.pop(i);
         if (request == (int) tmp->data)
             exist = 1;
@@ -52,9 +52,9 @@
 int dir(int current, int next){
     int direction;
     if (current > next)
-        direction = 1;
+        direction = 0;
     if (current <= next)
-        direction = 0;
+        direction = 1;
 return direction;
 }
 void serve(){
@@ -93,7 +93,8 @@
                     in2 = 0;
                     busy = 0;
                     servo1.write(open);
-                    servo2.write(open2); 
+                    servo2.write(open2);
+                    wait(1); 
                     location = serving;
                     pc.printf("at:   %d \n",location);
                     sensor.fall(0);
@@ -133,20 +134,25 @@
     node *tmp;
     int i;
     int assigned = 0;
-    if (list.length() < 4 && next > 0.5){
+    
+    if (list.length() < 4 && next > 0 && inqueue(next) == 0){
         list.append((int *)next);
         }
-        
-    if (busy == 0){
+    for (i = 0; i<= list.length();i++){
+        tmp = list.pop(i);    
+        pc.printf(" %d \n",(int)tmp->data);
+        }       
+    if (busy == 0 && (int)list.pop(1)->data < 6){
         
         if (globedir == 2){
-                    tmp = list.pop(0);
+                    tmp = list.pop(1);
                     serving = (int)tmp->data;
-                    list.remove(0);
+                    list.remove(1);
+                    assigned = 1;
                     }
                     
-        else if (globedir ==1){
-            for (i = 0; i<= list.length();i++){
+        else if (globedir == 1){
+            for (i = 1; i<= list.length();i++){
                 tmp = list.pop(i);
                 if (dir(location,(int) tmp->data) == 1){
                     serving = (int)tmp->data;
@@ -158,24 +164,26 @@
             }
             
         else if (globedir == 0){
-            for (i = 0; i<= list.length();i++){
+            for (i = 1; 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 (dir(location,(int) tmp->data) == 0){
+                    serving = (int)tmp->data;
+                    list.remove(i);
+                    assigned = 1;
+                    break;
             }
         }        
             }
         if (assigned != 1){
-            tmp = list.pop(0);
+            tmp = list.pop(1);
             serving = (int)tmp->data;
+            list.remove(1);
             }
             
             
         if (location > serving){
-            pc.printf("serving:   %d \n",requested_floor);
+            pc.printf("serving:   %d \n",serving);
+            globedir = 0;
             closeDoor();
             moveDown();
             busy = 1;
@@ -183,7 +191,8 @@
             }
             
         else if (location < serving ){
-            pc.printf("serving:   %d \n",requested_floor);
+            pc.printf("serving:   %d \n",serving);
+            globedir = 1;
             closeDoor();
             moveUp();
             busy = 1;
@@ -201,10 +210,9 @@
     in2 = 0;
     in1 = 0; 
     location = 2;
-
     while(1){
         
-        while(list.length() < 4){
+        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
@@ -222,7 +230,7 @@
             
             if (requested_floor1 != requested_floor2) {requested_floor = 0;}
             else{requested_floor = requested_floor1;}
-            if (inqueue(requested_floor) == 0 && requested_floor != 0){
+            if ((inqueue(requested_floor) == 0 && requested_floor != 0) || list.length() > 0){
                 call(requested_floor); 
                 }
 }