ese519 / Mbed 2 deprecated ESE519_Lab3_v4

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_v3 by ese519

main.cpp@3:2f8e2399dfe8, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 17:01:02 2015 +0000
Revision:
3:2f8e2399dfe8
Parent:
2:d7103c7e0671
Child:
4:0b22363eb57c
aa

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