ese519 / Mbed 2 deprecated ESE519_Lab3_v4

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_v3 by ese519

main.cpp@4:0b22363eb57c, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 17:22:40 2015 +0000
Revision:
4:0b22363eb57c
Parent:
3:2f8e2399dfe8 
q

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 4:0b22363eb57c 47 float dty_servo_close = 0.1125; // PCT [3.75-11.25]
jfields 4:0b22363eb57c 48 float dty_servo_open = 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 4:0b22363eb57c 110 servo1.write(dty_servo_close);
jfields 4:0b22363eb57c 111 servo2.write(dty_servo_close);
jfields 1:e79ac0826624 112 dc_motor.write(dty_dc);
jfields 1:e79ac0826624 113
jfields 1:e79ac0826624 114 // check IR sensors
jfields 1:e79ac0826624 115 while (cur_floor != desired_floor) {
jfields 1:e79ac0826624 116 wait(0.2);
jfields 1:e79ac0826624 117 }
jfields 1:e79ac0826624 118
jfields 1:e79ac0826624 119 // stop car
jfields 1:e79ac0826624 120 dc_motor.write(0);
jfields 0:015087d61ca1 121 }
jfields 1:e79ac0826624 122
jfields 1:e79ac0826624 123 // open door
jfields 1:e79ac0826624 124 servo1.write(dty_servo_open);
jfields 1:e79ac0826624 125 servo2.write(dty_servo_open);
jfields 1:e79ac0826624 126 keyPressed = 0;
jfields 0:015087d61ca1 127 }
jfields 1:e79ac0826624 128 }
jfields 1:e79ac0826624 129
jfields 0:015087d61ca1 130 }
jfields 0:015087d61ca1 131
jfields 1:e79ac0826624 132 /*
jfields 0:015087d61ca1 133 void find_keys() {
jfields 1:e79ac0826624 134
jfields 0:015087d61ca1 135 // check row 1
jfields 0:015087d61ca1 136 row1 = 0;
jfields 0:015087d61ca1 137 row2 = 1;
jfields 0:015087d61ca1 138 if (!col1) {
jfields 1:e79ac0826624 139 desired_floor = 1;
jfields 1:e79ac0826624 140 keyPressed = 1;
jfields 0:015087d61ca1 141 }
jfields 0:015087d61ca1 142 if (!col2) {
jfields 1:e79ac0826624 143 desired_floor = 2;
jfields 1:e79ac0826624 144 keyPressed = 1;
jfields 0:015087d61ca1 145 }
jfields 0:015087d61ca1 146 if (!col3) {
jfields 1:e79ac0826624 147 desired_floor = 3;
jfields 1:e79ac0826624 148 keyPressed = 1;
jfields 0:015087d61ca1 149 }
jfields 1:e79ac0826624 150
jfields 0:015087d61ca1 151 // check row 2
jfields 0:015087d61ca1 152 row1 = 1;
jfields 0:015087d61ca1 153 row2 = 0;
jfields 0:015087d61ca1 154 if (!col1) {
jfields 0:015087d61ca1 155 desired_floor = 4;
jfields 1:e79ac0826624 156 keyPressed = 1;
jfields 0:015087d61ca1 157 }
jfields 1:e79ac0826624 158 if (!col2) {
jfields 1:e79ac0826624 159 desired_floor = 5;
jfields 1:e79ac0826624 160 keyPressed = 1;
jfields 1:e79ac0826624 161 }
jfields 1:e79ac0826624 162 }
jfields 1:e79ac0826624 163 */
jfields 1:e79ac0826624 164
jfields 1:e79ac0826624 165 void get_floor() {
jfields 1:e79ac0826624 166 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 167 if (test_floor == 1) {
jfields 1:e79ac0826624 168 test_readings++;
jfields 1:e79ac0826624 169 } else {
jfields 1:e79ac0826624 170 test_readings = 0;
jfields 1:e79ac0826624 171 }
jfields 1:e79ac0826624 172 test_floor = 1;
jfields 1:e79ac0826624 173 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 174 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 175 if (test_floor == 2) {
jfields 1:e79ac0826624 176 test_readings++;
jfields 1:e79ac0826624 177 } else {
jfields 1:e79ac0826624 178 test_readings = 0;
jfields 1:e79ac0826624 179 }
jfields 1:e79ac0826624 180 test_floor = 2;
jfields 1:e79ac0826624 181 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 182 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 183 if (test_floor == 3) {
jfields 1:e79ac0826624 184 test_readings++;
jfields 1:e79ac0826624 185 } else {
jfields 1:e79ac0826624 186 test_readings = 0;
jfields 1:e79ac0826624 187 }
jfields 1:e79ac0826624 188 test_floor = 3;
jfields 1:e79ac0826624 189 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 190 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 191 if (test_floor == 4) {
jfields 1:e79ac0826624 192 test_readings++;
jfields 1:e79ac0826624 193 } else {
jfields 1:e79ac0826624 194 test_readings = 0;
jfields 1:e79ac0826624 195 }
jfields 1:e79ac0826624 196 test_floor = 4;
jfields 1:e79ac0826624 197 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 198 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 199 if (test_floor == 5) {
jfields 1:e79ac0826624 200 test_readings++;
jfields 1:e79ac0826624 201 } else {
jfields 1:e79ac0826624 202 test_readings = 0;
jfields 1:e79ac0826624 203 }
jfields 1:e79ac0826624 204 test_floor = 5;
jfields 1:e79ac0826624 205 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 206 }
jfields 0:015087d61ca1 207 }
jfields 1:e79ac0826624 208
jfields 1:e79ac0826624 209 void get_period() {
jfields 1:e79ac0826624 210 count++;
jfields 1:e79ac0826624 211 if (count == 1)
jfields 1:e79ac0826624 212 t.start();
jfields 1:e79ac0826624 213 else if (count == 2) {
jfields 1:e79ac0826624 214 t.stop();
jfields 1:e79ac0826624 215 floor_period = t.read_us();
jfields 1:e79ac0826624 216 t.reset();
jfields 1:e79ac0826624 217 get_floor();
jfields 1:e79ac0826624 218 count = 0;
jfields 1:e79ac0826624 219 }
jfields 1:e79ac0826624 220 }
jfields 1:e79ac0826624 221
jfields 1:e79ac0826624 222 void led_update() {
jfields 1:e79ac0826624 223 if (cur_floor == 1) {
jfields 1:e79ac0826624 224 led1 = 1;
jfields 1:e79ac0826624 225 led2 = 0;
jfields 1:e79ac0826624 226 led3 = 0;
jfields 1:e79ac0826624 227 led4 = 0;
jfields 1:e79ac0826624 228 }
jfields 1:e79ac0826624 229 if (cur_floor == 2) {
jfields 1:e79ac0826624 230 led2 = 1;
jfields 1:e79ac0826624 231 led1 = 0;
jfields 1:e79ac0826624 232 led3 = 0;
jfields 1:e79ac0826624 233 led4 = 0;
jfields 1:e79ac0826624 234 }
jfields 1:e79ac0826624 235 if (cur_floor == 3) {
jfields 1:e79ac0826624 236 led3 = 1;
jfields 1:e79ac0826624 237 led2 = 0;
jfields 1:e79ac0826624 238 led1 = 0;
jfields 1:e79ac0826624 239 led4 = 0;
jfields 1:e79ac0826624 240 }
jfields 1:e79ac0826624 241 if (cur_floor == 4) {
jfields 1:e79ac0826624 242 led4 = 1;
jfields 1:e79ac0826624 243 led2 = 0;
jfields 1:e79ac0826624 244 led3 = 0;
jfields 1:e79ac0826624 245 led1 = 0;
jfields 1:e79ac0826624 246 }
jfields 1:e79ac0826624 247 if (cur_floor == 5) {
jfields 1:e79ac0826624 248 led4 = 1;
jfields 1:e79ac0826624 249 led1 = 1;
jfields 1:e79ac0826624 250 led2 = 0;
jfields 1:e79ac0826624 251 led3 = 0;
jfields 1:e79ac0826624 252 }
jfields 1:e79ac0826624 253 }
jfields 1:e79ac0826624 254
jfields 1:e79ac0826624 255 void bpc_func(void const *args) {
jfields 1:e79ac0826624 256 while (1) {
jfields 1:e79ac0826624 257 float ADC_val = Button.read();
jfields 1:e79ac0826624 258 int val = ADC_val*10;
jfields 1:e79ac0826624 259 if(val == 2) {
jfields 1:e79ac0826624 260 desired_floor = 1;
jfields 1:e79ac0826624 261 keyPressed = 1;
jfields 1:e79ac0826624 262 } else if(val == 4) {
jfields 1:e79ac0826624 263 desired_floor = 2;
jfields 1:e79ac0826624 264 keyPressed = 1;
jfields 1:e79ac0826624 265 } else if(val == 6) {
jfields 1:e79ac0826624 266 desired_floor = 3;
jfields 1:e79ac0826624 267 keyPressed = 1;
jfields 1:e79ac0826624 268 } else if(val == 8) {
jfields 1:e79ac0826624 269 desired_floor = 4;
jfields 1:e79ac0826624 270 keyPressed = 1;
jfields 1:e79ac0826624 271 } else if(val == 10) {
jfields 1:e79ac0826624 272 desired_floor = 5;
jfields 1:e79ac0826624 273 keyPressed = 1;
jfields 1:e79ac0826624 274 }
jfields 1:e79ac0826624 275 wait(0.1);
jfields 2:d7103c7e0671 276 while (keyPressed) { // wait for elevator to arrive at desired floor
jfields 2:d7103c7e0671 277 wait(.2);
jfields 2:d7103c7e0671 278 }
jfields 1:e79ac0826624 279 }
jfields 1:e79ac0826624 280 }