Mohammed Alshiekh / Mbed 2 deprecated elevator_with_queue

course work

Dependencies:   LinkedList mbed

elevator.cpp@3:19106c977df1, 2015-10-08 (annotated)

Committer:
sahabi
Date:
Thu Oct 08 22:29:56 2015 +0000
Revision:
3:19106c977df1
Parent:
2:07f2fe858dbd
Child:
4:fba6c97a6192
working without voltage

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sahabi 0:69be3faae0a0 1 #include "mbed.h"
sahabi 0:69be3faae0a0 2
sahabi 3:19106c977df1 3 Serial pc(USBTX,USBRX);
sahabi 3:19106c977df1 4 PwmOut dc_motor(p23);
sahabi 0:69be3faae0a0 5 PwmOut servo1(p21);
sahabi 0:69be3faae0a0 6 PwmOut servo2(p22);
sahabi 0:69be3faae0a0 7 DigitalInOut in1(p5);
sahabi 0:69be3faae0a0 8 DigitalInOut in2(p6);
sahabi 3:19106c977df1 9 AnalogIn an(A0);
sahabi 3:19106c977df1 10 InterruptIn firstfloor(p15);
sahabi 0:69be3faae0a0 11 InterruptIn secondfloor(p8);
sahabi 0:69be3faae0a0 12 InterruptIn thirdfloor(p9);
sahabi 0:69be3faae0a0 13 InterruptIn fourthfloor(p10);
sahabi 0:69be3faae0a0 14 InterruptIn fifthfloor(p11);
sahabi 1:28120714ad80 15 InterruptIn sensor(p12);
sahabi 1:28120714ad80 16 DigitalOut led4(LED4);
sahabi 1:28120714ad80 17 DigitalOut led3(LED3);
sahabi 1:28120714ad80 18 DigitalOut led2(LED2);
sahabi 1:28120714ad80 19 DigitalOut led1(LED1);
sahabi 2:07f2fe858dbd 20 int first = 0;
sahabi 0:69be3faae0a0 21 Timer time1;
sahabi 2:07f2fe858dbd 22 int once = 0;
sahabi 0:69be3faae0a0 23 float stationary = 0.075;
sahabi 0:69be3faae0a0 24 float ccw = 0.085; // counter clock-wise
sahabi 0:69be3faae0a0 25 float cw = 0.065; // clock-wise
sahabi 1:28120714ad80 26 int reached = 0;
sahabi 3:19106c977df1 27 int location;
sahabi 2:07f2fe858dbd 28 float frequency;
sahabi 0:69be3faae0a0 29 int requested_floor;
sahabi 0:69be3faae0a0 30 int busy = 0;
sahabi 2:07f2fe858dbd 31 int currentfloor;
sahabi 1:28120714ad80 32
sahabi 3:19106c977df1 33 void serve(){
sahabi 2:07f2fe858dbd 34
sahabi 3:19106c977df1 35 if(busy == 1){
sahabi 3:19106c977df1 36 if (first == 0){
sahabi 3:19106c977df1 37 time1.reset();
sahabi 3:19106c977df1 38 time1.start();
sahabi 3:19106c977df1 39 first = 1;
sahabi 3:19106c977df1 40 }
sahabi 3:19106c977df1 41
sahabi 3:19106c977df1 42 else if (first == 1){
sahabi 3:19106c977df1 43 time1.stop();
sahabi 3:19106c977df1 44 first = 0;
sahabi 3:19106c977df1 45 once++;
sahabi 3:19106c977df1 46 frequency = 1./time1.read();
sahabi 3:19106c977df1 47
sahabi 3:19106c977df1 48 if (once == 2){
sahabi 3:19106c977df1 49 once = 0;
sahabi 3:19106c977df1 50
sahabi 3:19106c977df1 51 if (frequency >= 900 && frequency <= 1100){currentfloor = 5;}
sahabi 3:19106c977df1 52 else if (frequency >= 600 && frequency <= 800){currentfloor = 4;led4 = 1;}
sahabi 3:19106c977df1 53 else if (frequency >= 400 && frequency <= 590){currentfloor = 3;led3 = 1;}
sahabi 3:19106c977df1 54 else if (frequency >= 200 && frequency <= 300){currentfloor = 2;led2 = 1;}
sahabi 3:19106c977df1 55 else if (frequency >= 50 && frequency <= 150){currentfloor = 1;led1 = 1;}
sahabi 3:19106c977df1 56
sahabi 3:19106c977df1 57 if (currentfloor == requested_floor){
sahabi 3:19106c977df1 58 dc_motor.write(0.0f);
sahabi 3:19106c977df1 59 led1 = 0;
sahabi 3:19106c977df1 60 led2 = 0;
sahabi 3:19106c977df1 61 led3 = 0;
sahabi 3:19106c977df1 62 led4 = 0;
sahabi 3:19106c977df1 63 in1 = 0;
sahabi 3:19106c977df1 64 in2 = 0;
sahabi 3:19106c977df1 65 servo1.write(cw);
sahabi 3:19106c977df1 66 servo2.write(ccw);
sahabi 3:19106c977df1 67 wait(0.33);
sahabi 3:19106c977df1 68 servo1.write(stationary);
sahabi 3:19106c977df1 69 servo2.write(stationary);
sahabi 3:19106c977df1 70 busy = 0;
sahabi 3:19106c977df1 71 location = requested_floor;
sahabi 3:19106c977df1 72 }
sahabi 2:07f2fe858dbd 73
sahabi 0:69be3faae0a0 74 }
sahabi 2:07f2fe858dbd 75
sahabi 2:07f2fe858dbd 76 }
sahabi 1:28120714ad80 77 }
sahabi 0:69be3faae0a0 78 }
sahabi 1:28120714ad80 79
sahabi 0:69be3faae0a0 80 void moveUp(){
sahabi 3:19106c977df1 81 dc_motor.write(1.0f);
sahabi 0:69be3faae0a0 82 in1 = 1;
sahabi 0:69be3faae0a0 83 in2 = 0;
sahabi 0:69be3faae0a0 84 }
sahabi 0:69be3faae0a0 85
sahabi 0:69be3faae0a0 86 void moveDown(){
sahabi 3:19106c977df1 87
sahabi 3:19106c977df1 88 dc_motor.write(1.0f);
sahabi 3:19106c977df1 89 pc.printf("A");
sahabi 0:69be3faae0a0 90 in1 = 0;
sahabi 0:69be3faae0a0 91 in2 = 1;
sahabi 0:69be3faae0a0 92 }
sahabi 0:69be3faae0a0 93
sahabi 0:69be3faae0a0 94
sahabi 0:69be3faae0a0 95 void closeDoor(){
sahabi 0:69be3faae0a0 96 servo1.write(ccw);
sahabi 0:69be3faae0a0 97 servo2.write(cw);
sahabi 1:28120714ad80 98 wait(0.33);
sahabi 0:69be3faae0a0 99 servo1.write(stationary);
sahabi 0:69be3faae0a0 100 servo2.write(stationary);
sahabi 0:69be3faae0a0 101 }
sahabi 0:69be3faae0a0 102
sahabi 0:69be3faae0a0 103 void openDoor(){
sahabi 0:69be3faae0a0 104 servo1.write(cw);
sahabi 0:69be3faae0a0 105 servo2.write(ccw);
sahabi 1:28120714ad80 106 wait(0.33);
sahabi 0:69be3faae0a0 107 servo1.write(stationary);
sahabi 0:69be3faae0a0 108 servo2.write(stationary);
sahabi 0:69be3faae0a0 109 }
sahabi 0:69be3faae0a0 110
sahabi 0:69be3faae0a0 111 void call(int next) {
sahabi 1:28120714ad80 112 if (busy == 0){
sahabi 1:28120714ad80 113 requested_floor = next;
sahabi 1:28120714ad80 114 if (location > next){
sahabi 1:28120714ad80 115 closeDoor();
sahabi 3:19106c977df1 116 pc.printf("%d",next);
sahabi 1:28120714ad80 117 moveDown();
sahabi 1:28120714ad80 118 busy = 1;
sahabi 1:28120714ad80 119 }
sahabi 1:28120714ad80 120
sahabi 1:28120714ad80 121 else if (location < next){
sahabi 1:28120714ad80 122 closeDoor();
sahabi 3:19106c977df1 123 pc.printf("C");
sahabi 1:28120714ad80 124 moveUp();
sahabi 1:28120714ad80 125 busy = 1;
sahabi 1:28120714ad80 126 }
sahabi 0:69be3faae0a0 127 }
sahabi 1:28120714ad80 128 }
sahabi 0:69be3faae0a0 129
sahabi 0:69be3faae0a0 130 void call1(void) { call(1); }
sahabi 0:69be3faae0a0 131 void call2(void) { call(2); }
sahabi 0:69be3faae0a0 132 void call3(void) { call(3); }
sahabi 0:69be3faae0a0 133 void call4(void) { call(4); }
sahabi 0:69be3faae0a0 134 void call5(void) { call(5); }
sahabi 0:69be3faae0a0 135
sahabi 0:69be3faae0a0 136
sahabi 0:69be3faae0a0 137 int main() {
sahabi 0:69be3faae0a0 138 servo1.period(0.02f);
sahabi 0:69be3faae0a0 139 servo2.period(0.02f);
sahabi 0:69be3faae0a0 140 servo1.write(stationary);
sahabi 0:69be3faae0a0 141 servo2.write(stationary);
sahabi 0:69be3faae0a0 142 in1.output();
sahabi 0:69be3faae0a0 143 in2.output();
sahabi 0:69be3faae0a0 144 in2 = 0;
sahabi 3:19106c977df1 145 in1 = 0;
sahabi 0:69be3faae0a0 146 firstfloor.rise(&call1);
sahabi 0:69be3faae0a0 147 secondfloor.rise(&call2);
sahabi 0:69be3faae0a0 148 thirdfloor.rise(&call3);
sahabi 0:69be3faae0a0 149 fourthfloor.rise(&call4);
sahabi 0:69be3faae0a0 150 fifthfloor.rise(&call5);
sahabi 3:19106c977df1 151 location = 2;
sahabi 3:19106c977df1 152 sensor.fall(&serve);
sahabi 2:07f2fe858dbd 153
sahabi 3:19106c977df1 154 while(1){
sahabi 3:19106c977df1 155 // if (an > 0.59 && an < 0.7 ){requested_floor = 1;}
sahabi 3:19106c977df1 156 // else if (an > 1.19 && an < 1.3 ){requested_floor = 2;}
sahabi 3:19106c977df1 157 // else if (an > 1.79 && an < 1.9 ){requested_floor = 3;}
sahabi 3:19106c977df1 158 // else if (an > 2.39 && an < 2.5 ){requested_floor = 4;}
sahabi 3:19106c977df1 159 // else if (an > 2.99 && an < 3.1 ){requested_floor = 5;}
sahabi 0:69be3faae0a0 160 }
sahabi 0:69be3faae0a0 161 }
sahabi 0:69be3faae0a0 162
sahabi 0:69be3faae0a0 163