ese519 / Mbed 2 deprecated ESE519_Lab3_EC_v4

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v3 by ese519

main.cpp@6:c6de60c953d2, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 18:48:07 2015 +0000
Revision:
6:c6de60c953d2
Parent:
5:5380953108bb
Child:
7:f4814b9756fc
ec v2

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