ese519 / Mbed 2 deprecated ESE519_Lab3_EC_v4

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v3 by ese519

main.cpp@7:f4814b9756fc, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 19:39:08 2015 +0000
Revision:
7:f4814b9756fc
Parent:
6:c6de60c953d2
Child:
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 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 7:f4814b9756fc 51 char cur_dir = '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 7:f4814b9756fc 94 /*
jfields 6:c6de60c953d2 95 q_mutex.lock();
jfields 5:5380953108bb 96 desired_floor = el_q[0];
jfields 5:5380953108bb 97 update_q();
jfields 6:c6de60c953d2 98 q_mutex.unlock();
jfields 7:f4814b9756fc 99 */
jfields 2:d7103c7e0671 100
jfields 1:e79ac0826624 101 // check if need to move
jfields 1:e79ac0826624 102 if (cur_floor != desired_floor) {
jfields 1:e79ac0826624 103
jfields 1:e79ac0826624 104 // determine direction
jfields 1:e79ac0826624 105 if (cur_floor > desired_floor) { // move down
jfields 1:e79ac0826624 106 in1 = 1;
jfields 1:e79ac0826624 107 in2 = 0;
jfields 7:f4814b9756fc 108 cur_dir = 'U';
jfields 1:e79ac0826624 109 } else { // move up
jfields 1:e79ac0826624 110 in1 = 0;
jfields 1:e79ac0826624 111 in2 = 1;
jfields 7:f4814b9756fc 112 cur_dir = 'D';
jfields 1:e79ac0826624 113 }
jfields 1:e79ac0826624 114
jfields 1:e79ac0826624 115 // start car
jfields 4:0b22363eb57c 116 servo1.write(dty_servo_close);
jfields 4:0b22363eb57c 117 servo2.write(dty_servo_close);
jfields 1:e79ac0826624 118 dc_motor.write(dty_dc);
jfields 1:e79ac0826624 119
jfields 1:e79ac0826624 120 // check IR sensors
jfields 1:e79ac0826624 121 while (cur_floor != desired_floor) {
jfields 1:e79ac0826624 122 wait(0.2);
jfields 1:e79ac0826624 123 }
jfields 1:e79ac0826624 124
jfields 1:e79ac0826624 125 // stop car
jfields 1:e79ac0826624 126 dc_motor.write(0);
jfields 0:015087d61ca1 127 }
jfields 1:e79ac0826624 128
jfields 1:e79ac0826624 129 // open door
jfields 1:e79ac0826624 130 servo1.write(dty_servo_open);
jfields 1:e79ac0826624 131 servo2.write(dty_servo_open);
jfields 7:f4814b9756fc 132
jfields 7:f4814b9756fc 133 // update queue
jfields 7:f4814b9756fc 134 q_mutex.lock();
jfields 7:f4814b9756fc 135 update_q();
jfields 7:f4814b9756fc 136 q_mutex.unlock();
jfields 7:f4814b9756fc 137 wait(1);
jfields 5:5380953108bb 138 //keyPressed = 0;
jfields 0:015087d61ca1 139 }
jfields 1:e79ac0826624 140 }
jfields 1:e79ac0826624 141
jfields 0:015087d61ca1 142 }
jfields 0:015087d61ca1 143
jfields 1:e79ac0826624 144 void get_floor() {
jfields 1:e79ac0826624 145 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 146 if (test_floor == 1) {
jfields 1:e79ac0826624 147 test_readings++;
jfields 1:e79ac0826624 148 } else {
jfields 1:e79ac0826624 149 test_readings = 0;
jfields 1:e79ac0826624 150 }
jfields 1:e79ac0826624 151 test_floor = 1;
jfields 1:e79ac0826624 152 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 153 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 154 if (test_floor == 2) {
jfields 1:e79ac0826624 155 test_readings++;
jfields 1:e79ac0826624 156 } else {
jfields 1:e79ac0826624 157 test_readings = 0;
jfields 1:e79ac0826624 158 }
jfields 1:e79ac0826624 159 test_floor = 2;
jfields 1:e79ac0826624 160 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 161 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 162 if (test_floor == 3) {
jfields 1:e79ac0826624 163 test_readings++;
jfields 1:e79ac0826624 164 } else {
jfields 1:e79ac0826624 165 test_readings = 0;
jfields 1:e79ac0826624 166 }
jfields 1:e79ac0826624 167 test_floor = 3;
jfields 1:e79ac0826624 168 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 169 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 170 if (test_floor == 4) {
jfields 1:e79ac0826624 171 test_readings++;
jfields 1:e79ac0826624 172 } else {
jfields 1:e79ac0826624 173 test_readings = 0;
jfields 1:e79ac0826624 174 }
jfields 1:e79ac0826624 175 test_floor = 4;
jfields 1:e79ac0826624 176 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 177 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 178 if (test_floor == 5) {
jfields 1:e79ac0826624 179 test_readings++;
jfields 1:e79ac0826624 180 } else {
jfields 1:e79ac0826624 181 test_readings = 0;
jfields 1:e79ac0826624 182 }
jfields 1:e79ac0826624 183 test_floor = 5;
jfields 1:e79ac0826624 184 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 185 }
jfields 0:015087d61ca1 186 }
jfields 1:e79ac0826624 187
jfields 1:e79ac0826624 188 void get_period() {
jfields 1:e79ac0826624 189 count++;
jfields 1:e79ac0826624 190 if (count == 1)
jfields 1:e79ac0826624 191 t.start();
jfields 1:e79ac0826624 192 else if (count == 2) {
jfields 1:e79ac0826624 193 t.stop();
jfields 1:e79ac0826624 194 floor_period = t.read_us();
jfields 1:e79ac0826624 195 t.reset();
jfields 1:e79ac0826624 196 get_floor();
jfields 1:e79ac0826624 197 count = 0;
jfields 1:e79ac0826624 198 }
jfields 1:e79ac0826624 199 }
jfields 1:e79ac0826624 200
jfields 1:e79ac0826624 201 void led_update() {
jfields 1:e79ac0826624 202 if (cur_floor == 1) {
jfields 1:e79ac0826624 203 led1 = 1;
jfields 1:e79ac0826624 204 led2 = 0;
jfields 1:e79ac0826624 205 led3 = 0;
jfields 1:e79ac0826624 206 led4 = 0;
jfields 1:e79ac0826624 207 }
jfields 1:e79ac0826624 208 if (cur_floor == 2) {
jfields 1:e79ac0826624 209 led2 = 1;
jfields 1:e79ac0826624 210 led1 = 0;
jfields 1:e79ac0826624 211 led3 = 0;
jfields 1:e79ac0826624 212 led4 = 0;
jfields 1:e79ac0826624 213 }
jfields 1:e79ac0826624 214 if (cur_floor == 3) {
jfields 1:e79ac0826624 215 led3 = 1;
jfields 1:e79ac0826624 216 led2 = 0;
jfields 1:e79ac0826624 217 led1 = 0;
jfields 1:e79ac0826624 218 led4 = 0;
jfields 1:e79ac0826624 219 }
jfields 1:e79ac0826624 220 if (cur_floor == 4) {
jfields 1:e79ac0826624 221 led4 = 1;
jfields 1:e79ac0826624 222 led2 = 0;
jfields 1:e79ac0826624 223 led3 = 0;
jfields 1:e79ac0826624 224 led1 = 0;
jfields 1:e79ac0826624 225 }
jfields 1:e79ac0826624 226 if (cur_floor == 5) {
jfields 1:e79ac0826624 227 led4 = 1;
jfields 1:e79ac0826624 228 led1 = 1;
jfields 1:e79ac0826624 229 led2 = 0;
jfields 1:e79ac0826624 230 led3 = 0;
jfields 1:e79ac0826624 231 }
jfields 1:e79ac0826624 232 }
jfields 1:e79ac0826624 233
jfields 1:e79ac0826624 234 void bpc_func(void const *args) {
jfields 1:e79ac0826624 235 while (1) {
jfields 1:e79ac0826624 236 float ADC_val = Button.read();
jfields 1:e79ac0826624 237 int val = ADC_val*10;
jfields 6:c6de60c953d2 238 q_mutex.lock();
jfields 1:e79ac0826624 239 if(val == 2) {
jfields 5:5380953108bb 240 el_q[el_q_size] = 1;
jfields 5:5380953108bb 241 el_q_size++;
jfields 7:f4814b9756fc 242 bubble_sort();
jfields 1:e79ac0826624 243 } else if(val == 4) {
jfields 5:5380953108bb 244 el_q[el_q_size] = 2;
jfields 5:5380953108bb 245 el_q_size++;
jfields 7:f4814b9756fc 246 bubble_sort();
jfields 1:e79ac0826624 247 } else if(val == 6) {
jfields 5:5380953108bb 248 el_q[el_q_size] = 3;
jfields 5:5380953108bb 249 el_q_size++;
jfields 7:f4814b9756fc 250 bubble_sort();
jfields 1:e79ac0826624 251 } else if(val == 8) {
jfields 5:5380953108bb 252 el_q[el_q_size] = 4;
jfields 5:5380953108bb 253 el_q_size++;
jfields 7:f4814b9756fc 254 bubble_sort();
jfields 1:e79ac0826624 255 } else if(val == 10) {
jfields 5:5380953108bb 256 el_q[el_q_size] = 5;
jfields 5:5380953108bb 257 el_q_size++;
jfields 7:f4814b9756fc 258 bubble_sort();
jfields 1:e79ac0826624 259 }
jfields 6:c6de60c953d2 260 q_mutex.unlock();
jfields 1:e79ac0826624 261 wait(0.1);
jfields 5:5380953108bb 262 while (el_q_size == 4) { // wait for queue to empty
jfields 5:5380953108bb 263 wait(0.2);
jfields 2:d7103c7e0671 264 }
jfields 1:e79ac0826624 265 }
jfields 5:5380953108bb 266 }
jfields 5:5380953108bb 267
jfields 5:5380953108bb 268 void update_q() {
jfields 5:5380953108bb 269 for (int i=1;i<el_q_size;i++) {
jfields 5:5380953108bb 270 el_q[i-1] = el_q[i];
jfields 5:5380953108bb 271 }
jfields 5:5380953108bb 272 el_q_size--;
jfields 5:5380953108bb 273 }
jfields 6:c6de60c953d2 274
jfields 6:c6de60c953d2 275 void el_alg() {
jfields 7:f4814b9756fc 276 int n = el_q_size;
jfields 7:f4814b9756fc 277 if (n>1) {
jfields 7:f4814b9756fc 278 int index;
jfields 7:f4814b9756fc 279 int temp;
jfields 7:f4814b9756fc 280 for (int i=0;i<n;i++) {
jfields 7:f4814b9756fc 281 if (el_q[i] > cur_floor) {
jfields 6:c6de60c953d2 282 index = i;
jfields 6:c6de60c953d2 283 break;
jfields 6:c6de60c953d2 284 }
jfields 6:c6de60c953d2 285 }
jfields 7:f4814b9756fc 286 if (cur_dir == 'U') {
jfields 7:f4814b9756fc 287 for (int i=index;i<n;i++) {
jfields 7:f4814b9756fc 288 temp = el_q[i-index];
jfields 7:f4814b9756fc 289 el_q[i-index] = el_q[i];
jfields 7:f4814b9756fc 290 el_q[i] = temp;
jfields 6:c6de60c953d2 291 }
jfields 6:c6de60c953d2 292 }
jfields 7:f4814b9756fc 293 if (cur_dir == 'D') {
jfields 7:f4814b9756fc 294 for (int i=0;i<index-1;i++) {
jfields 7:f4814b9756fc 295 temp = el_q[i];
jfields 7:f4814b9756fc 296 el_q[i] = el_q[index-i-1];
jfields 7:f4814b9756fc 297 el_q[index-i-1] = temp;
jfields 6:c6de60c953d2 298 }
jfields 6:c6de60c953d2 299 }
jfields 6:c6de60c953d2 300 }
jfields 7:f4814b9756fc 301 desired_floor = el_q[0];
jfields 6:c6de60c953d2 302 }
jfields 7:f4814b9756fc 303
jfields 7:f4814b9756fc 304
jfields 7:f4814b9756fc 305 void bubble_sort() {
jfields 7:f4814b9756fc 306 int n = el_q_size;
jfields 7:f4814b9756fc 307 bool sorted = false;
jfields 7:f4814b9756fc 308 while (!sorted) {
jfields 7:f4814b9756fc 309 sorted = true;
jfields 7:f4814b9756fc 310 for (int i=0;i<n-1;i++) {
jfields 7:f4814b9756fc 311 for (int j=i;j<n;j++) {
jfields 7:f4814b9756fc 312 if (el_q[i] > el_q[j]) {
jfields 7:f4814b9756fc 313 int temp = el_q[j];
jfields 7:f4814b9756fc 314 el_q[j] = el_q[i];
jfields 7:f4814b9756fc 315 el_q[i] = temp;
jfields 7:f4814b9756fc 316 sorted = false;
jfields 7:f4814b9756fc 317 }
jfields 7:f4814b9756fc 318 }
jfields 7:f4814b9756fc 319 }
jfields 7:f4814b9756fc 320 }
jfields 7:f4814b9756fc 321 el_alg();
jfields 7:f4814b9756fc 322 }
jfields 7:f4814b9756fc 323
jfields 7:f4814b9756fc 324
jfields 7:f4814b9756fc 325