ese519 / Mbed 2 deprecated ESE519_Lab3_EC_v3

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v2 by ese519

main.cpp@5:5380953108bb, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 17:41:00 2015 +0000
Revision:
5:5380953108bb
Parent:
4:0b22363eb57c
Child:
6:c6de60c953d2
ec

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 1:e79ac0826624 14
jfields 1:e79ac0826624 15 // Threads
jfields 1:e79ac0826624 16 void bpc_func(void const *args);
jfields 1:e79ac0826624 17 Thread * bpc_thread;
jfields 0:015087d61ca1 18
jfields 0:015087d61ca1 19 // init board
jfields 0:015087d61ca1 20 PwmOut dc_motor(p26);
jfields 0:015087d61ca1 21 PwmOut servo1(p25);
jfields 0:015087d61ca1 22 PwmOut servo2(p24);
jfields 1:e79ac0826624 23 DigitalOut in1(p11); // elevator direction
jfields 1:e79ac0826624 24 DigitalOut in2(p12); // elevator direction
jfields 1:e79ac0826624 25 InterruptIn IRSensor(p10); // read IR sensor
jfields 1:e79ac0826624 26 AnalogIn Button(p15); // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number
jfields 0:015087d61ca1 27
jfields 1:e79ac0826624 28 // LEDs for testing
jfields 1:e79ac0826624 29 DigitalOut led1(LED1);
jfields 1:e79ac0826624 30 DigitalOut led2(LED2);
jfields 1:e79ac0826624 31 DigitalOut led3(LED3);
jfields 1:e79ac0826624 32 DigitalOut led4(LED4);
jfields 0:015087d61ca1 33
jfields 0:015087d61ca1 34 // init vars
jfields 1:e79ac0826624 35 Timer t;
jfields 0:015087d61ca1 36 float period = 0.02; // sec
jfields 0:015087d61ca1 37 float dty_dc = 0.75; // PCT [0-100]
jfields 4:0b22363eb57c 38 float dty_servo_close = 0.1125; // PCT [3.75-11.25]
jfields 4:0b22363eb57c 39 float dty_servo_open = 0.0375; // PCT [3.75-11.25]
jfields 1:e79ac0826624 40 int cur_floor = 0;
jfields 1:e79ac0826624 41 int desired_floor = 0;
jfields 1:e79ac0826624 42 int floor_period; // for period detection
jfields 1:e79ac0826624 43 int keyPressed = 0;
jfields 1:e79ac0826624 44 int count = 0;
jfields 1:e79ac0826624 45 int test_readings = 0; // need 4 consecutive readings to declare cur_floor
jfields 1:e79ac0826624 46 int test_floor = 0;
jfields 5:5380953108bb 47 int el_q [4];
jfields 5:5380953108bb 48 int el_q_size = 0;
jfields 0:015087d61ca1 49
jfields 0:015087d61ca1 50 int main() {
jfields 1:e79ac0826624 51
jfields 1:e79ac0826624 52 // init interrupt handler
jfields 1:e79ac0826624 53 IRSensor.fall(&get_period);
jfields 1:e79ac0826624 54
jfields 5:5380953108bb 55 // init queue
jfields 5:5380953108bb 56 for (int i=0;i<4;i++) el_q[i] = 0;
jfields 5:5380953108bb 57
jfields 0:015087d61ca1 58 // set period (constant)
jfields 0:015087d61ca1 59 dc_motor.period(period);
jfields 1:e79ac0826624 60 servo1.period(period);
jfields 1:e79ac0826624 61 servo2.period(period);
jfields 1:e79ac0826624 62
jfields 1:e79ac0826624 63 // start with elevator stationary
jfields 1:e79ac0826624 64 dc_motor.write(0);
jfields 1:e79ac0826624 65
jfields 0:015087d61ca1 66 // start with closed doors
jfields 1:e79ac0826624 67 servo1.write(dty_servo_close);
jfields 1:e79ac0826624 68 servo2.write(dty_servo_close);
jfields 0:015087d61ca1 69
jfields 0:015087d61ca1 70 while(1) {
jfields 1:e79ac0826624 71
jfields 1:e79ac0826624 72 // get init floor
jfields 1:e79ac0826624 73 while (!cur_floor) { // wait for a floor to be detected
jfields 1:e79ac0826624 74 wait(0.25);
jfields 1:e79ac0826624 75 }
jfields 3:2f8e2399dfe8 76 if (!desired_floor) {
jfields 3:2f8e2399dfe8 77 desired_floor = cur_floor;
jfields 3:2f8e2399dfe8 78 bpc_thread = new Thread(bpc_func); // start button checker thread
jfields 3:2f8e2399dfe8 79 }
jfields 1:e79ac0826624 80 led_update();
jfields 1:e79ac0826624 81
jfields 0:015087d61ca1 82 // check for key press
jfields 1:e79ac0826624 83 //find_keys();
jfields 1:e79ac0826624 84
jfields 1:e79ac0826624 85 // execute elevator alg
jfields 5:5380953108bb 86 if (!el_q_size) {
jfields 1:e79ac0826624 87
jfields 5:5380953108bb 88 desired_floor = el_q[0];
jfields 5:5380953108bb 89 update_q();
jfields 2:d7103c7e0671 90
jfields 1:e79ac0826624 91 // check if need to move
jfields 1:e79ac0826624 92 if (cur_floor != desired_floor) {
jfields 1:e79ac0826624 93
jfields 1:e79ac0826624 94 // determine direction
jfields 1:e79ac0826624 95 if (cur_floor > desired_floor) { // move down
jfields 1:e79ac0826624 96 in1 = 1;
jfields 1:e79ac0826624 97 in2 = 0;
jfields 1:e79ac0826624 98 } else { // move up
jfields 1:e79ac0826624 99 in1 = 0;
jfields 1:e79ac0826624 100 in2 = 1;
jfields 1:e79ac0826624 101 }
jfields 1:e79ac0826624 102
jfields 1:e79ac0826624 103 // start car
jfields 4:0b22363eb57c 104 servo1.write(dty_servo_close);
jfields 4:0b22363eb57c 105 servo2.write(dty_servo_close);
jfields 1:e79ac0826624 106 dc_motor.write(dty_dc);
jfields 1:e79ac0826624 107
jfields 1:e79ac0826624 108 // check IR sensors
jfields 1:e79ac0826624 109 while (cur_floor != desired_floor) {
jfields 1:e79ac0826624 110 wait(0.2);
jfields 1:e79ac0826624 111 }
jfields 1:e79ac0826624 112
jfields 1:e79ac0826624 113 // stop car
jfields 1:e79ac0826624 114 dc_motor.write(0);
jfields 0:015087d61ca1 115 }
jfields 1:e79ac0826624 116
jfields 1:e79ac0826624 117 // open door
jfields 1:e79ac0826624 118 servo1.write(dty_servo_open);
jfields 1:e79ac0826624 119 servo2.write(dty_servo_open);
jfields 5:5380953108bb 120 wait(2);
jfields 5:5380953108bb 121 //keyPressed = 0;
jfields 0:015087d61ca1 122 }
jfields 1:e79ac0826624 123 }
jfields 1:e79ac0826624 124
jfields 0:015087d61ca1 125 }
jfields 0:015087d61ca1 126
jfields 1:e79ac0826624 127 void get_floor() {
jfields 1:e79ac0826624 128 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 129 if (test_floor == 1) {
jfields 1:e79ac0826624 130 test_readings++;
jfields 1:e79ac0826624 131 } else {
jfields 1:e79ac0826624 132 test_readings = 0;
jfields 1:e79ac0826624 133 }
jfields 1:e79ac0826624 134 test_floor = 1;
jfields 1:e79ac0826624 135 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 136 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 137 if (test_floor == 2) {
jfields 1:e79ac0826624 138 test_readings++;
jfields 1:e79ac0826624 139 } else {
jfields 1:e79ac0826624 140 test_readings = 0;
jfields 1:e79ac0826624 141 }
jfields 1:e79ac0826624 142 test_floor = 2;
jfields 1:e79ac0826624 143 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 144 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 145 if (test_floor == 3) {
jfields 1:e79ac0826624 146 test_readings++;
jfields 1:e79ac0826624 147 } else {
jfields 1:e79ac0826624 148 test_readings = 0;
jfields 1:e79ac0826624 149 }
jfields 1:e79ac0826624 150 test_floor = 3;
jfields 1:e79ac0826624 151 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 152 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 153 if (test_floor == 4) {
jfields 1:e79ac0826624 154 test_readings++;
jfields 1:e79ac0826624 155 } else {
jfields 1:e79ac0826624 156 test_readings = 0;
jfields 1:e79ac0826624 157 }
jfields 1:e79ac0826624 158 test_floor = 4;
jfields 1:e79ac0826624 159 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 160 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 161 if (test_floor == 5) {
jfields 1:e79ac0826624 162 test_readings++;
jfields 1:e79ac0826624 163 } else {
jfields 1:e79ac0826624 164 test_readings = 0;
jfields 1:e79ac0826624 165 }
jfields 1:e79ac0826624 166 test_floor = 5;
jfields 1:e79ac0826624 167 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 168 }
jfields 0:015087d61ca1 169 }
jfields 1:e79ac0826624 170
jfields 1:e79ac0826624 171 void get_period() {
jfields 1:e79ac0826624 172 count++;
jfields 1:e79ac0826624 173 if (count == 1)
jfields 1:e79ac0826624 174 t.start();
jfields 1:e79ac0826624 175 else if (count == 2) {
jfields 1:e79ac0826624 176 t.stop();
jfields 1:e79ac0826624 177 floor_period = t.read_us();
jfields 1:e79ac0826624 178 t.reset();
jfields 1:e79ac0826624 179 get_floor();
jfields 1:e79ac0826624 180 count = 0;
jfields 1:e79ac0826624 181 }
jfields 1:e79ac0826624 182 }
jfields 1:e79ac0826624 183
jfields 1:e79ac0826624 184 void led_update() {
jfields 1:e79ac0826624 185 if (cur_floor == 1) {
jfields 1:e79ac0826624 186 led1 = 1;
jfields 1:e79ac0826624 187 led2 = 0;
jfields 1:e79ac0826624 188 led3 = 0;
jfields 1:e79ac0826624 189 led4 = 0;
jfields 1:e79ac0826624 190 }
jfields 1:e79ac0826624 191 if (cur_floor == 2) {
jfields 1:e79ac0826624 192 led2 = 1;
jfields 1:e79ac0826624 193 led1 = 0;
jfields 1:e79ac0826624 194 led3 = 0;
jfields 1:e79ac0826624 195 led4 = 0;
jfields 1:e79ac0826624 196 }
jfields 1:e79ac0826624 197 if (cur_floor == 3) {
jfields 1:e79ac0826624 198 led3 = 1;
jfields 1:e79ac0826624 199 led2 = 0;
jfields 1:e79ac0826624 200 led1 = 0;
jfields 1:e79ac0826624 201 led4 = 0;
jfields 1:e79ac0826624 202 }
jfields 1:e79ac0826624 203 if (cur_floor == 4) {
jfields 1:e79ac0826624 204 led4 = 1;
jfields 1:e79ac0826624 205 led2 = 0;
jfields 1:e79ac0826624 206 led3 = 0;
jfields 1:e79ac0826624 207 led1 = 0;
jfields 1:e79ac0826624 208 }
jfields 1:e79ac0826624 209 if (cur_floor == 5) {
jfields 1:e79ac0826624 210 led4 = 1;
jfields 1:e79ac0826624 211 led1 = 1;
jfields 1:e79ac0826624 212 led2 = 0;
jfields 1:e79ac0826624 213 led3 = 0;
jfields 1:e79ac0826624 214 }
jfields 1:e79ac0826624 215 }
jfields 1:e79ac0826624 216
jfields 1:e79ac0826624 217 void bpc_func(void const *args) {
jfields 1:e79ac0826624 218 while (1) {
jfields 1:e79ac0826624 219 float ADC_val = Button.read();
jfields 1:e79ac0826624 220 int val = ADC_val*10;
jfields 1:e79ac0826624 221 if(val == 2) {
jfields 5:5380953108bb 222 //desired_floor = 1;
jfields 5:5380953108bb 223 //keyPressed = 1;
jfields 5:5380953108bb 224 el_q[el_q_size] = 1;
jfields 5:5380953108bb 225 el_q_size++;
jfields 1:e79ac0826624 226 } else if(val == 4) {
jfields 5:5380953108bb 227 //desired_floor = 2;
jfields 5:5380953108bb 228 //keyPressed = 1;
jfields 5:5380953108bb 229 el_q[el_q_size] = 2;
jfields 5:5380953108bb 230 el_q_size++;
jfields 1:e79ac0826624 231 } else if(val == 6) {
jfields 5:5380953108bb 232 //desired_floor = 3;
jfields 5:5380953108bb 233 //keyPressed = 1;
jfields 5:5380953108bb 234 el_q[el_q_size] = 3;
jfields 5:5380953108bb 235 el_q_size++;
jfields 1:e79ac0826624 236 } else if(val == 8) {
jfields 5:5380953108bb 237 //desired_floor = 4;
jfields 5:5380953108bb 238 //keyPressed = 1;
jfields 5:5380953108bb 239 el_q[el_q_size] = 4;
jfields 5:5380953108bb 240 el_q_size++;
jfields 1:e79ac0826624 241 } else if(val == 10) {
jfields 5:5380953108bb 242 //desired_floor = 5;
jfields 5:5380953108bb 243 //keyPressed = 1;
jfields 5:5380953108bb 244 el_q[el_q_size] = 5;
jfields 5:5380953108bb 245 el_q_size++;
jfields 1:e79ac0826624 246 }
jfields 1:e79ac0826624 247 wait(0.1);
jfields 5:5380953108bb 248 while (el_q_size == 4) { // wait for queue to empty
jfields 5:5380953108bb 249 wait(0.2);
jfields 2:d7103c7e0671 250 }
jfields 1:e79ac0826624 251 }
jfields 5:5380953108bb 252 }
jfields 5:5380953108bb 253
jfields 5:5380953108bb 254 void update_q() {
jfields 5:5380953108bb 255 for (int i=1;i<el_q_size;i++) {
jfields 5:5380953108bb 256 el_q[i-1] = el_q[i];
jfields 5:5380953108bb 257 }
jfields 5:5380953108bb 258 el_q_size--;
jfields 5:5380953108bb 259 }