Mohammed Alshiekh
/
elevator_with_queue
course work
elevator.cpp@9:95cbc05bd81d, 2015-10-20 (annotated)
- Committer:
- sahabi
- Date:
- Tue Oct 20 19:29:47 2015 +0000
- Revision:
- 9:95cbc05bd81d
- Parent:
- 8:dd34a528a47c
new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sahabi | 0:69be3faae0a0 | 1 | #include "mbed.h" |
sahabi | 8:dd34a528a47c | 2 | #include "LinkedList.h" |
sahabi | 0:69be3faae0a0 | 3 | |
sahabi | 8:dd34a528a47c | 4 | int serving; |
sahabi | 8:dd34a528a47c | 5 | LinkedList<node>list; |
sahabi | 3:19106c977df1 | 6 | Serial pc(USBTX,USBRX); |
sahabi | 3:19106c977df1 | 7 | PwmOut dc_motor(p23); |
sahabi | 0:69be3faae0a0 | 8 | PwmOut servo1(p21); |
sahabi | 0:69be3faae0a0 | 9 | PwmOut servo2(p22); |
sahabi | 0:69be3faae0a0 | 10 | DigitalInOut in1(p5); |
sahabi | 0:69be3faae0a0 | 11 | DigitalInOut in2(p6); |
sahabi | 4:fba6c97a6192 | 12 | AnalogIn an(p16); |
sahabi | 3:19106c977df1 | 13 | InterruptIn firstfloor(p15); |
sahabi | 0:69be3faae0a0 | 14 | InterruptIn secondfloor(p8); |
sahabi | 0:69be3faae0a0 | 15 | InterruptIn thirdfloor(p9); |
sahabi | 0:69be3faae0a0 | 16 | InterruptIn fourthfloor(p10); |
sahabi | 0:69be3faae0a0 | 17 | InterruptIn fifthfloor(p11); |
sahabi | 1:28120714ad80 | 18 | InterruptIn sensor(p12); |
sahabi | 1:28120714ad80 | 19 | DigitalOut led4(LED4); |
sahabi | 1:28120714ad80 | 20 | DigitalOut led3(LED3); |
sahabi | 1:28120714ad80 | 21 | DigitalOut led2(LED2); |
sahabi | 1:28120714ad80 | 22 | DigitalOut led1(LED1); |
sahabi | 2:07f2fe858dbd | 23 | int first = 0; |
sahabi | 0:69be3faae0a0 | 24 | Timer time1; |
sahabi | 2:07f2fe858dbd | 25 | int once = 0; |
sahabi | 0:69be3faae0a0 | 26 | float stationary = 0.075; |
sahabi | 7:9ff7df122ff6 | 27 | float open = 0.085; // counter clock-wise |
sahabi | 7:9ff7df122ff6 | 28 | float closed = 0.040; // clock-wise |
sahabi | 7:9ff7df122ff6 | 29 | float open2 = 0.100; |
sahabi | 7:9ff7df122ff6 | 30 | float closed2 = 0.055; |
sahabi | 1:28120714ad80 | 31 | int reached = 0; |
sahabi | 3:19106c977df1 | 32 | int location; |
sahabi | 2:07f2fe858dbd | 33 | float frequency; |
sahabi | 4:fba6c97a6192 | 34 | int requested_floor1, requested_floor2, requested_floor; |
sahabi | 0:69be3faae0a0 | 35 | int busy = 0; |
sahabi | 2:07f2fe858dbd | 36 | int currentfloor; |
sahabi | 8:dd34a528a47c | 37 | int globedir = 2; |
sahabi | 1:28120714ad80 | 38 | |
sahabi | 8:dd34a528a47c | 39 | |
sahabi | 8:dd34a528a47c | 40 | int inqueue(int request){ |
sahabi | 8:dd34a528a47c | 41 | int i; |
sahabi | 8:dd34a528a47c | 42 | int exist = 0; |
sahabi | 8:dd34a528a47c | 43 | node *tmp; |
sahabi | 9:95cbc05bd81d | 44 | for (i = 1; i<= list.length();i++){ |
sahabi | 8:dd34a528a47c | 45 | tmp = list.pop(i); |
sahabi | 8:dd34a528a47c | 46 | if (request == (int) tmp->data) |
sahabi | 8:dd34a528a47c | 47 | exist = 1; |
sahabi | 8:dd34a528a47c | 48 | } |
sahabi | 8:dd34a528a47c | 49 | return exist; |
sahabi | 8:dd34a528a47c | 50 | } |
sahabi | 8:dd34a528a47c | 51 | |
sahabi | 8:dd34a528a47c | 52 | int dir(int current, int next){ |
sahabi | 8:dd34a528a47c | 53 | int direction; |
sahabi | 8:dd34a528a47c | 54 | if (current > next) |
sahabi | 9:95cbc05bd81d | 55 | direction = 0; |
sahabi | 8:dd34a528a47c | 56 | if (current <= next) |
sahabi | 9:95cbc05bd81d | 57 | direction = 1; |
sahabi | 8:dd34a528a47c | 58 | return direction; |
sahabi | 8:dd34a528a47c | 59 | } |
sahabi | 3:19106c977df1 | 60 | void serve(){ |
sahabi | 2:07f2fe858dbd | 61 | |
sahabi | 3:19106c977df1 | 62 | if(busy == 1){ |
sahabi | 3:19106c977df1 | 63 | if (first == 0){ |
sahabi | 3:19106c977df1 | 64 | time1.reset(); |
sahabi | 3:19106c977df1 | 65 | time1.start(); |
sahabi | 3:19106c977df1 | 66 | first = 1; |
sahabi | 3:19106c977df1 | 67 | } |
sahabi | 3:19106c977df1 | 68 | |
sahabi | 3:19106c977df1 | 69 | else if (first == 1){ |
sahabi | 3:19106c977df1 | 70 | time1.stop(); |
sahabi | 3:19106c977df1 | 71 | first = 0; |
sahabi | 3:19106c977df1 | 72 | once++; |
sahabi | 3:19106c977df1 | 73 | frequency = 1./time1.read(); |
sahabi | 6:1f9baf8d833e | 74 | |
sahabi | 8:dd34a528a47c | 75 | if (once == 50){ |
sahabi | 3:19106c977df1 | 76 | once = 0; |
sahabi | 3:19106c977df1 | 77 | |
sahabi | 6:1f9baf8d833e | 78 | if (frequency >= 900.0f && frequency <= 1100.0f){currentfloor = 5;} |
sahabi | 6:1f9baf8d833e | 79 | else if (frequency >= 600.0f && frequency <= 800.0f){currentfloor = 4;} |
sahabi | 6:1f9baf8d833e | 80 | else if (frequency >= 400.0f && frequency <= 590.0f){currentfloor = 3;} |
sahabi | 6:1f9baf8d833e | 81 | else if (frequency >= 200.0f && frequency <= 300.0f){currentfloor = 2;} |
sahabi | 6:1f9baf8d833e | 82 | else if (frequency >= 50.0f && frequency <= 150.0f){currentfloor = 1;} |
sahabi | 3:19106c977df1 | 83 | |
sahabi | 8:dd34a528a47c | 84 | if (currentfloor == serving){ |
sahabi | 6:1f9baf8d833e | 85 | pc.printf("served: %d floor frequency: %f \n",currentfloor,frequency); |
sahabi | 6:1f9baf8d833e | 86 | |
sahabi | 3:19106c977df1 | 87 | dc_motor.write(0.0f); |
sahabi | 3:19106c977df1 | 88 | led1 = 0; |
sahabi | 3:19106c977df1 | 89 | led2 = 0; |
sahabi | 3:19106c977df1 | 90 | led3 = 0; |
sahabi | 3:19106c977df1 | 91 | led4 = 0; |
sahabi | 3:19106c977df1 | 92 | in1 = 0; |
sahabi | 3:19106c977df1 | 93 | in2 = 0; |
sahabi | 3:19106c977df1 | 94 | busy = 0; |
sahabi | 7:9ff7df122ff6 | 95 | servo1.write(open); |
sahabi | 9:95cbc05bd81d | 96 | servo2.write(open2); |
sahabi | 9:95cbc05bd81d | 97 | wait(1); |
sahabi | 8:dd34a528a47c | 98 | location = serving; |
sahabi | 6:1f9baf8d833e | 99 | pc.printf("at: %d \n",location); |
sahabi | 4:fba6c97a6192 | 100 | sensor.fall(0); |
sahabi | 3:19106c977df1 | 101 | } |
sahabi | 2:07f2fe858dbd | 102 | |
sahabi | 0:69be3faae0a0 | 103 | } |
sahabi | 2:07f2fe858dbd | 104 | |
sahabi | 2:07f2fe858dbd | 105 | } |
sahabi | 1:28120714ad80 | 106 | } |
sahabi | 0:69be3faae0a0 | 107 | } |
sahabi | 1:28120714ad80 | 108 | |
sahabi | 0:69be3faae0a0 | 109 | void moveUp(){ |
sahabi | 8:dd34a528a47c | 110 | dc_motor.write(0.5f); |
sahabi | 0:69be3faae0a0 | 111 | in1 = 1; |
sahabi | 0:69be3faae0a0 | 112 | in2 = 0; |
sahabi | 0:69be3faae0a0 | 113 | } |
sahabi | 0:69be3faae0a0 | 114 | |
sahabi | 0:69be3faae0a0 | 115 | void moveDown(){ |
sahabi | 3:19106c977df1 | 116 | |
sahabi | 8:dd34a528a47c | 117 | dc_motor.write(0.5f); |
sahabi | 0:69be3faae0a0 | 118 | in1 = 0; |
sahabi | 0:69be3faae0a0 | 119 | in2 = 1; |
sahabi | 0:69be3faae0a0 | 120 | } |
sahabi | 0:69be3faae0a0 | 121 | |
sahabi | 0:69be3faae0a0 | 122 | |
sahabi | 0:69be3faae0a0 | 123 | void closeDoor(){ |
sahabi | 7:9ff7df122ff6 | 124 | servo1.write(closed); |
sahabi | 7:9ff7df122ff6 | 125 | servo2.write(closed2); |
sahabi | 0:69be3faae0a0 | 126 | } |
sahabi | 0:69be3faae0a0 | 127 | |
sahabi | 0:69be3faae0a0 | 128 | void openDoor(){ |
sahabi | 7:9ff7df122ff6 | 129 | servo1.write(open); |
sahabi | 7:9ff7df122ff6 | 130 | servo2.write(open2); |
sahabi | 0:69be3faae0a0 | 131 | } |
sahabi | 0:69be3faae0a0 | 132 | |
sahabi | 0:69be3faae0a0 | 133 | void call(int next) { |
sahabi | 8:dd34a528a47c | 134 | node *tmp; |
sahabi | 8:dd34a528a47c | 135 | int i; |
sahabi | 8:dd34a528a47c | 136 | int assigned = 0; |
sahabi | 9:95cbc05bd81d | 137 | |
sahabi | 9:95cbc05bd81d | 138 | if (list.length() < 4 && next > 0 && inqueue(next) == 0){ |
sahabi | 8:dd34a528a47c | 139 | list.append((int *)next); |
sahabi | 8:dd34a528a47c | 140 | } |
sahabi | 9:95cbc05bd81d | 141 | for (i = 0; i<= list.length();i++){ |
sahabi | 9:95cbc05bd81d | 142 | tmp = list.pop(i); |
sahabi | 9:95cbc05bd81d | 143 | pc.printf(" %d \n",(int)tmp->data); |
sahabi | 9:95cbc05bd81d | 144 | } |
sahabi | 9:95cbc05bd81d | 145 | if (busy == 0 && (int)list.pop(1)->data < 6){ |
sahabi | 8:dd34a528a47c | 146 | |
sahabi | 8:dd34a528a47c | 147 | if (globedir == 2){ |
sahabi | 9:95cbc05bd81d | 148 | tmp = list.pop(1); |
sahabi | 8:dd34a528a47c | 149 | serving = (int)tmp->data; |
sahabi | 9:95cbc05bd81d | 150 | list.remove(1); |
sahabi | 9:95cbc05bd81d | 151 | assigned = 1; |
sahabi | 8:dd34a528a47c | 152 | } |
sahabi | 8:dd34a528a47c | 153 | |
sahabi | 9:95cbc05bd81d | 154 | else if (globedir == 1){ |
sahabi | 9:95cbc05bd81d | 155 | for (i = 1; i<= list.length();i++){ |
sahabi | 8:dd34a528a47c | 156 | tmp = list.pop(i); |
sahabi | 8:dd34a528a47c | 157 | if (dir(location,(int) tmp->data) == 1){ |
sahabi | 8:dd34a528a47c | 158 | serving = (int)tmp->data; |
sahabi | 8:dd34a528a47c | 159 | list.remove(i); |
sahabi | 8:dd34a528a47c | 160 | assigned = 1; |
sahabi | 8:dd34a528a47c | 161 | break; |
sahabi | 8:dd34a528a47c | 162 | } |
sahabi | 8:dd34a528a47c | 163 | } |
sahabi | 8:dd34a528a47c | 164 | } |
sahabi | 8:dd34a528a47c | 165 | |
sahabi | 8:dd34a528a47c | 166 | else if (globedir == 0){ |
sahabi | 9:95cbc05bd81d | 167 | for (i = 1; i<= list.length();i++){ |
sahabi | 8:dd34a528a47c | 168 | tmp = list.pop(i); |
sahabi | 9:95cbc05bd81d | 169 | if (dir(location,(int) tmp->data) == 0){ |
sahabi | 9:95cbc05bd81d | 170 | serving = (int)tmp->data; |
sahabi | 9:95cbc05bd81d | 171 | list.remove(i); |
sahabi | 9:95cbc05bd81d | 172 | assigned = 1; |
sahabi | 9:95cbc05bd81d | 173 | break; |
sahabi | 8:dd34a528a47c | 174 | } |
sahabi | 8:dd34a528a47c | 175 | } |
sahabi | 8:dd34a528a47c | 176 | } |
sahabi | 8:dd34a528a47c | 177 | if (assigned != 1){ |
sahabi | 9:95cbc05bd81d | 178 | tmp = list.pop(1); |
sahabi | 8:dd34a528a47c | 179 | serving = (int)tmp->data; |
sahabi | 9:95cbc05bd81d | 180 | list.remove(1); |
sahabi | 8:dd34a528a47c | 181 | } |
sahabi | 8:dd34a528a47c | 182 | |
sahabi | 8:dd34a528a47c | 183 | |
sahabi | 8:dd34a528a47c | 184 | if (location > serving){ |
sahabi | 9:95cbc05bd81d | 185 | pc.printf("serving: %d \n",serving); |
sahabi | 9:95cbc05bd81d | 186 | globedir = 0; |
sahabi | 1:28120714ad80 | 187 | closeDoor(); |
sahabi | 1:28120714ad80 | 188 | moveDown(); |
sahabi | 1:28120714ad80 | 189 | busy = 1; |
sahabi | 4:fba6c97a6192 | 190 | sensor.fall(&serve); |
sahabi | 1:28120714ad80 | 191 | } |
sahabi | 1:28120714ad80 | 192 | |
sahabi | 8:dd34a528a47c | 193 | else if (location < serving ){ |
sahabi | 9:95cbc05bd81d | 194 | pc.printf("serving: %d \n",serving); |
sahabi | 9:95cbc05bd81d | 195 | globedir = 1; |
sahabi | 1:28120714ad80 | 196 | closeDoor(); |
sahabi | 1:28120714ad80 | 197 | moveUp(); |
sahabi | 1:28120714ad80 | 198 | busy = 1; |
sahabi | 4:fba6c97a6192 | 199 | sensor.fall(&serve); |
sahabi | 1:28120714ad80 | 200 | } |
sahabi | 8:dd34a528a47c | 201 | |
sahabi | 0:69be3faae0a0 | 202 | } |
sahabi | 8:dd34a528a47c | 203 | } |
sahabi | 8:dd34a528a47c | 204 | |
sahabi | 0:69be3faae0a0 | 205 | int main() { |
sahabi | 0:69be3faae0a0 | 206 | servo1.period(0.02f); |
sahabi | 0:69be3faae0a0 | 207 | servo2.period(0.02f); |
sahabi | 0:69be3faae0a0 | 208 | in1.output(); |
sahabi | 0:69be3faae0a0 | 209 | in2.output(); |
sahabi | 0:69be3faae0a0 | 210 | in2 = 0; |
sahabi | 7:9ff7df122ff6 | 211 | in1 = 0; |
sahabi | 8:dd34a528a47c | 212 | location = 2; |
sahabi | 3:19106c977df1 | 213 | while(1){ |
sahabi | 4:fba6c97a6192 | 214 | |
sahabi | 9:95cbc05bd81d | 215 | while(list.length() <= 4){ |
sahabi | 4:fba6c97a6192 | 216 | |
sahabi | 6:1f9baf8d833e | 217 | if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor1 = 1;} //1 |
sahabi | 6:1f9baf8d833e | 218 | else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor1 = 2;}//2 |
sahabi | 6:1f9baf8d833e | 219 | else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor1 = 3;} //3 |
sahabi | 6:1f9baf8d833e | 220 | else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor1 = 4;} //4 |
sahabi | 6:1f9baf8d833e | 221 | else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor1 = 5;} //5 |
sahabi | 4:fba6c97a6192 | 222 | else{requested_floor1 = 0;} |
sahabi | 4:fba6c97a6192 | 223 | |
sahabi | 6:1f9baf8d833e | 224 | if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor2 = 1;led1 = 1;} |
sahabi | 6:1f9baf8d833e | 225 | else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor2 = 2;led2 = 1;} |
sahabi | 6:1f9baf8d833e | 226 | else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor2 = 3;led3 = 1;} |
sahabi | 6:1f9baf8d833e | 227 | else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor2 = 4; led4 = 1;} |
sahabi | 5:15bb8f6b6d85 | 228 | else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor2 = 5;} |
sahabi | 4:fba6c97a6192 | 229 | else{requested_floor2 = 0;} |
sahabi | 4:fba6c97a6192 | 230 | |
sahabi | 4:fba6c97a6192 | 231 | if (requested_floor1 != requested_floor2) {requested_floor = 0;} |
sahabi | 4:fba6c97a6192 | 232 | else{requested_floor = requested_floor1;} |
sahabi | 9:95cbc05bd81d | 233 | if ((inqueue(requested_floor) == 0 && requested_floor != 0) || list.length() > 0){ |
sahabi | 8:dd34a528a47c | 234 | call(requested_floor); |
sahabi | 8:dd34a528a47c | 235 | } |
sahabi | 7:9ff7df122ff6 | 236 | } |
sahabi | 0:69be3faae0a0 | 237 | } |
sahabi | 7:9ff7df122ff6 | 238 | } |