Diff: main.cpp

Revision:

4:a5a7611a8d25

Parent:

3:4f15d12f0ac0

Child:

5:8e2b7caedd1d

--- a/main.cpp	Sat Oct 10 01:16:27 2015 +0000
+++ b/main.cpp	Thu Oct 15 03:50:43 2015 +0000
@@ -81,7 +81,7 @@
     wait(0.1f);
     avg+=freqcnt;
     pc.printf("%f\r\n",avg);
-    if (avg>270 && avg < 300) {    //fifth floor
+    if (avg>270 && avg < 310) {    //fifth floor
         floorat = 5;
     } else if (avg>180) {    //fourth floor
         floorat = 4;
@@ -124,17 +124,19 @@
     }
     wait(0.5f);
     DCenable = 0;
-    
+    short current_floor;
     pc.printf("at floor: %d\r\n",get_floor());
     pc.printf("go to floor: %d\r\n",get_requested(request));
     while(1) {
-    if(get_requested(request) > get_floor())
+        if(get_floor() !=0)
+            current_floor = get_floor();
+    if(get_requested(request) > current_floor)
         {
             pc.printf("go up\r\n");
             motor1 = 0;
             motor2 = 1;
             DCenable = 0.8f;
-        } else if(get_requested(request) < get_floor()) {
+        } else if(get_requested(request) < current_floor) {
             pc.printf("go down\r\n");
             motor1 = 1;
             motor2 = 0;
@@ -147,8 +149,26 @@
     
     while(onFloor) {
         wait(1);
-        pc.printf("at floor: %d\r\n",get_floor());
+        if(get_floor()!=0)
+            current_floor = get_floor();
+        pc.printf("at floor: %d\r\n",current_floor);
         DCenable = 0.8f;
+        if(get_requested(request) > current_floor)
+        {
+            pc.printf("go up\r\n");
+            motor1 = 0;
+            motor2 = 1;
+            DCenable = 0.8f;
+        } else if(get_requested(request) < current_floor) {
+            pc.printf("go down\r\n");
+            motor1 = 1;
+            motor2 = 0;
+            DCenable = 0.8f;
+        } else {
+            motor1=0;
+            motor2=0;
+            DCenable = 0;
+        }
     }
     pc.printf("off current floor\r\n");
     
@@ -160,7 +180,7 @@
     
     wait(2);
     
-    pc.printf("at floor: %d\r\n",get_floor());
+    pc.printf("at floor: %d\r\n",current_floor);
     pc.printf("go to floor: %d\r\n",get_requested(request));
     
     }