Diff: elevator.cpp

Revision:

0:69be3faae0a0

Child:

1:28120714ad80

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/elevator.cpp	Tue Oct 06 00:58:39 2015 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+PwmOut dc_motor(p23);
+DigitalInOut in1(p5);
+DigitalInOut in2(p6);
+InterruptIn firstfloor(p7);
+InterruptIn secondfloor(p8);
+InterruptIn thirdfloor(p9);
+InterruptIn fourthfloor(p10);
+InterruptIn fifthfloor(p11);
+InterruptIn afloor(p12);
+ 
+Timeout timeout;
+ 
+Timer time1;
+ 
+float stationary = 0.075;
+int closed;
+float ccw = 0.085;   // counter clock-wise
+float cw = 0.065;    // clock-wise
+int location = 1;
+int reached = 0;
+float frequency;
+int requested_floor;
+int busy = 0;
+
+void startTimer(){
+    reached = 0;
+    time1.start();
+    }
+
+void serve(){
+    time1.stop();
+    frequency = 1./time1.read();
+    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 (floor == requested_floor){
+        reached = 1;
+        }
+        else{
+            reached = 0;
+        }
+    if (reached == 1){
+        in1 = 0;
+        in2 = 0;
+        servo1.write(cw);
+        servo2.write(ccw); 
+        wait(0.5); 
+        servo1.write(stationary);
+        servo2.write(stationary);
+        busy = 0;
+        }
+    }
+       
+
+void moveUp(){
+    dc_motor.write(0.02f);
+    in1 = 1;
+    in2 = 0;
+    }
+    
+void moveDown(){
+    dc_motor.write(0.02f);
+    in1 = 0;
+    in2 = 1;
+    }
+
+void stop(){
+    in1 = 0;
+    in2 = 0;
+    }
+
+void closeDoor(){
+    servo1.write(ccw);
+    servo2.write(cw); 
+    wait(0.5); 
+    servo1.write(stationary);
+    servo2.write(stationary);   
+}
+
+void openDoor(){
+    servo1.write(cw);
+    servo2.write(ccw); 
+    wait(0.5); 
+    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;
+    }
+
+void call1(void) { call(1); }
+void call2(void) { call(2); }
+void call3(void) { call(3); }
+void call4(void) { call(4); }
+void call5(void) { call(5); }
+
+
+int main() {
+    servo1.period(0.02f);
+    servo2.period(0.02f);
+    dc_motor.period(0.02f);
+    servo1.write(stationary);
+    servo2.write(stationary);
+    dc_motor.write(0);
+    in1.output();
+    in1 = 0;
+    in2.output();
+    in2 = 0;
+    firstfloor.rise(&call1); 
+    secondfloor.rise(&call2); 
+    thirdfloor.rise(&call3); 
+    fourthfloor.rise(&call4); 
+    fifthfloor.rise(&call5);
+    afloor.fall(&startTimer);
+    afloor.rise(&serve);
+
+    while(1){    
+ }
+}
+ 
+