Hemanth Kumar Lekkala
/
Lab4
final
main.cpp@0:9de45e2c32dc, 2015-03-31 (annotated)
- Committer:
- snehanayak16
- Date:
- Tue Mar 31 03:43:30 2015 +0000
- Revision:
- 0:9de45e2c32dc
- Child:
- 1:d4b4c89e4a43
Done!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
snehanayak16 | 0:9de45e2c32dc | 1 | // Test |
snehanayak16 | 0:9de45e2c32dc | 2 | |
snehanayak16 | 0:9de45e2c32dc | 3 | #include "mbed.h" |
snehanayak16 | 0:9de45e2c32dc | 4 | #include "Motor.h" |
snehanayak16 | 0:9de45e2c32dc | 5 | #include <vector> |
snehanayak16 | 0:9de45e2c32dc | 6 | |
snehanayak16 | 0:9de45e2c32dc | 7 | PwmOut servo1(p21); // Left motor |
snehanayak16 | 0:9de45e2c32dc | 8 | PwmOut servo2(p22); // Right motor |
snehanayak16 | 0:9de45e2c32dc | 9 | |
snehanayak16 | 0:9de45e2c32dc | 10 | AnalogIn a_pin(p20); |
snehanayak16 | 0:9de45e2c32dc | 11 | |
snehanayak16 | 0:9de45e2c32dc | 12 | using namespace std; |
snehanayak16 | 0:9de45e2c32dc | 13 | Timer period_t; |
snehanayak16 | 0:9de45e2c32dc | 14 | |
snehanayak16 | 0:9de45e2c32dc | 15 | InterruptIn pin(p24); |
snehanayak16 | 0:9de45e2c32dc | 16 | |
snehanayak16 | 0:9de45e2c32dc | 17 | Motor m(p23, p6, p7); |
snehanayak16 | 0:9de45e2c32dc | 18 | |
snehanayak16 | 0:9de45e2c32dc | 19 | Serial pc(USBTX, USBRX); |
snehanayak16 | 0:9de45e2c32dc | 20 | vector<int> request; |
snehanayak16 | 0:9de45e2c32dc | 21 | // Scanning Keypad |
snehanayak16 | 0:9de45e2c32dc | 22 | DigitalOut row2(p11); |
snehanayak16 | 0:9de45e2c32dc | 23 | //DigitalOut row3(p12); |
snehanayak16 | 0:9de45e2c32dc | 24 | //DigitalOut row4(p14); |
snehanayak16 | 0:9de45e2c32dc | 25 | DigitalOut row1(p18); |
snehanayak16 | 0:9de45e2c32dc | 26 | |
snehanayak16 | 0:9de45e2c32dc | 27 | InterruptIn column1(p13); |
snehanayak16 | 0:9de45e2c32dc | 28 | InterruptIn column2(p15); |
snehanayak16 | 0:9de45e2c32dc | 29 | InterruptIn column3(p16); |
snehanayak16 | 0:9de45e2c32dc | 30 | //DigitalIn column4(p17); |
snehanayak16 | 0:9de45e2c32dc | 31 | Ticker t_scan; |
snehanayak16 | 0:9de45e2c32dc | 32 | |
snehanayak16 | 0:9de45e2c32dc | 33 | Ticker rowsTick; |
snehanayak16 | 0:9de45e2c32dc | 34 | |
snehanayak16 | 0:9de45e2c32dc | 35 | int flag=0; |
snehanayak16 | 0:9de45e2c32dc | 36 | float freq,period; |
snehanayak16 | 0:9de45e2c32dc | 37 | int count = 0; |
snehanayak16 | 0:9de45e2c32dc | 38 | int i=0; |
snehanayak16 | 0:9de45e2c32dc | 39 | int numfloor,diff; // Floor number from keypad (input) |
snehanayak16 | 0:9de45e2c32dc | 40 | int curfloor; |
snehanayak16 | 0:9de45e2c32dc | 41 | |
snehanayak16 | 0:9de45e2c32dc | 42 | int move; // Up = 1 , down = 0 |
snehanayak16 | 0:9de45e2c32dc | 43 | |
snehanayak16 | 0:9de45e2c32dc | 44 | void sort(){ |
snehanayak16 | 0:9de45e2c32dc | 45 | int num; |
snehanayak16 | 0:9de45e2c32dc | 46 | int n = request.size(); |
snehanayak16 | 0:9de45e2c32dc | 47 | for(int l=0; l<n; l++){ |
snehanayak16 | 0:9de45e2c32dc | 48 | for(int m=0; m < n-l-1; m++){ |
snehanayak16 | 0:9de45e2c32dc | 49 | if(request[m] > request[m+1]){ |
snehanayak16 | 0:9de45e2c32dc | 50 | num = request[m]; |
snehanayak16 | 0:9de45e2c32dc | 51 | request[m] = request[m+1]; |
snehanayak16 | 0:9de45e2c32dc | 52 | request[m+1] = num; |
snehanayak16 | 0:9de45e2c32dc | 53 | } |
snehanayak16 | 0:9de45e2c32dc | 54 | } |
snehanayak16 | 0:9de45e2c32dc | 55 | } |
snehanayak16 | 0:9de45e2c32dc | 56 | //pc.printf("Queue : "); |
snehanayak16 | 0:9de45e2c32dc | 57 | /* |
snehanayak16 | 0:9de45e2c32dc | 58 | for(int m = 0; m<n; m++){ |
snehanayak16 | 0:9de45e2c32dc | 59 | pc.printf("%d ",request[m]); |
snehanayak16 | 0:9de45e2c32dc | 60 | } */ |
snehanayak16 | 0:9de45e2c32dc | 61 | //pc.printf("\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 62 | } |
snehanayak16 | 0:9de45e2c32dc | 63 | |
snehanayak16 | 0:9de45e2c32dc | 64 | void door_oc(){ |
snehanayak16 | 0:9de45e2c32dc | 65 | pc.printf("On floor : %d \r\n",numfloor); |
snehanayak16 | 0:9de45e2c32dc | 66 | pc.printf("Door Opening\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 67 | servo1.pulsewidth(0.00075); |
snehanayak16 | 0:9de45e2c32dc | 68 | servo2.pulsewidth(0.00075); |
snehanayak16 | 0:9de45e2c32dc | 69 | |
snehanayak16 | 0:9de45e2c32dc | 70 | wait(2); |
snehanayak16 | 0:9de45e2c32dc | 71 | |
snehanayak16 | 0:9de45e2c32dc | 72 | pc.printf("Door Closing\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 73 | servo1.pulsewidth(0.00225); |
snehanayak16 | 0:9de45e2c32dc | 74 | servo2.pulsewidth(0.00225); |
snehanayak16 | 0:9de45e2c32dc | 75 | |
snehanayak16 | 0:9de45e2c32dc | 76 | } |
snehanayak16 | 0:9de45e2c32dc | 77 | |
snehanayak16 | 0:9de45e2c32dc | 78 | |
snehanayak16 | 0:9de45e2c32dc | 79 | int exist(int number) |
snehanayak16 | 0:9de45e2c32dc | 80 | { |
snehanayak16 | 0:9de45e2c32dc | 81 | |
snehanayak16 | 0:9de45e2c32dc | 82 | for(int t=0;t<request.size();t++) |
snehanayak16 | 0:9de45e2c32dc | 83 | { |
snehanayak16 | 0:9de45e2c32dc | 84 | if(number == request[t]) |
snehanayak16 | 0:9de45e2c32dc | 85 | { |
snehanayak16 | 0:9de45e2c32dc | 86 | return 1; |
snehanayak16 | 0:9de45e2c32dc | 87 | } |
snehanayak16 | 0:9de45e2c32dc | 88 | } |
snehanayak16 | 0:9de45e2c32dc | 89 | return 0; |
snehanayak16 | 0:9de45e2c32dc | 90 | } |
snehanayak16 | 0:9de45e2c32dc | 91 | |
snehanayak16 | 0:9de45e2c32dc | 92 | // Scanning Keypad |
snehanayak16 | 0:9de45e2c32dc | 93 | |
snehanayak16 | 0:9de45e2c32dc | 94 | void scan(){ |
snehanayak16 | 0:9de45e2c32dc | 95 | flag++; |
snehanayak16 | 0:9de45e2c32dc | 96 | if(flag>2){ |
snehanayak16 | 0:9de45e2c32dc | 97 | flag = 1; |
snehanayak16 | 0:9de45e2c32dc | 98 | } |
snehanayak16 | 0:9de45e2c32dc | 99 | switch(flag){ |
snehanayak16 | 0:9de45e2c32dc | 100 | case 1 : row2 = 1;row1 = 0; break; |
snehanayak16 | 0:9de45e2c32dc | 101 | case 2 : row1 = 1;row2 = 0; break; |
snehanayak16 | 0:9de45e2c32dc | 102 | default : break; |
snehanayak16 | 0:9de45e2c32dc | 103 | } |
snehanayak16 | 0:9de45e2c32dc | 104 | |
snehanayak16 | 0:9de45e2c32dc | 105 | } |
snehanayak16 | 0:9de45e2c32dc | 106 | void fall1(){ |
snehanayak16 | 0:9de45e2c32dc | 107 | if(flag == 1) |
snehanayak16 | 0:9de45e2c32dc | 108 | { |
snehanayak16 | 0:9de45e2c32dc | 109 | int c1 = exist(1); |
snehanayak16 | 0:9de45e2c32dc | 110 | if( c1 == 0 && request.size()<4) |
snehanayak16 | 0:9de45e2c32dc | 111 | { |
snehanayak16 | 0:9de45e2c32dc | 112 | request.push_back(1); |
snehanayak16 | 0:9de45e2c32dc | 113 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 114 | //pc.printf("Buff size %d\r\n",request.size()); |
snehanayak16 | 0:9de45e2c32dc | 115 | pc.printf("You typed : 1\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 116 | } |
snehanayak16 | 0:9de45e2c32dc | 117 | } |
snehanayak16 | 0:9de45e2c32dc | 118 | else if(flag == 2) |
snehanayak16 | 0:9de45e2c32dc | 119 | { |
snehanayak16 | 0:9de45e2c32dc | 120 | int c4 = exist(4); |
snehanayak16 | 0:9de45e2c32dc | 121 | if( c4 == 0 && request.size()<4) |
snehanayak16 | 0:9de45e2c32dc | 122 | { |
snehanayak16 | 0:9de45e2c32dc | 123 | request.push_back(4); |
snehanayak16 | 0:9de45e2c32dc | 124 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 125 | //pc.printf("Buff size %d\r\n",request.size()); |
snehanayak16 | 0:9de45e2c32dc | 126 | pc.printf("You typed : 4\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 127 | } |
snehanayak16 | 0:9de45e2c32dc | 128 | } |
snehanayak16 | 0:9de45e2c32dc | 129 | } |
snehanayak16 | 0:9de45e2c32dc | 130 | void fall2(){ |
snehanayak16 | 0:9de45e2c32dc | 131 | if(flag == 1) |
snehanayak16 | 0:9de45e2c32dc | 132 | { |
snehanayak16 | 0:9de45e2c32dc | 133 | int c2 = exist(2); |
snehanayak16 | 0:9de45e2c32dc | 134 | if( c2 == 0 && request.size()<4) |
snehanayak16 | 0:9de45e2c32dc | 135 | { |
snehanayak16 | 0:9de45e2c32dc | 136 | request.push_back(2); |
snehanayak16 | 0:9de45e2c32dc | 137 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 138 | //pc.printf("Buff size %d\r\n",request.size()); |
snehanayak16 | 0:9de45e2c32dc | 139 | pc.printf("You typed : 2\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 140 | } |
snehanayak16 | 0:9de45e2c32dc | 141 | } |
snehanayak16 | 0:9de45e2c32dc | 142 | else if(flag == 2) |
snehanayak16 | 0:9de45e2c32dc | 143 | { |
snehanayak16 | 0:9de45e2c32dc | 144 | int c5 = exist(5); |
snehanayak16 | 0:9de45e2c32dc | 145 | if( c5 == 0 && request.size()<4) |
snehanayak16 | 0:9de45e2c32dc | 146 | { |
snehanayak16 | 0:9de45e2c32dc | 147 | request.push_back(5); |
snehanayak16 | 0:9de45e2c32dc | 148 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 149 | //pc.printf("Buff size %d\r\n",request.size()); |
snehanayak16 | 0:9de45e2c32dc | 150 | pc.printf("You typed : 5\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 151 | } |
snehanayak16 | 0:9de45e2c32dc | 152 | } |
snehanayak16 | 0:9de45e2c32dc | 153 | } |
snehanayak16 | 0:9de45e2c32dc | 154 | void fall3(){ |
snehanayak16 | 0:9de45e2c32dc | 155 | if(flag == 1) |
snehanayak16 | 0:9de45e2c32dc | 156 | { |
snehanayak16 | 0:9de45e2c32dc | 157 | int c3 = exist(3); |
snehanayak16 | 0:9de45e2c32dc | 158 | if( c3 == 0 && request.size()<4) |
snehanayak16 | 0:9de45e2c32dc | 159 | { |
snehanayak16 | 0:9de45e2c32dc | 160 | request.push_back(3); |
snehanayak16 | 0:9de45e2c32dc | 161 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 162 | //pc.printf("Buff size %d\r\n",request.size()); |
snehanayak16 | 0:9de45e2c32dc | 163 | pc.printf("You typed : 3\r\n"); |
snehanayak16 | 0:9de45e2c32dc | 164 | } |
snehanayak16 | 0:9de45e2c32dc | 165 | } |
snehanayak16 | 0:9de45e2c32dc | 166 | } |
snehanayak16 | 0:9de45e2c32dc | 167 | |
snehanayak16 | 0:9de45e2c32dc | 168 | |
snehanayak16 | 0:9de45e2c32dc | 169 | void direction(){ |
snehanayak16 | 0:9de45e2c32dc | 170 | for(int k = 0;k<request.size();k++){ |
snehanayak16 | 0:9de45e2c32dc | 171 | if(request[k] == curfloor){ |
snehanayak16 | 0:9de45e2c32dc | 172 | numfloor = curfloor; |
snehanayak16 | 0:9de45e2c32dc | 173 | door_oc(); |
snehanayak16 | 0:9de45e2c32dc | 174 | request.erase(request.begin()+k); |
snehanayak16 | 0:9de45e2c32dc | 175 | break; |
snehanayak16 | 0:9de45e2c32dc | 176 | } |
snehanayak16 | 0:9de45e2c32dc | 177 | } |
snehanayak16 | 0:9de45e2c32dc | 178 | if(request.size()>0){ |
snehanayak16 | 0:9de45e2c32dc | 179 | int max = request[request.size()-1]; |
snehanayak16 | 0:9de45e2c32dc | 180 | if(max > curfloor){ |
snehanayak16 | 0:9de45e2c32dc | 181 | move = 1; |
snehanayak16 | 0:9de45e2c32dc | 182 | } |
snehanayak16 | 0:9de45e2c32dc | 183 | else if(max < curfloor){ |
snehanayak16 | 0:9de45e2c32dc | 184 | move = 0; |
snehanayak16 | 0:9de45e2c32dc | 185 | } |
snehanayak16 | 0:9de45e2c32dc | 186 | } |
snehanayak16 | 0:9de45e2c32dc | 187 | } |
snehanayak16 | 0:9de45e2c32dc | 188 | |
snehanayak16 | 0:9de45e2c32dc | 189 | |
snehanayak16 | 0:9de45e2c32dc | 190 | int floor_detect(){ |
snehanayak16 | 0:9de45e2c32dc | 191 | |
snehanayak16 | 0:9de45e2c32dc | 192 | if(freq > 90.0 && freq < 110.0){ |
snehanayak16 | 0:9de45e2c32dc | 193 | curfloor = 1; |
snehanayak16 | 0:9de45e2c32dc | 194 | pc.printf("Floor : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 195 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 196 | } |
snehanayak16 | 0:9de45e2c32dc | 197 | else if(freq > 190.0 && freq < 210.0){ |
snehanayak16 | 0:9de45e2c32dc | 198 | curfloor = 2; |
snehanayak16 | 0:9de45e2c32dc | 199 | pc.printf("Floor : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 200 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 201 | } |
snehanayak16 | 0:9de45e2c32dc | 202 | else if(freq > 290.0 && freq < 310.0){ |
snehanayak16 | 0:9de45e2c32dc | 203 | curfloor = 3; |
snehanayak16 | 0:9de45e2c32dc | 204 | pc.printf("Floor : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 205 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 206 | } |
snehanayak16 | 0:9de45e2c32dc | 207 | else if(freq > 390.0 && freq < 410.0){ |
snehanayak16 | 0:9de45e2c32dc | 208 | curfloor = 4; |
snehanayak16 | 0:9de45e2c32dc | 209 | pc.printf("Floor : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 210 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 211 | } |
snehanayak16 | 0:9de45e2c32dc | 212 | else if(freq > 490.0 && freq < 510.0){ |
snehanayak16 | 0:9de45e2c32dc | 213 | curfloor = 5; |
snehanayak16 | 0:9de45e2c32dc | 214 | pc.printf("Floor : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 215 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 216 | } |
snehanayak16 | 0:9de45e2c32dc | 217 | |
snehanayak16 | 0:9de45e2c32dc | 218 | |
snehanayak16 | 0:9de45e2c32dc | 219 | |
snehanayak16 | 0:9de45e2c32dc | 220 | return curfloor; |
snehanayak16 | 0:9de45e2c32dc | 221 | } |
snehanayak16 | 0:9de45e2c32dc | 222 | |
snehanayak16 | 0:9de45e2c32dc | 223 | |
snehanayak16 | 0:9de45e2c32dc | 224 | |
snehanayak16 | 0:9de45e2c32dc | 225 | void rise_edge(){ |
snehanayak16 | 0:9de45e2c32dc | 226 | |
snehanayak16 | 0:9de45e2c32dc | 227 | if(a_pin.read()>0.95){ |
snehanayak16 | 0:9de45e2c32dc | 228 | count ++; |
snehanayak16 | 0:9de45e2c32dc | 229 | if(count == 1){ |
snehanayak16 | 0:9de45e2c32dc | 230 | period_t.start(); |
snehanayak16 | 0:9de45e2c32dc | 231 | } |
snehanayak16 | 0:9de45e2c32dc | 232 | else if(count == 2){ |
snehanayak16 | 0:9de45e2c32dc | 233 | period_t.stop(); |
snehanayak16 | 0:9de45e2c32dc | 234 | period = period_t.read(); |
snehanayak16 | 0:9de45e2c32dc | 235 | freq = 1.0/period; |
snehanayak16 | 0:9de45e2c32dc | 236 | period_t.reset(); |
snehanayak16 | 0:9de45e2c32dc | 237 | count = 0; |
snehanayak16 | 0:9de45e2c32dc | 238 | } |
snehanayak16 | 0:9de45e2c32dc | 239 | } |
snehanayak16 | 0:9de45e2c32dc | 240 | if(freq > 0.0){ |
snehanayak16 | 0:9de45e2c32dc | 241 | //pc.printf("Freq: %3.2f\r\n",freq); |
snehanayak16 | 0:9de45e2c32dc | 242 | } |
snehanayak16 | 0:9de45e2c32dc | 243 | } |
snehanayak16 | 0:9de45e2c32dc | 244 | |
snehanayak16 | 0:9de45e2c32dc | 245 | int get_index(int floor){ |
snehanayak16 | 0:9de45e2c32dc | 246 | for(int k = 0;k<request.size();k++){ |
snehanayak16 | 0:9de45e2c32dc | 247 | if(request[k]==floor){ |
snehanayak16 | 0:9de45e2c32dc | 248 | return k; |
snehanayak16 | 0:9de45e2c32dc | 249 | } |
snehanayak16 | 0:9de45e2c32dc | 250 | } |
snehanayak16 | 0:9de45e2c32dc | 251 | return -1; |
snehanayak16 | 0:9de45e2c32dc | 252 | } |
snehanayak16 | 0:9de45e2c32dc | 253 | |
snehanayak16 | 0:9de45e2c32dc | 254 | int main() { |
snehanayak16 | 0:9de45e2c32dc | 255 | |
snehanayak16 | 0:9de45e2c32dc | 256 | t_scan.attach(&scan,0.004); |
snehanayak16 | 0:9de45e2c32dc | 257 | column1.fall(&fall1); |
snehanayak16 | 0:9de45e2c32dc | 258 | column2.fall(&fall2); |
snehanayak16 | 0:9de45e2c32dc | 259 | column3.fall(&fall3); |
snehanayak16 | 0:9de45e2c32dc | 260 | |
snehanayak16 | 0:9de45e2c32dc | 261 | servo1.period(0.020); |
snehanayak16 | 0:9de45e2c32dc | 262 | |
snehanayak16 | 0:9de45e2c32dc | 263 | //request.push_back(5); |
snehanayak16 | 0:9de45e2c32dc | 264 | //request.push_back(4); |
snehanayak16 | 0:9de45e2c32dc | 265 | //request.push_back(3); |
snehanayak16 | 0:9de45e2c32dc | 266 | //request.push_back(1); |
snehanayak16 | 0:9de45e2c32dc | 267 | |
snehanayak16 | 0:9de45e2c32dc | 268 | /* |
snehanayak16 | 0:9de45e2c32dc | 269 | row1 = 0; //row initialization |
snehanayak16 | 0:9de45e2c32dc | 270 | row2 = 0; |
snehanayak16 | 0:9de45e2c32dc | 271 | */ |
snehanayak16 | 0:9de45e2c32dc | 272 | pc.printf("Start freq : %d\r\n", freq); |
snehanayak16 | 0:9de45e2c32dc | 273 | pin.rise(&rise_edge); |
snehanayak16 | 0:9de45e2c32dc | 274 | /* |
snehanayak16 | 0:9de45e2c32dc | 275 | while(!(freq>90.0 && freq <110.0)){ |
snehanayak16 | 0:9de45e2c32dc | 276 | m.speed(-1.0); |
snehanayak16 | 0:9de45e2c32dc | 277 | } |
snehanayak16 | 0:9de45e2c32dc | 278 | m.speed(0.0); |
snehanayak16 | 0:9de45e2c32dc | 279 | */ |
snehanayak16 | 0:9de45e2c32dc | 280 | pc.printf("Start Now ............................. \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 281 | while(1){ |
snehanayak16 | 0:9de45e2c32dc | 282 | |
snehanayak16 | 0:9de45e2c32dc | 283 | //sort(); |
snehanayak16 | 0:9de45e2c32dc | 284 | while(request.size()!=0){ |
snehanayak16 | 0:9de45e2c32dc | 285 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 286 | curfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 287 | direction(); |
snehanayak16 | 0:9de45e2c32dc | 288 | if(move == 1){ |
snehanayak16 | 0:9de45e2c32dc | 289 | for(int k = 0;k<request.size();k++){ |
snehanayak16 | 0:9de45e2c32dc | 290 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 291 | numfloor = request[k]; |
snehanayak16 | 0:9de45e2c32dc | 292 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 293 | while(diff > 0){ |
snehanayak16 | 0:9de45e2c32dc | 294 | //pc.printf("DC ON +1 \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 295 | m.speed(1.0); |
snehanayak16 | 0:9de45e2c32dc | 296 | //pc.printf("Service for : %d \r\n",numfloor); |
snehanayak16 | 0:9de45e2c32dc | 297 | //pc.printf("Now at : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 298 | //pc.printf("Going Up \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 299 | curfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 300 | if(exist(curfloor)){ |
snehanayak16 | 0:9de45e2c32dc | 301 | numfloor = curfloor; |
snehanayak16 | 0:9de45e2c32dc | 302 | } |
snehanayak16 | 0:9de45e2c32dc | 303 | int f_index = get_index(curfloor); |
snehanayak16 | 0:9de45e2c32dc | 304 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 305 | if(diff == 0 && f_index!= -1){ |
snehanayak16 | 0:9de45e2c32dc | 306 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 307 | pc.printf("DC OFF \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 308 | m.speed(0.0); |
snehanayak16 | 0:9de45e2c32dc | 309 | request.erase(request.begin()+f_index); |
snehanayak16 | 0:9de45e2c32dc | 310 | k--; |
snehanayak16 | 0:9de45e2c32dc | 311 | door_oc(); |
snehanayak16 | 0:9de45e2c32dc | 312 | } |
snehanayak16 | 0:9de45e2c32dc | 313 | } |
snehanayak16 | 0:9de45e2c32dc | 314 | } |
snehanayak16 | 0:9de45e2c32dc | 315 | |
snehanayak16 | 0:9de45e2c32dc | 316 | |
snehanayak16 | 0:9de45e2c32dc | 317 | } |
snehanayak16 | 0:9de45e2c32dc | 318 | else if(move == 0){ |
snehanayak16 | 0:9de45e2c32dc | 319 | for(int k = request.size()-1;k >= 0;k--){ |
snehanayak16 | 0:9de45e2c32dc | 320 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 321 | numfloor = request[k]; |
snehanayak16 | 0:9de45e2c32dc | 322 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 323 | while(diff < 0){ |
snehanayak16 | 0:9de45e2c32dc | 324 | //pc.printf("DC ON -1 \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 325 | m.speed(-1.0); |
snehanayak16 | 0:9de45e2c32dc | 326 | //pc.printf("Service for : %d \r\n",numfloor); |
snehanayak16 | 0:9de45e2c32dc | 327 | //pc.printf("Now at : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 328 | //pc.printf("Going Down \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 329 | curfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 330 | if(exist(curfloor)){ |
snehanayak16 | 0:9de45e2c32dc | 331 | numfloor = curfloor; |
snehanayak16 | 0:9de45e2c32dc | 332 | } |
snehanayak16 | 0:9de45e2c32dc | 333 | int f_index = get_index(curfloor); |
snehanayak16 | 0:9de45e2c32dc | 334 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 335 | if(diff == 0 && f_index!= -1){ |
snehanayak16 | 0:9de45e2c32dc | 336 | pc.printf("DC OFF \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 337 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 338 | m.speed(0.0); |
snehanayak16 | 0:9de45e2c32dc | 339 | request.erase(request.begin()+k); |
snehanayak16 | 0:9de45e2c32dc | 340 | k++; |
snehanayak16 | 0:9de45e2c32dc | 341 | door_oc(); |
snehanayak16 | 0:9de45e2c32dc | 342 | } |
snehanayak16 | 0:9de45e2c32dc | 343 | } |
snehanayak16 | 0:9de45e2c32dc | 344 | } |
snehanayak16 | 0:9de45e2c32dc | 345 | } |
snehanayak16 | 0:9de45e2c32dc | 346 | } |
snehanayak16 | 0:9de45e2c32dc | 347 | } |
snehanayak16 | 0:9de45e2c32dc | 348 | } |
snehanayak16 | 0:9de45e2c32dc | 349 | |
snehanayak16 | 0:9de45e2c32dc | 350 | // Move 1 |
snehanayak16 | 0:9de45e2c32dc | 351 | /* |
snehanayak16 | 0:9de45e2c32dc | 352 | for(int k = 0;k<request.size();k++){ |
snehanayak16 | 0:9de45e2c32dc | 353 | sort(); |
snehanayak16 | 0:9de45e2c32dc | 354 | numfloor = request[k]; |
snehanayak16 | 0:9de45e2c32dc | 355 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 356 | while(diff > 0){ |
snehanayak16 | 0:9de45e2c32dc | 357 | //pc.printf("DC ON +1 \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 358 | m.speed(1.0); |
snehanayak16 | 0:9de45e2c32dc | 359 | //pc.printf("Service for : %d \r\n",numfloor); |
snehanayak16 | 0:9de45e2c32dc | 360 | //pc.printf("Now at : %d \r\n",curfloor); |
snehanayak16 | 0:9de45e2c32dc | 361 | //pc.printf("Going Up \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 362 | curfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 363 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 364 | if(diff == 0){ |
snehanayak16 | 0:9de45e2c32dc | 365 | //wait(2); |
snehanayak16 | 0:9de45e2c32dc | 366 | pc.printf("DC OFF \r\n"); |
snehanayak16 | 0:9de45e2c32dc | 367 | m.speed(0.0); |
snehanayak16 | 0:9de45e2c32dc | 368 | request.erase(request.begin()+k); |
snehanayak16 | 0:9de45e2c32dc | 369 | k--; |
snehanayak16 | 0:9de45e2c32dc | 370 | door_oc(); |
snehanayak16 | 0:9de45e2c32dc | 371 | } |
snehanayak16 | 0:9de45e2c32dc | 372 | } |
snehanayak16 | 0:9de45e2c32dc | 373 | } */ |
snehanayak16 | 0:9de45e2c32dc | 374 | |
snehanayak16 | 0:9de45e2c32dc | 375 | /* |
snehanayak16 | 0:9de45e2c32dc | 376 | |
snehanayak16 | 0:9de45e2c32dc | 377 | numfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 378 | while(numfloor!=request[request.size()-1]){ |
snehanayak16 | 0:9de45e2c32dc | 379 | numfloor = floor_detect(); |
snehanayak16 | 0:9de45e2c32dc | 380 | diff = numfloor - curfloor; |
snehanayak16 | 0:9de45e2c32dc | 381 | if(diff > 0){ |
snehanayak16 | 0:9de45e2c32dc | 382 | m.speed(1.0); |
snehanayak16 | 0:9de45e2c32dc | 383 | if(exist(numfloor)){ |
snehanayak16 | 0:9de45e2c32dc | 384 | door_oc(); |
snehanayak16 | 0:9de45e2c32dc | 385 | |
snehanayak16 | 0:9de45e2c32dc | 386 | } |
snehanayak16 | 0:9de45e2c32dc | 387 | |
snehanayak16 | 0:9de45e2c32dc | 388 | } |
snehanayak16 | 0:9de45e2c32dc | 389 | |
snehanayak16 | 0:9de45e2c32dc | 390 | } */ |