ese519 / Mbed 2 deprecated ESE519_Lab3_EC_v4

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v3 by ese519

main.cpp@9:d7a9c2345e9e, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 20:13:24 2015 +0000
Revision:
9:d7a9c2345e9e
Parent:
8:4b0f6f68db12 
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 7:f4814b9756fc 138 wait(1);
jfields 5:5380953108bb 139 //keyPressed = 0;
jfields 0:015087d61ca1 140 }
jfields 1:e79ac0826624 141 }
jfields 1:e79ac0826624 142
jfields 0:015087d61ca1 143 }
jfields 0:015087d61ca1 144
jfields 1:e79ac0826624 145 void get_floor() {
jfields 1:e79ac0826624 146 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 147 if (test_floor == 1) {
jfields 1:e79ac0826624 148 test_readings++;
jfields 1:e79ac0826624 149 } else {
jfields 1:e79ac0826624 150 test_readings = 0;
jfields 1:e79ac0826624 151 }
jfields 1:e79ac0826624 152 test_floor = 1;
jfields 1:e79ac0826624 153 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 154 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 155 if (test_floor == 2) {
jfields 1:e79ac0826624 156 test_readings++;
jfields 1:e79ac0826624 157 } else {
jfields 1:e79ac0826624 158 test_readings = 0;
jfields 1:e79ac0826624 159 }
jfields 1:e79ac0826624 160 test_floor = 2;
jfields 1:e79ac0826624 161 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 162 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 163 if (test_floor == 3) {
jfields 1:e79ac0826624 164 test_readings++;
jfields 1:e79ac0826624 165 } else {
jfields 1:e79ac0826624 166 test_readings = 0;
jfields 1:e79ac0826624 167 }
jfields 1:e79ac0826624 168 test_floor = 3;
jfields 1:e79ac0826624 169 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 170 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 171 if (test_floor == 4) {
jfields 1:e79ac0826624 172 test_readings++;
jfields 1:e79ac0826624 173 } else {
jfields 1:e79ac0826624 174 test_readings = 0;
jfields 1:e79ac0826624 175 }
jfields 1:e79ac0826624 176 test_floor = 4;
jfields 1:e79ac0826624 177 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 178 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 179 if (test_floor == 5) {
jfields 1:e79ac0826624 180 test_readings++;
jfields 1:e79ac0826624 181 } else {
jfields 1:e79ac0826624 182 test_readings = 0;
jfields 1:e79ac0826624 183 }
jfields 1:e79ac0826624 184 test_floor = 5;
jfields 1:e79ac0826624 185 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 186 }
jfields 0:015087d61ca1 187 }
jfields 1:e79ac0826624 188
jfields 1:e79ac0826624 189 void get_period() {
jfields 1:e79ac0826624 190 count++;
jfields 1:e79ac0826624 191 if (count == 1)
jfields 1:e79ac0826624 192 t.start();
jfields 1:e79ac0826624 193 else if (count == 2) {
jfields 1:e79ac0826624 194 t.stop();
jfields 1:e79ac0826624 195 floor_period = t.read_us();
jfields 1:e79ac0826624 196 t.reset();
jfields 1:e79ac0826624 197 get_floor();
jfields 1:e79ac0826624 198 count = 0;
jfields 1:e79ac0826624 199 }
jfields 1:e79ac0826624 200 }
jfields 1:e79ac0826624 201
jfields 1:e79ac0826624 202 void led_update() {
jfields 1:e79ac0826624 203 if (cur_floor == 1) {
jfields 1:e79ac0826624 204 led1 = 1;
jfields 1:e79ac0826624 205 led2 = 0;
jfields 1:e79ac0826624 206 led3 = 0;
jfields 1:e79ac0826624 207 led4 = 0;
jfields 1:e79ac0826624 208 }
jfields 1:e79ac0826624 209 if (cur_floor == 2) {
jfields 1:e79ac0826624 210 led2 = 1;
jfields 1:e79ac0826624 211 led1 = 0;
jfields 1:e79ac0826624 212 led3 = 0;
jfields 1:e79ac0826624 213 led4 = 0;
jfields 1:e79ac0826624 214 }
jfields 1:e79ac0826624 215 if (cur_floor == 3) {
jfields 1:e79ac0826624 216 led3 = 1;
jfields 1:e79ac0826624 217 led2 = 0;
jfields 1:e79ac0826624 218 led1 = 0;
jfields 1:e79ac0826624 219 led4 = 0;
jfields 1:e79ac0826624 220 }
jfields 1:e79ac0826624 221 if (cur_floor == 4) {
jfields 1:e79ac0826624 222 led4 = 1;
jfields 1:e79ac0826624 223 led2 = 0;
jfields 1:e79ac0826624 224 led3 = 0;
jfields 1:e79ac0826624 225 led1 = 0;
jfields 1:e79ac0826624 226 }
jfields 1:e79ac0826624 227 if (cur_floor == 5) {
jfields 1:e79ac0826624 228 led4 = 1;
jfields 1:e79ac0826624 229 led1 = 1;
jfields 1:e79ac0826624 230 led2 = 0;
jfields 1:e79ac0826624 231 led3 = 0;
jfields 1:e79ac0826624 232 }
jfields 1:e79ac0826624 233 }
jfields 1:e79ac0826624 234
jfields 1:e79ac0826624 235 void bpc_func(void const *args) {
jfields 1:e79ac0826624 236 while (1) {
jfields 1:e79ac0826624 237 float ADC_val = Button.read();
jfields 1:e79ac0826624 238 int val = ADC_val*10;
jfields 6:c6de60c953d2 239 q_mutex.lock();
jfields 1:e79ac0826624 240 if(val == 2) {
jfields 5:5380953108bb 241 el_q[el_q_size] = 1;
jfields 5:5380953108bb 242 el_q_size++;
jfields 7:f4814b9756fc 243 bubble_sort();
jfields 1:e79ac0826624 244 } else if(val == 4) {
jfields 5:5380953108bb 245 el_q[el_q_size] = 2;
jfields 5:5380953108bb 246 el_q_size++;
jfields 7:f4814b9756fc 247 bubble_sort();
jfields 1:e79ac0826624 248 } else if(val == 6) {
jfields 5:5380953108bb 249 el_q[el_q_size] = 3;
jfields 5:5380953108bb 250 el_q_size++;
jfields 7:f4814b9756fc 251 bubble_sort();
jfields 1:e79ac0826624 252 } else if(val == 8) {
jfields 5:5380953108bb 253 el_q[el_q_size] = 4;
jfields 5:5380953108bb 254 el_q_size++;
jfields 7:f4814b9756fc 255 bubble_sort();
jfields 1:e79ac0826624 256 } else if(val == 10) {
jfields 5:5380953108bb 257 el_q[el_q_size] = 5;
jfields 5:5380953108bb 258 el_q_size++;
jfields 7:f4814b9756fc 259 bubble_sort();
jfields 1:e79ac0826624 260 }
jfields 6:c6de60c953d2 261 q_mutex.unlock();
jfields 1:e79ac0826624 262 wait(0.1);
jfields 5:5380953108bb 263 while (el_q_size == 4) { // wait for queue to empty
jfields 5:5380953108bb 264 wait(0.2);
jfields 2:d7103c7e0671 265 }
jfields 1:e79ac0826624 266 }
jfields 5:5380953108bb 267 }
jfields 5:5380953108bb 268
jfields 5:5380953108bb 269 void update_q() {
jfields 5:5380953108bb 270 for (int i=1;i<el_q_size;i++) {
jfields 5:5380953108bb 271 el_q[i-1] = el_q[i];
jfields 5:5380953108bb 272 }
jfields 5:5380953108bb 273 el_q_size--;
jfields 5:5380953108bb 274 }
jfields 6:c6de60c953d2 275
jfields 6:c6de60c953d2 276 void el_alg() {
jfields 7:f4814b9756fc 277 int n = el_q_size;
jfields 7:f4814b9756fc 278 if (n>1) {
jfields 7:f4814b9756fc 279 int index;
jfields 7:f4814b9756fc 280 int temp;
jfields 7:f4814b9756fc 281 for (int i=0;i<n;i++) {
jfields 7:f4814b9756fc 282 if (el_q[i] > cur_floor) {
jfields 6:c6de60c953d2 283 index = i;
jfields 6:c6de60c953d2 284 break;
jfields 6:c6de60c953d2 285 }
jfields 6:c6de60c953d2 286 }
jfields 7:f4814b9756fc 287 if (cur_dir == 'U') {
jfields 7:f4814b9756fc 288 for (int i=index;i<n;i++) {
jfields 7:f4814b9756fc 289 temp = el_q[i-index];
jfields 7:f4814b9756fc 290 el_q[i-index] = el_q[i];
jfields 7:f4814b9756fc 291 el_q[i] = temp;
jfields 6:c6de60c953d2 292 }
jfields 6:c6de60c953d2 293 }
jfields 7:f4814b9756fc 294 if (cur_dir == 'D') {
jfields 7:f4814b9756fc 295 for (int i=0;i<index-1;i++) {
jfields 7:f4814b9756fc 296 temp = el_q[i];
jfields 7:f4814b9756fc 297 el_q[i] = el_q[index-i-1];
jfields 7:f4814b9756fc 298 el_q[index-i-1] = temp;
jfields 6:c6de60c953d2 299 }
jfields 6:c6de60c953d2 300 }
jfields 6:c6de60c953d2 301 }
jfields 7:f4814b9756fc 302 desired_floor = el_q[0];
jfields 9:d7a9c2345e9e 303 led_q();
jfields 6:c6de60c953d2 304 }
jfields 7:f4814b9756fc 305
jfields 7:f4814b9756fc 306
jfields 7:f4814b9756fc 307 void bubble_sort() {
jfields 7:f4814b9756fc 308 int n = el_q_size;
jfields 7:f4814b9756fc 309 bool sorted = false;
jfields 7:f4814b9756fc 310 while (!sorted) {
jfields 7:f4814b9756fc 311 sorted = true;
jfields 7:f4814b9756fc 312 for (int i=0;i<n-1;i++) {
jfields 7:f4814b9756fc 313 for (int j=i;j<n;j++) {
jfields 7:f4814b9756fc 314 if (el_q[i] > el_q[j]) {
jfields 7:f4814b9756fc 315 int temp = el_q[j];
jfields 7:f4814b9756fc 316 el_q[j] = el_q[i];
jfields 7:f4814b9756fc 317 el_q[i] = temp;
jfields 7:f4814b9756fc 318 sorted = false;
jfields 7:f4814b9756fc 319 }
jfields 7:f4814b9756fc 320 }
jfields 7:f4814b9756fc 321 }
jfields 7:f4814b9756fc 322 }
jfields 7:f4814b9756fc 323 el_alg();
jfields 7:f4814b9756fc 324 }
jfields 7:f4814b9756fc 325
jfields 9:d7a9c2345e9e 326 void led_q() {
jfields 9:d7a9c2345e9e 327 led1 = 0;
jfields 9:d7a9c2345e9e 328 led2 = 0;
jfields 9:d7a9c2345e9e 329 led3 = 0;
jfields 9:d7a9c2345e9e 330 led4 = 0;
jfields 9:d7a9c2345e9e 331 if (el_q_size >= 1) led1 = 1;
jfields 9:d7a9c2345e9e 332 if (el_q_size >= 2) led2 = 1;
jfields 9:d7a9c2345e9e 333 if (el_q_size >= 3) led3 = 1;
jfields 9:d7a9c2345e9e 334 if (el_q_size >= 4) led4 = 1;
jfields 9:d7a9c2345e9e 335 }
jfields 7:f4814b9756fc 336