Mohammed Alshiekh
/
elevator_with_queue
course work
elevator.cpp@6:1f9baf8d833e, 2015-10-11 (annotated)
- Committer:
- sahabi
- Date:
- Sun Oct 11 21:32:14 2015 +0000
- Revision:
- 6:1f9baf8d833e
- Parent:
- 5:15bb8f6b6d85
- Child:
- 7:9ff7df122ff6
done. servo not tested.
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:fba6c97a6192 | 9 | AnalogIn an(p16); |
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 | 4:fba6c97a6192 | 29 | int requested_floor1, requested_floor2, 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 | 6:1f9baf8d833e | 47 | |
sahabi | 3:19106c977df1 | 48 | |
sahabi | 6:1f9baf8d833e | 49 | if (once == 200){ |
sahabi | 3:19106c977df1 | 50 | once = 0; |
sahabi | 3:19106c977df1 | 51 | |
sahabi | 6:1f9baf8d833e | 52 | if (frequency >= 900.0f && frequency <= 1100.0f){currentfloor = 5;} |
sahabi | 6:1f9baf8d833e | 53 | else if (frequency >= 600.0f && frequency <= 800.0f){currentfloor = 4;} |
sahabi | 6:1f9baf8d833e | 54 | else if (frequency >= 400.0f && frequency <= 590.0f){currentfloor = 3;} |
sahabi | 6:1f9baf8d833e | 55 | else if (frequency >= 200.0f && frequency <= 300.0f){currentfloor = 2;} |
sahabi | 6:1f9baf8d833e | 56 | else if (frequency >= 50.0f && frequency <= 150.0f){currentfloor = 1;} |
sahabi | 3:19106c977df1 | 57 | |
sahabi | 3:19106c977df1 | 58 | if (currentfloor == requested_floor){ |
sahabi | 6:1f9baf8d833e | 59 | pc.printf("served: %d floor frequency: %f \n",currentfloor,frequency); |
sahabi | 6:1f9baf8d833e | 60 | |
sahabi | 3:19106c977df1 | 61 | dc_motor.write(0.0f); |
sahabi | 3:19106c977df1 | 62 | led1 = 0; |
sahabi | 3:19106c977df1 | 63 | led2 = 0; |
sahabi | 3:19106c977df1 | 64 | led3 = 0; |
sahabi | 3:19106c977df1 | 65 | led4 = 0; |
sahabi | 3:19106c977df1 | 66 | in1 = 0; |
sahabi | 3:19106c977df1 | 67 | in2 = 0; |
sahabi | 3:19106c977df1 | 68 | servo1.write(cw); |
sahabi | 3:19106c977df1 | 69 | servo2.write(ccw); |
sahabi | 3:19106c977df1 | 70 | wait(0.33); |
sahabi | 3:19106c977df1 | 71 | servo1.write(stationary); |
sahabi | 3:19106c977df1 | 72 | servo2.write(stationary); |
sahabi | 3:19106c977df1 | 73 | busy = 0; |
sahabi | 3:19106c977df1 | 74 | location = requested_floor; |
sahabi | 6:1f9baf8d833e | 75 | pc.printf("at: %d \n",location); |
sahabi | 4:fba6c97a6192 | 76 | sensor.fall(0); |
sahabi | 3:19106c977df1 | 77 | } |
sahabi | 2:07f2fe858dbd | 78 | |
sahabi | 0:69be3faae0a0 | 79 | } |
sahabi | 2:07f2fe858dbd | 80 | |
sahabi | 2:07f2fe858dbd | 81 | } |
sahabi | 1:28120714ad80 | 82 | } |
sahabi | 0:69be3faae0a0 | 83 | } |
sahabi | 1:28120714ad80 | 84 | |
sahabi | 0:69be3faae0a0 | 85 | void moveUp(){ |
sahabi | 6:1f9baf8d833e | 86 | dc_motor.write(0.9f); |
sahabi | 0:69be3faae0a0 | 87 | in1 = 1; |
sahabi | 0:69be3faae0a0 | 88 | in2 = 0; |
sahabi | 0:69be3faae0a0 | 89 | } |
sahabi | 0:69be3faae0a0 | 90 | |
sahabi | 0:69be3faae0a0 | 91 | void moveDown(){ |
sahabi | 3:19106c977df1 | 92 | |
sahabi | 6:1f9baf8d833e | 93 | dc_motor.write(0.9f); |
sahabi | 0:69be3faae0a0 | 94 | in1 = 0; |
sahabi | 0:69be3faae0a0 | 95 | in2 = 1; |
sahabi | 0:69be3faae0a0 | 96 | } |
sahabi | 0:69be3faae0a0 | 97 | |
sahabi | 0:69be3faae0a0 | 98 | |
sahabi | 0:69be3faae0a0 | 99 | void closeDoor(){ |
sahabi | 0:69be3faae0a0 | 100 | servo1.write(ccw); |
sahabi | 0:69be3faae0a0 | 101 | servo2.write(cw); |
sahabi | 1:28120714ad80 | 102 | wait(0.33); |
sahabi | 0:69be3faae0a0 | 103 | servo1.write(stationary); |
sahabi | 0:69be3faae0a0 | 104 | servo2.write(stationary); |
sahabi | 0:69be3faae0a0 | 105 | } |
sahabi | 0:69be3faae0a0 | 106 | |
sahabi | 0:69be3faae0a0 | 107 | void openDoor(){ |
sahabi | 0:69be3faae0a0 | 108 | servo1.write(cw); |
sahabi | 0:69be3faae0a0 | 109 | servo2.write(ccw); |
sahabi | 1:28120714ad80 | 110 | wait(0.33); |
sahabi | 0:69be3faae0a0 | 111 | servo1.write(stationary); |
sahabi | 0:69be3faae0a0 | 112 | servo2.write(stationary); |
sahabi | 0:69be3faae0a0 | 113 | } |
sahabi | 0:69be3faae0a0 | 114 | |
sahabi | 0:69be3faae0a0 | 115 | void call(int next) { |
sahabi | 4:fba6c97a6192 | 116 | if (busy == 0 && next > 0.5){ |
sahabi | 1:28120714ad80 | 117 | requested_floor = next; |
sahabi | 1:28120714ad80 | 118 | if (location > next){ |
sahabi | 6:1f9baf8d833e | 119 | pc.printf("serving: %d \n",next); |
sahabi | 1:28120714ad80 | 120 | closeDoor(); |
sahabi | 1:28120714ad80 | 121 | moveDown(); |
sahabi | 1:28120714ad80 | 122 | busy = 1; |
sahabi | 4:fba6c97a6192 | 123 | sensor.fall(&serve); |
sahabi | 1:28120714ad80 | 124 | } |
sahabi | 1:28120714ad80 | 125 | |
sahabi | 4:fba6c97a6192 | 126 | else if (location < next ){ |
sahabi | 6:1f9baf8d833e | 127 | pc.printf("serving: %d \n",next); |
sahabi | 1:28120714ad80 | 128 | closeDoor(); |
sahabi | 1:28120714ad80 | 129 | moveUp(); |
sahabi | 1:28120714ad80 | 130 | busy = 1; |
sahabi | 4:fba6c97a6192 | 131 | sensor.fall(&serve); |
sahabi | 1:28120714ad80 | 132 | } |
sahabi | 0:69be3faae0a0 | 133 | } |
sahabi | 1:28120714ad80 | 134 | } |
sahabi | 0:69be3faae0a0 | 135 | |
sahabi | 4:fba6c97a6192 | 136 | //void call1(void) { call(1); } |
sahabi | 4:fba6c97a6192 | 137 | //void call2(void) { call(2); } |
sahabi | 4:fba6c97a6192 | 138 | //void call3(void) { call(3); } |
sahabi | 4:fba6c97a6192 | 139 | //void call4(void) { call(4); } |
sahabi | 4:fba6c97a6192 | 140 | //void call5(void) { call(5); } |
sahabi | 0:69be3faae0a0 | 141 | |
sahabi | 0:69be3faae0a0 | 142 | |
sahabi | 0:69be3faae0a0 | 143 | int main() { |
sahabi | 0:69be3faae0a0 | 144 | servo1.period(0.02f); |
sahabi | 0:69be3faae0a0 | 145 | servo2.period(0.02f); |
sahabi | 0:69be3faae0a0 | 146 | servo1.write(stationary); |
sahabi | 0:69be3faae0a0 | 147 | servo2.write(stationary); |
sahabi | 0:69be3faae0a0 | 148 | in1.output(); |
sahabi | 0:69be3faae0a0 | 149 | in2.output(); |
sahabi | 0:69be3faae0a0 | 150 | in2 = 0; |
sahabi | 3:19106c977df1 | 151 | in1 = 0; |
sahabi | 4:fba6c97a6192 | 152 | // firstfloor.rise(&call1); |
sahabi | 4:fba6c97a6192 | 153 | // secondfloor.rise(&call2); |
sahabi | 4:fba6c97a6192 | 154 | // thirdfloor.rise(&call3); |
sahabi | 4:fba6c97a6192 | 155 | // fourthfloor.rise(&call4); |
sahabi | 4:fba6c97a6192 | 156 | // fifthfloor.rise(&call5); |
sahabi | 6:1f9baf8d833e | 157 | location = 1; |
sahabi | 4:fba6c97a6192 | 158 | // |
sahabi | 2:07f2fe858dbd | 159 | |
sahabi | 3:19106c977df1 | 160 | while(1){ |
sahabi | 4:fba6c97a6192 | 161 | |
sahabi | 4:fba6c97a6192 | 162 | while(busy == 0){ |
sahabi | 4:fba6c97a6192 | 163 | |
sahabi | 6:1f9baf8d833e | 164 | if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor1 = 1;} //1 |
sahabi | 6:1f9baf8d833e | 165 | else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor1 = 2;}//2 |
sahabi | 6:1f9baf8d833e | 166 | else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor1 = 3;} //3 |
sahabi | 6:1f9baf8d833e | 167 | else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor1 = 4;} //4 |
sahabi | 6:1f9baf8d833e | 168 | else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor1 = 5;} //5 |
sahabi | 4:fba6c97a6192 | 169 | else{requested_floor1 = 0;} |
sahabi | 4:fba6c97a6192 | 170 | |
sahabi | 6:1f9baf8d833e | 171 | if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor2 = 1;led1 = 1;} |
sahabi | 6:1f9baf8d833e | 172 | else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor2 = 2;led2 = 1;} |
sahabi | 6:1f9baf8d833e | 173 | else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor2 = 3;led3 = 1;} |
sahabi | 6:1f9baf8d833e | 174 | else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor2 = 4; led4 = 1;} |
sahabi | 5:15bb8f6b6d85 | 175 | else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor2 = 5;} |
sahabi | 4:fba6c97a6192 | 176 | else{requested_floor2 = 0;} |
sahabi | 4:fba6c97a6192 | 177 | |
sahabi | 4:fba6c97a6192 | 178 | if (requested_floor1 != requested_floor2) {requested_floor = 0;} |
sahabi | 4:fba6c97a6192 | 179 | else{requested_floor = requested_floor1;} |
sahabi | 6:1f9baf8d833e | 180 | |
sahabi | 4:fba6c97a6192 | 181 | call(requested_floor); |
sahabi | 4:fba6c97a6192 | 182 | } |
sahabi | 0:69be3faae0a0 | 183 | } |
sahabi | 0:69be3faae0a0 | 184 | } |
sahabi | 0:69be3faae0a0 | 185 | |
sahabi | 0:69be3faae0a0 | 186 |