ese519 / Mbed 2 deprecated ESE519_Lab3_v2

Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3 by ese519

main.cpp@1:e79ac0826624, 2015-10-16 (annotated)

Committer:
jfields
Date:
Fri Oct 16 16:41:38 2015 +0000
Revision:
1:e79ac0826624
Parent:
0:015087d61ca1
Child:
2:d7103c7e0671
elevator code w button

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 0:015087d61ca1 4
jfields 0:015087d61ca1 5 Serial pc(USBTX,USBRX);
jfields 1:e79ac0826624 6
jfields 0:015087d61ca1 7 // functions
jfields 1:e79ac0826624 8 //void find_keys();
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 1:e79ac0826624 13
jfields 1:e79ac0826624 14 // Threads
jfields 1:e79ac0826624 15 void bpc_func(void const *args);
jfields 1:e79ac0826624 16 Thread * bpc_thread;
jfields 0:015087d61ca1 17
jfields 0:015087d61ca1 18 // init board
jfields 0:015087d61ca1 19 PwmOut dc_motor(p26);
jfields 0:015087d61ca1 20 PwmOut servo1(p25);
jfields 0:015087d61ca1 21 PwmOut servo2(p24);
jfields 1:e79ac0826624 22 DigitalOut in1(p11); // elevator direction
jfields 1:e79ac0826624 23 DigitalOut in2(p12); // elevator direction
jfields 1:e79ac0826624 24 InterruptIn IRSensor(p10); // read IR sensor
jfields 1:e79ac0826624 25 AnalogIn Button(p15); // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number
jfields 0:015087d61ca1 26
jfields 0:015087d61ca1 27 // keyboard read
jfields 1:e79ac0826624 28 /*
jfields 0:015087d61ca1 29 DigitalIn col1(p20);
jfields 0:015087d61ca1 30 DigitalIn col2(p19);
jfields 0:015087d61ca1 31 DigitalIn col3(p18);
jfields 0:015087d61ca1 32 DigitalOut row1(p30);
jfields 0:015087d61ca1 33 DigitalOut row2(p27);
jfields 1:e79ac0826624 34 */
jfields 0:015087d61ca1 35
jfields 1:e79ac0826624 36 // LEDs for testing
jfields 1:e79ac0826624 37 DigitalOut led1(LED1);
jfields 1:e79ac0826624 38 DigitalOut led2(LED2);
jfields 1:e79ac0826624 39 DigitalOut led3(LED3);
jfields 1:e79ac0826624 40 DigitalOut led4(LED4);
jfields 0:015087d61ca1 41
jfields 0:015087d61ca1 42 // init vars
jfields 1:e79ac0826624 43 Timer t;
jfields 0:015087d61ca1 44 float period = 0.02; // sec
jfields 0:015087d61ca1 45 float dty_dc = 0.75; // PCT [0-100]
jfields 0:015087d61ca1 46 float dty_servo_open = 0.1125; // PCT [3.75-11.25]
jfields 0:015087d61ca1 47 float dty_servo_close = 0.0375; // PCT [3.75-11.25]
jfields 1:e79ac0826624 48 int cur_floor = 0;
jfields 1:e79ac0826624 49 int desired_floor = 0;
jfields 1:e79ac0826624 50 int floor_period; // for period detection
jfields 1:e79ac0826624 51 int keyPressed = 0;
jfields 1:e79ac0826624 52 int count = 0;
jfields 1:e79ac0826624 53 int test_readings = 0; // need 4 consecutive readings to declare cur_floor
jfields 1:e79ac0826624 54 int test_floor = 0;
jfields 0:015087d61ca1 55
jfields 0:015087d61ca1 56 int main() {
jfields 1:e79ac0826624 57
jfields 1:e79ac0826624 58 // start button checker thread
jfields 1:e79ac0826624 59 bpc_thread = new Thread(bpc_func);
jfields 1:e79ac0826624 60
jfields 1:e79ac0826624 61 // init interrupt handler
jfields 1:e79ac0826624 62 IRSensor.fall(&get_period);
jfields 1:e79ac0826624 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 1:e79ac0826624 82 desired_floor = cur_floor;
jfields 1:e79ac0826624 83 led_update();
jfields 1:e79ac0826624 84
jfields 0:015087d61ca1 85 // check for key press
jfields 1:e79ac0826624 86 //find_keys();
jfields 1:e79ac0826624 87
jfields 1:e79ac0826624 88 // execute elevator alg
jfields 1:e79ac0826624 89 if (keyPressed) {
jfields 1:e79ac0826624 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 1:e79ac0826624 104 dc_motor.write(dty_dc);
jfields 1:e79ac0826624 105
jfields 1:e79ac0826624 106 // check IR sensors
jfields 1:e79ac0826624 107 while (cur_floor != desired_floor) {
jfields 1:e79ac0826624 108 wait(0.2);
jfields 1:e79ac0826624 109 }
jfields 1:e79ac0826624 110
jfields 1:e79ac0826624 111 // stop car
jfields 1:e79ac0826624 112 dc_motor.write(0);
jfields 0:015087d61ca1 113 }
jfields 1:e79ac0826624 114
jfields 1:e79ac0826624 115 // open door
jfields 1:e79ac0826624 116 servo1.write(dty_servo_open);
jfields 1:e79ac0826624 117 servo2.write(dty_servo_open);
jfields 1:e79ac0826624 118 keyPressed = 0;
jfields 0:015087d61ca1 119 }
jfields 1:e79ac0826624 120 }
jfields 1:e79ac0826624 121
jfields 0:015087d61ca1 122 }
jfields 0:015087d61ca1 123
jfields 1:e79ac0826624 124 /*
jfields 0:015087d61ca1 125 void find_keys() {
jfields 1:e79ac0826624 126
jfields 0:015087d61ca1 127 // check row 1
jfields 0:015087d61ca1 128 row1 = 0;
jfields 0:015087d61ca1 129 row2 = 1;
jfields 0:015087d61ca1 130 if (!col1) {
jfields 1:e79ac0826624 131 desired_floor = 1;
jfields 1:e79ac0826624 132 keyPressed = 1;
jfields 0:015087d61ca1 133 }
jfields 0:015087d61ca1 134 if (!col2) {
jfields 1:e79ac0826624 135 desired_floor = 2;
jfields 1:e79ac0826624 136 keyPressed = 1;
jfields 0:015087d61ca1 137 }
jfields 0:015087d61ca1 138 if (!col3) {
jfields 1:e79ac0826624 139 desired_floor = 3;
jfields 1:e79ac0826624 140 keyPressed = 1;
jfields 0:015087d61ca1 141 }
jfields 1:e79ac0826624 142
jfields 0:015087d61ca1 143 // check row 2
jfields 0:015087d61ca1 144 row1 = 1;
jfields 0:015087d61ca1 145 row2 = 0;
jfields 0:015087d61ca1 146 if (!col1) {
jfields 0:015087d61ca1 147 desired_floor = 4;
jfields 1:e79ac0826624 148 keyPressed = 1;
jfields 0:015087d61ca1 149 }
jfields 1:e79ac0826624 150 if (!col2) {
jfields 1:e79ac0826624 151 desired_floor = 5;
jfields 1:e79ac0826624 152 keyPressed = 1;
jfields 1:e79ac0826624 153 }
jfields 1:e79ac0826624 154 }
jfields 1:e79ac0826624 155 */
jfields 1:e79ac0826624 156
jfields 1:e79ac0826624 157 void get_floor() {
jfields 1:e79ac0826624 158 if(floor_period > 9900 && floor_period < 10100) {
jfields 1:e79ac0826624 159 if (test_floor == 1) {
jfields 1:e79ac0826624 160 test_readings++;
jfields 1:e79ac0826624 161 } else {
jfields 1:e79ac0826624 162 test_readings = 0;
jfields 1:e79ac0826624 163 }
jfields 1:e79ac0826624 164 test_floor = 1;
jfields 1:e79ac0826624 165 if (test_readings > 3) cur_floor = 1;
jfields 1:e79ac0826624 166 } else if(floor_period > 3900 && floor_period < 4100) {
jfields 1:e79ac0826624 167 if (test_floor == 2) {
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 = 2;
jfields 1:e79ac0826624 173 if (test_readings > 3) cur_floor = 2;
jfields 1:e79ac0826624 174 } else if(floor_period > 1900 && floor_period < 2100) {
jfields 1:e79ac0826624 175 if (test_floor == 3) {
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 = 3;
jfields 1:e79ac0826624 181 if (test_readings > 3) cur_floor = 3;
jfields 1:e79ac0826624 182 } else if(floor_period > 1300 && floor_period < 1500) {
jfields 1:e79ac0826624 183 if (test_floor == 4) {
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 = 4;
jfields 1:e79ac0826624 189 if (test_readings > 3) cur_floor = 4;
jfields 1:e79ac0826624 190 } else if(floor_period > 900 && floor_period < 1100) {
jfields 1:e79ac0826624 191 if (test_floor == 5) {
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 = 5;
jfields 1:e79ac0826624 197 if (test_readings > 3) cur_floor = 5;
jfields 1:e79ac0826624 198 }
jfields 0:015087d61ca1 199 }
jfields 1:e79ac0826624 200
jfields 1:e79ac0826624 201 void get_period() {
jfields 1:e79ac0826624 202 count++;
jfields 1:e79ac0826624 203 if (count == 1)
jfields 1:e79ac0826624 204 t.start();
jfields 1:e79ac0826624 205 else if (count == 2) {
jfields 1:e79ac0826624 206 t.stop();
jfields 1:e79ac0826624 207 floor_period = t.read_us();
jfields 1:e79ac0826624 208 t.reset();
jfields 1:e79ac0826624 209 get_floor();
jfields 1:e79ac0826624 210 count = 0;
jfields 1:e79ac0826624 211 }
jfields 1:e79ac0826624 212 }
jfields 1:e79ac0826624 213
jfields 1:e79ac0826624 214 void led_update() {
jfields 1:e79ac0826624 215 if (cur_floor == 1) {
jfields 1:e79ac0826624 216 led1 = 1;
jfields 1:e79ac0826624 217 led2 = 0;
jfields 1:e79ac0826624 218 led3 = 0;
jfields 1:e79ac0826624 219 led4 = 0;
jfields 1:e79ac0826624 220 }
jfields 1:e79ac0826624 221 if (cur_floor == 2) {
jfields 1:e79ac0826624 222 led2 = 1;
jfields 1:e79ac0826624 223 led1 = 0;
jfields 1:e79ac0826624 224 led3 = 0;
jfields 1:e79ac0826624 225 led4 = 0;
jfields 1:e79ac0826624 226 }
jfields 1:e79ac0826624 227 if (cur_floor == 3) {
jfields 1:e79ac0826624 228 led3 = 1;
jfields 1:e79ac0826624 229 led2 = 0;
jfields 1:e79ac0826624 230 led1 = 0;
jfields 1:e79ac0826624 231 led4 = 0;
jfields 1:e79ac0826624 232 }
jfields 1:e79ac0826624 233 if (cur_floor == 4) {
jfields 1:e79ac0826624 234 led4 = 1;
jfields 1:e79ac0826624 235 led2 = 0;
jfields 1:e79ac0826624 236 led3 = 0;
jfields 1:e79ac0826624 237 led1 = 0;
jfields 1:e79ac0826624 238 }
jfields 1:e79ac0826624 239 if (cur_floor == 5) {
jfields 1:e79ac0826624 240 led4 = 1;
jfields 1:e79ac0826624 241 led1 = 1;
jfields 1:e79ac0826624 242 led2 = 0;
jfields 1:e79ac0826624 243 led3 = 0;
jfields 1:e79ac0826624 244 }
jfields 1:e79ac0826624 245 }
jfields 1:e79ac0826624 246
jfields 1:e79ac0826624 247 void bpc_func(void const *args) {
jfields 1:e79ac0826624 248 while (1) {
jfields 1:e79ac0826624 249 float ADC_val = Button.read();
jfields 1:e79ac0826624 250 int val = ADC_val*10;
jfields 1:e79ac0826624 251 if(val == 2) {
jfields 1:e79ac0826624 252 desired_floor = 1;
jfields 1:e79ac0826624 253 keyPressed = 1;
jfields 1:e79ac0826624 254 } else if(val == 4) {
jfields 1:e79ac0826624 255 desired_floor = 2;
jfields 1:e79ac0826624 256 keyPressed = 1;
jfields 1:e79ac0826624 257 } else if(val == 6) {
jfields 1:e79ac0826624 258 desired_floor = 3;
jfields 1:e79ac0826624 259 keyPressed = 1;
jfields 1:e79ac0826624 260 } else if(val == 8) {
jfields 1:e79ac0826624 261 desired_floor = 4;
jfields 1:e79ac0826624 262 keyPressed = 1;
jfields 1:e79ac0826624 263 } else if(val == 10) {
jfields 1:e79ac0826624 264 desired_floor = 5;
jfields 1:e79ac0826624 265 keyPressed = 1;
jfields 1:e79ac0826624 266 }
jfields 1:e79ac0826624 267 wait(0.1);
jfields 1:e79ac0826624 268 while (keyPressed); // wait for elevator to arrive at desired floor
jfields 1:e79ac0826624 269 }
jfields 1:e79ac0826624 270 }