Diff: elevator.cpp

Revision:

1:28120714ad80

Parent:

0:69be3faae0a0

Child:

2:07f2fe858dbd

--- a/elevator.cpp	Tue Oct 06 00:58:39 2015 +0000
+++ b/elevator.cpp	Thu Oct 08 17:31:08 2015 +0000
@@ -10,8 +10,11 @@
 InterruptIn thirdfloor(p9);
 InterruptIn fourthfloor(p10);
 InterruptIn fifthfloor(p11);
-InterruptIn afloor(p12);
- 
+InterruptIn sensor(p12);
+DigitalOut led4(LED4);
+DigitalOut led3(LED3); 
+DigitalOut led2(LED2); 
+DigitalOut led1(LED1); 
 Timeout timeout;
  
 Timer time1;
@@ -20,28 +23,34 @@
 int closed;
 float ccw = 0.085;   // counter clock-wise
 float cw = 0.065;    // clock-wise
+int reached = 0;
 int location = 1;
-int reached = 0;
-float frequency;
+int frequency;
 int requested_floor;
 int busy = 0;
 
-void startTimer(){
+void startTimer(){ 
+    if(busy == 1){
     reached = 0;
     time1.start();
     }
+}
 
 void serve(){
+
+    if(busy == 1){
     time1.stop();
-    frequency = 1./time1.read();
+
+    frequency = 1/(time1.read()*2);
     int floor;
-    if (frequency == 100){floor = 1;}
-    if (frequency == 250){floor = 2;}
-    if (frequency == 500){floor = 3;}
-    if (frequency == 750){floor = 4;}
-    if (frequency == 1000){floor = 5;}
+    if (frequency >= 900 && frequency <= 1100){floor = 5;}
+    else if (frequency >= 700 && frequency <= 800){floor = 4;led4 = !led4;}
+    else if (frequency >= 450 && frequency <= 550){floor = 3;led3 = !led3;}
+    else if (frequency >= 220 && frequency <= 290){floor = 2;led2 = !led2;}
+    else if (frequency >= 70 && frequency <= 150){floor = 1;led1 = !led1;}
+    
     if (floor == requested_floor){
-        reached = 1;
+        reached = 1;     
         }
         else{
             reached = 0;
@@ -51,22 +60,23 @@
         in2 = 0;
         servo1.write(cw);
         servo2.write(ccw); 
-        wait(0.5); 
+        wait(0.33); 
         servo1.write(stationary);
         servo2.write(stationary);
         busy = 0;
+        location = requested_floor;
         }
+}
     }
-       
-
+    
 void moveUp(){
-    dc_motor.write(0.02f);
+    dc_motor.write(0.6f);
     in1 = 1;
     in2 = 0;
     }
     
 void moveDown(){
-    dc_motor.write(0.02f);
+    dc_motor.write(0.6f);
     in1 = 0;
     in2 = 1;
     }
@@ -79,7 +89,7 @@
 void closeDoor(){
     servo1.write(ccw);
     servo2.write(cw); 
-    wait(0.5); 
+    wait(0.33); 
     servo1.write(stationary);
     servo2.write(stationary);   
 }
@@ -87,25 +97,27 @@
 void openDoor(){
     servo1.write(cw);
     servo2.write(ccw); 
-    wait(0.5); 
+    wait(0.33); 
     servo1.write(stationary);
     servo2.write(stationary);
 }
 
 void call(int next) {
-    busy = 1;
-    requested_floor = next;
-    if (location > next){
-        closeDoor();
-        moveUp();
-        }
-        
-    else if (location < next){
-        closeDoor();
-        moveDown();
-        }
-    location = next;
+    if (busy == 0){
+        requested_floor = next;
+        if (location > next){
+            closeDoor();
+            moveDown();
+            busy = 1;
+            }
+            
+        else if (location < next){
+            closeDoor();
+            moveUp();
+            busy = 1;
+            }
     }
+}
 
 void call1(void) { call(1); }
 void call2(void) { call(2); }
@@ -130,8 +142,8 @@
     thirdfloor.rise(&call3); 
     fourthfloor.rise(&call4); 
     fifthfloor.rise(&call5);
-    afloor.fall(&startTimer);
-    afloor.rise(&serve);
+    sensor.rise(&startTimer);
+    sensor.fall(&serve);
 
     while(1){    
  }