ese519 / Mbed 2 deprecated ESE519_Lab3_EC_v5

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v4 by ese519

main.cpp@10:680612dfabe5, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 22:18:40 2015 +0000
Revision:
10:680612dfabe5
Parent:
9:d7a9c2345e9e 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jfields 1:e79ac0826624 1 // ESE 519 Lab 3 Code //
jfields 1:e79ac0826624 2
jfields 0:015087d61ca1 3 #include "mbed.h"
jfields 2:d7103c7e0671 4 #include "rtos.h"
jfields 0:015087d61ca1 5
jfields 0:015087d61ca1 6 Serial pc(USBTX,USBRX);
jfields 1:e79ac0826624 7
jfields 0:015087d61ca1 8 // functions
jfields 1:e79ac0826624 9 void check_floor();
jfields 1:e79ac0826624 10 void get_floor(); // uses floor period to set cur_floor
jfields 1:e79ac0826624 11 void get_period();
jfields 1:e79ac0826624 12 void led_update();
jfields 5:5380953108bb 13 void update_q();
jfields 6:c6de60c953d2 14 void el_alg(); // need sorted list
jfields 7:f4814b9756fc 15 void bubble_sort();
jfields 9:d7a9c2345e9e 16 void led_q();
jfields 1:e79ac0826624 17
jfields 1:e79ac0826624 18 // Threads
jfields 1:e79ac0826624 19 void bpc_func(void const *args);
jfields 1:e79ac0826624 20 Thread * bpc_thread;
jfields 0:015087d61ca1 21
jfields 0:015087d61ca1 22 // init board
jfields 0:015087d61ca1 23 PwmOut dc_motor(p26);
jfields 0:015087d61ca1 24 PwmOut servo1(p25);
jfields 0:015087d61ca1 25 PwmOut servo2(p24);
jfields 1:e79ac0826624 26 DigitalOut in1(p11); // elevator direction
jfields 1:e79ac0826624 27 DigitalOut in2(p12); // elevator direction
jfields 1:e79ac0826624 28 InterruptIn IRSensor(p10); // read IR sensor
jfields 1:e79ac0826624 29 AnalogIn Button(p15); // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number
jfields 0:015087d61ca1 30
jfields 1:e79ac0826624 31 // LEDs for testing
jfields 1:e79ac0826624 32 DigitalOut led1(LED1);
jfields 1:e79ac0826624 33 DigitalOut led2(LED2);
jfields 1:e79ac0826624 34 DigitalOut led3(LED3);
jfields 1:e79ac0826624 35 DigitalOut led4(LED4);
jfields 0:015087d61ca1 36
jfields 0:015087d61ca1 37 // init vars
jfields 1:e79ac0826624 38 Timer t;
jfields 0:015087d61ca1 39 float period = 0.02; // sec
jfields 0:015087d61ca1 40 float dty_dc = 0.75; // PCT [0-100]
jfields 4:0b22363eb57c 41 float dty_servo_close = 0.1125; // PCT [3.75-11.25]
jfields 4:0b22363eb57c 42 float dty_servo_open = 0.0375; // PCT [3.75-11.25]
jfields 1:e79ac0826624 43 int cur_floor = 0;
jfields 1:e79ac0826624 44 int desired_floor = 0;
jfields 1:e79ac0826624 45 int floor_period; // for period detection
jfields 1:e79ac0826624 46 int keyPressed = 0;
jfields 1:e79ac0826624 47 int count = 0;
jfields 1:e79ac0826624 48 int test_readings = 0; // need 4 consecutive readings to declare cur_floor
jfields 1:e79ac0826624 49 int test_floor = 0;
jfields 5:5380953108bb 50 int el_q [4];
jfields 5:5380953108bb 51 int el_q_size = 0;
jfields 7:f4814b9756fc 52 char cur_dir = 'S'; // S = stationary, U = up, D = down
jfields 6:c6de60c953d2 53
jfields 6:c6de60c953d2 54 // mutex
jfields 6:c6de60c953d2 55 Mutex q_mutex; // protect el_q
jfields 0:015087d61ca1 56
jfields 0:015087d61ca1 57 int main() {
jfields 1:e79ac0826624 58
jfields 1:e79ac0826624 59 // init interrupt handler
jfields 1:e79ac0826624 60 IRSensor.fall(&get_period);
jfields 1:e79ac0826624 61
jfields 5:5380953108bb 62 // init queue
jfields 5:5380953108bb 63 for (int i=0;i<4;i++) el_q[i] = 0;
jfields 5:5380953108bb 64
jfields 0:015087d61ca1 65 // set period (constant)
jfields 0:015087d61ca1 66 dc_motor.period(period);
jfields 1:e79ac0826624 67 servo1.period(period);
jfields 1:e79ac0826624 68 servo2.period(period);
jfields 1:e79ac0826624 69
jfields 1:e79ac0826624 70 // start with elevator stationary
jfields 1:e79ac0826624 71 dc_motor.write(0);
jfields 1:e79ac0826624 72
jfields 0:015087d61ca1 73 // start with closed doors
jfields 1:e79ac0826624 74 servo1.write(dty_servo_close);
jfields 1:e79ac0826624 75 servo2.write(dty_servo_close);
jfields 0:015087d61ca1 76
jfields 0:015087d61ca1 77 while(1) {
jfields 1:e79ac0826624 78
jfields 1:e79ac0826624 79 // get init floor
jfields 1:e79ac0826624 80 while (!cur_floor) { // wait for a floor to be detected
jfields 1:e79ac0826624 81 wait(0.25);
jfields 1:e79ac0826624 82 }
jfields 3:2f8e2399dfe8 83 if (!desired_floor) {
jfields 3:2f8e2399dfe8 84 desired_floor = cur_floor;
jfields 9:d7a9c2345e9e 85 led_update();
jfields 3:2f8e2399dfe8 86 bpc_thread = new Thread(bpc_func); // start button checker thread
jfields 3:2f8e2399dfe8 87 }
jfields 1:e79ac0826624 88
jfields 0:015087d61ca1 89 // check for key press
jfields 1:e79ac0826624 90 //find_keys();
jfields 1:e79ac0826624 91
jfields 1:e79ac0826624 92 // execute elevator alg
jfields 8:4b0f6f68db12 93 if (el_q_size) {
jfields 1:e79ac0826624 94
jfields 7:f4814b9756fc 95 /*
jfields 6:c6de60c953d2 96 q_mutex.lock();
jfields 5:5380953108bb 97 desired_floor = el_q[0];
jfields 5:5380953108bb 98 update_q();
jfields 6:c6de60c953d2 99 q_mutex.unlock();
jfields 7:f4814b9756fc 100 */
jfields 2:d7103c7e0671 101
jfields 1:e79ac0826624 102 // check if need to move
jfields 1:e79ac0826624 103 if (cur_floor != desired_floor) {
jfields 1:e79ac0826624 104
jfields 1:e79ac0826624 105 // determine direction
jfields 1:e79ac0826624 106 if (cur_floor > desired_floor) { // move down
jfields 1:e79ac0826624 107 in1 = 1;
jfields 1:e79ac0826624 108 in2 = 0;
jfields 7:f4814b9756fc 109 cur_dir = 'U';
jfields 1:e79ac0826624 110 } else { // move up
jfields 1:e79ac0826624 111 in1 = 0;
jfields 1:e79ac0826624 112 in2 = 1;
jfields 7:f4814b9756fc 113 cur_dir = 'D';
jfields 1:e79ac0826624 114 }
jfields 1:e79ac0826624 115
jfields 1:e79ac0826624 116 // start car
jfields 4:0b22363eb57c 117 servo1.write(dty_servo_close);
jfields 4:0b22363eb57c 118 servo2.write(dty_servo_close);
jfields 1:e79ac0826624 119 dc_motor.write(dty_dc);
jfields 1:e79ac0826624 120
jfields 1:e79ac0826624 121 // check IR sensors
jfields 1:e79ac0826624 122 while (cur_floor != desired_floor) {
jfields 1:e79ac0826624 123 wait(0.2);
jfields 1:e79ac0826624 124 }
jfields 1:e79ac0826624 125
jfields 1:e79ac0826624 126 // stop car
jfields 1:e79ac0826624 127 dc_motor.write(0);
jfields 0:015087d61ca1 128 }
jfields 1:e79ac0826624 129
jfields 1:e79ac0826624 130 // open door
jfields 1:e79ac0826624 131 servo1.write(dty_servo_open);
jfields 1:e79ac0826624 132 servo2.write(dty_servo_open);
jfields 7:f4814b9756fc 133
jfields 7:f4814b9756fc 134 // update queue
jfields 7:f4814b9756fc 135 q_mutex.lock();
jfields 7:f4814b9756fc 136 update_q();
jfields 7:f4814b9756fc 137 q_mutex.unlock();
jfields 10:680612dfabe5 138 led_q();
jfields 7:f4814b9756fc 139 wait(1);
jfields 5:5380953108bb 140 //keyPressed = 0;
jfields 0:015087d61ca1 141 }
jfields 1:e79ac0826624 142 }
jfields 1:e79ac0826624 143
jfields 0:015087d61ca1 144 }
jfields 0:015087d61ca1 145
jfields 1:e79ac0826624 146 void get_floor() {
jfields 1:e79ac0826624 147 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 148 if (test_floor == 1) {
jfields 1:e79ac0826624 149 test_readings++;
jfields 1:e79ac0826624 150 } else {
jfields 1:e79ac0826624 151 test_readings = 0;
jfields 1:e79ac0826624 152 }
jfields 1:e79ac0826624 153 test_floor = 1;
jfields 1:e79ac0826624 154 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 155 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 156 if (test_floor == 2) {
jfields 1:e79ac0826624 157 test_readings++;
jfields 1:e79ac0826624 158 } else {
jfields 1:e79ac0826624 159 test_readings = 0;
jfields 1:e79ac0826624 160 }
jfields 1:e79ac0826624 161 test_floor = 2;
jfields 1:e79ac0826624 162 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 163 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 164 if (test_floor == 3) {
jfields 1:e79ac0826624 165 test_readings++;
jfields 1:e79ac0826624 166 } else {
jfields 1:e79ac0826624 167 test_readings = 0;
jfields 1:e79ac0826624 168 }
jfields 1:e79ac0826624 169 test_floor = 3;
jfields 1:e79ac0826624 170 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 171 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 172 if (test_floor == 4) {
jfields 1:e79ac0826624 173 test_readings++;
jfields 1:e79ac0826624 174 } else {
jfields 1:e79ac0826624 175 test_readings = 0;
jfields 1:e79ac0826624 176 }
jfields 1:e79ac0826624 177 test_floor = 4;
jfields 1:e79ac0826624 178 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 179 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 180 if (test_floor == 5) {
jfields 1:e79ac0826624 181 test_readings++;
jfields 1:e79ac0826624 182 } else {
jfields 1:e79ac0826624 183 test_readings = 0;
jfields 1:e79ac0826624 184 }
jfields 1:e79ac0826624 185 test_floor = 5;
jfields 1:e79ac0826624 186 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 187 }
jfields 0:015087d61ca1 188 }
jfields 1:e79ac0826624 189
jfields 1:e79ac0826624 190 void get_period() {
jfields 1:e79ac0826624 191 count++;
jfields 1:e79ac0826624 192 if (count == 1)
jfields 1:e79ac0826624 193 t.start();
jfields 1:e79ac0826624 194 else if (count == 2) {
jfields 1:e79ac0826624 195 t.stop();
jfields 1:e79ac0826624 196 floor_period = t.read_us();
jfields 1:e79ac0826624 197 t.reset();
jfields 1:e79ac0826624 198 get_floor();
jfields 1:e79ac0826624 199 count = 0;
jfields 1:e79ac0826624 200 }
jfields 1:e79ac0826624 201 }
jfields 1:e79ac0826624 202
jfields 1:e79ac0826624 203 void led_update() {
jfields 1:e79ac0826624 204 if (cur_floor == 1) {
jfields 1:e79ac0826624 205 led1 = 1;
jfields 1:e79ac0826624 206 led2 = 0;
jfields 1:e79ac0826624 207 led3 = 0;
jfields 1:e79ac0826624 208 led4 = 0;
jfields 1:e79ac0826624 209 }
jfields 1:e79ac0826624 210 if (cur_floor == 2) {
jfields 1:e79ac0826624 211 led2 = 1;
jfields 1:e79ac0826624 212 led1 = 0;
jfields 1:e79ac0826624 213 led3 = 0;
jfields 1:e79ac0826624 214 led4 = 0;
jfields 1:e79ac0826624 215 }
jfields 1:e79ac0826624 216 if (cur_floor == 3) {
jfields 1:e79ac0826624 217 led3 = 1;
jfields 1:e79ac0826624 218 led2 = 0;
jfields 1:e79ac0826624 219 led1 = 0;
jfields 1:e79ac0826624 220 led4 = 0;
jfields 1:e79ac0826624 221 }
jfields 1:e79ac0826624 222 if (cur_floor == 4) {
jfields 1:e79ac0826624 223 led4 = 1;
jfields 1:e79ac0826624 224 led2 = 0;
jfields 1:e79ac0826624 225 led3 = 0;
jfields 1:e79ac0826624 226 led1 = 0;
jfields 1:e79ac0826624 227 }
jfields 1:e79ac0826624 228 if (cur_floor == 5) {
jfields 1:e79ac0826624 229 led4 = 1;
jfields 1:e79ac0826624 230 led1 = 1;
jfields 1:e79ac0826624 231 led2 = 0;
jfields 1:e79ac0826624 232 led3 = 0;
jfields 1:e79ac0826624 233 }
jfields 1:e79ac0826624 234 }
jfields 1:e79ac0826624 235
jfields 1:e79ac0826624 236 void bpc_func(void const *args) {
jfields 1:e79ac0826624 237 while (1) {
jfields 1:e79ac0826624 238 float ADC_val = Button.read();
jfields 1:e79ac0826624 239 int val = ADC_val*10;
jfields 10:680612dfabe5 240
jfields 10:680612dfabe5 241 int has1 = 0;
jfields 10:680612dfabe5 242 int has2 = 0;
jfields 10:680612dfabe5 243 int has3 = 0;
jfields 10:680612dfabe5 244 int has4 = 0;
jfields 10:680612dfabe5 245 int has5 = 0;
jfields 10:680612dfabe5 246 for (int i=0;i<4;i++) {
jfields 10:680612dfabe5 247 if (el_q[i] == 1) has1 = 1;
jfields 10:680612dfabe5 248 if (el_q[i] == 2) has2 = 1;
jfields 10:680612dfabe5 249 if (el_q[i] == 3) has3 = 1;
jfields 10:680612dfabe5 250 if (el_q[i] == 4) has4 = 1;
jfields 10:680612dfabe5 251 if (el_q[i] == 5) has5 = 1;
jfields 10:680612dfabe5 252 }
jfields 10:680612dfabe5 253
jfields 10:680612dfabe5 254
jfields 6:c6de60c953d2 255 q_mutex.lock();
jfields 10:680612dfabe5 256 if (val == 2 && !has1) {
jfields 5:5380953108bb 257 el_q[el_q_size] = 1;
jfields 5:5380953108bb 258 el_q_size++;
jfields 7:f4814b9756fc 259 bubble_sort();
jfields 10:680612dfabe5 260 } else if(val == 4 && !has2) {
jfields 5:5380953108bb 261 el_q[el_q_size] = 2;
jfields 5:5380953108bb 262 el_q_size++;
jfields 7:f4814b9756fc 263 bubble_sort();
jfields 10:680612dfabe5 264 } else if(val == 6 && !has3) {
jfields 5:5380953108bb 265 el_q[el_q_size] = 3;
jfields 5:5380953108bb 266 el_q_size++;
jfields 7:f4814b9756fc 267 bubble_sort();
jfields 10:680612dfabe5 268 } else if(val == 8 && !has4) {
jfields 5:5380953108bb 269 el_q[el_q_size] = 4;
jfields 5:5380953108bb 270 el_q_size++;
jfields 7:f4814b9756fc 271 bubble_sort();
jfields 10:680612dfabe5 272 } else if(val == 10 && !has5) {
jfields 5:5380953108bb 273 el_q[el_q_size] = 5;
jfields 5:5380953108bb 274 el_q_size++;
jfields 7:f4814b9756fc 275 bubble_sort();
jfields 1:e79ac0826624 276 }
jfields 6:c6de60c953d2 277 q_mutex.unlock();
jfields 10:680612dfabe5 278 wait(0.5);
jfields 5:5380953108bb 279 while (el_q_size == 4) { // wait for queue to empty
jfields 5:5380953108bb 280 wait(0.2);
jfields 2:d7103c7e0671 281 }
jfields 1:e79ac0826624 282 }
jfields 5:5380953108bb 283 }
jfields 5:5380953108bb 284
jfields 5:5380953108bb 285 void update_q() {
jfields 10:680612dfabe5 286 el_q_size--;
jfields 10:680612dfabe5 287 for (int i=0;i<el_q_size;i++) {
jfields 10:680612dfabe5 288 el_q[i] = el_q[i+1];
jfields 5:5380953108bb 289 }
jfields 5:5380953108bb 290 }
jfields 6:c6de60c953d2 291
jfields 6:c6de60c953d2 292 void el_alg() {
jfields 7:f4814b9756fc 293 int n = el_q_size;
jfields 7:f4814b9756fc 294 if (n>1) {
jfields 7:f4814b9756fc 295 int index;
jfields 7:f4814b9756fc 296 int temp;
jfields 7:f4814b9756fc 297 for (int i=0;i<n;i++) {
jfields 7:f4814b9756fc 298 if (el_q[i] > cur_floor) {
jfields 6:c6de60c953d2 299 index = i;
jfields 6:c6de60c953d2 300 break;
jfields 6:c6de60c953d2 301 }
jfields 6:c6de60c953d2 302 }
jfields 7:f4814b9756fc 303 if (cur_dir == 'U') {
jfields 7:f4814b9756fc 304 for (int i=index;i<n;i++) {
jfields 7:f4814b9756fc 305 temp = el_q[i-index];
jfields 7:f4814b9756fc 306 el_q[i-index] = el_q[i];
jfields 7:f4814b9756fc 307 el_q[i] = temp;
jfields 6:c6de60c953d2 308 }
jfields 6:c6de60c953d2 309 }
jfields 7:f4814b9756fc 310 if (cur_dir == 'D') {
jfields 7:f4814b9756fc 311 for (int i=0;i<index-1;i++) {
jfields 7:f4814b9756fc 312 temp = el_q[i];
jfields 7:f4814b9756fc 313 el_q[i] = el_q[index-i-1];
jfields 7:f4814b9756fc 314 el_q[index-i-1] = temp;
jfields 6:c6de60c953d2 315 }
jfields 6:c6de60c953d2 316 }
jfields 6:c6de60c953d2 317 }
jfields 7:f4814b9756fc 318 desired_floor = el_q[0];
jfields 9:d7a9c2345e9e 319 led_q();
jfields 6:c6de60c953d2 320 }
jfields 7:f4814b9756fc 321
jfields 7:f4814b9756fc 322
jfields 7:f4814b9756fc 323 void bubble_sort() {
jfields 7:f4814b9756fc 324 int n = el_q_size;
jfields 7:f4814b9756fc 325 bool sorted = false;
jfields 7:f4814b9756fc 326 while (!sorted) {
jfields 7:f4814b9756fc 327 sorted = true;
jfields 7:f4814b9756fc 328 for (int i=0;i<n-1;i++) {
jfields 7:f4814b9756fc 329 for (int j=i;j<n;j++) {
jfields 7:f4814b9756fc 330 if (el_q[i] > el_q[j]) {
jfields 7:f4814b9756fc 331 int temp = el_q[j];
jfields 7:f4814b9756fc 332 el_q[j] = el_q[i];
jfields 7:f4814b9756fc 333 el_q[i] = temp;
jfields 7:f4814b9756fc 334 sorted = false;
jfields 7:f4814b9756fc 335 }
jfields 7:f4814b9756fc 336 }
jfields 7:f4814b9756fc 337 }
jfields 7:f4814b9756fc 338 }
jfields 7:f4814b9756fc 339 el_alg();
jfields 7:f4814b9756fc 340 }
jfields 7:f4814b9756fc 341
jfields 9:d7a9c2345e9e 342 void led_q() {
jfields 9:d7a9c2345e9e 343 led1 = 0;
jfields 9:d7a9c2345e9e 344 led2 = 0;
jfields 9:d7a9c2345e9e 345 led3 = 0;
jfields 9:d7a9c2345e9e 346 led4 = 0;
jfields 9:d7a9c2345e9e 347 if (el_q_size >= 1) led1 = 1;
jfields 9:d7a9c2345e9e 348 if (el_q_size >= 2) led2 = 1;
jfields 9:d7a9c2345e9e 349 if (el_q_size >= 3) led3 = 1;
jfields 9:d7a9c2345e9e 350 if (el_q_size >= 4) led4 = 1;
jfields 9:d7a9c2345e9e 351 }
jfields 7:f4814b9756fc 352