Mohammed Alshiekh / Mbed 2 deprecated elevator_with_queue

course work

Dependencies:   LinkedList mbed

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?

UserRevisionLine numberNew 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 }