ese519
/
ESE519_Lab3_EC_v5
Lab3
Fork of ESE519_Lab3_EC_v4 by
Diff: main.cpp
- Revision:
- 1:e79ac0826624
- Parent:
- 0:015087d61ca1
- Child:
- 2:d7103c7e0671
--- a/main.cpp Sat Oct 03 00:02:31 2015 +0000 +++ b/main.cpp Fri Oct 16 16:41:38 2015 +0000 @@ -1,147 +1,270 @@ +// ESE 519 Lab 3 Code // + #include "mbed.h" Serial pc(USBTX,USBRX); + // functions -void find_keys(); -Timer t; +//void find_keys(); +void check_floor(); +void get_floor(); // uses floor period to set cur_floor +void get_period(); +void led_update(); + +// Threads +void bpc_func(void const *args); +Thread * bpc_thread; // init board PwmOut dc_motor(p26); PwmOut servo1(p25); PwmOut servo2(p24); -DigitalOut in1(p14); // elevator direction -DigitalOut in2(p13); // elevator direction -AnalogIn IRSensor(p16); // read IR sensor -//AnalogOut test(p18); +DigitalOut in1(p11); // elevator direction +DigitalOut in2(p12); // elevator direction +InterruptIn IRSensor(p10); // read IR sensor +AnalogIn Button(p15); // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number // keyboard read +/* DigitalIn col1(p20); DigitalIn col2(p19); DigitalIn col3(p18); DigitalOut row1(p30); DigitalOut row2(p27); +*/ -DigitalOut ind1(LED1); -DigitalOut ind2(LED2); -DigitalOut ind3(LED3); -DigitalOut ind4(LED4); - +// LEDs for testing +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); // init vars -float ADC_Voltage; +Timer t; float period = 0.02; // sec float dty_dc = 0.75; // PCT [0-100] float dty_servo_open = 0.1125; // PCT [3.75-11.25] float dty_servo_close = 0.0375; // PCT [3.75-11.25] -int cur_floor = 1; -int desired_floor = 1; -int tperiod; +int cur_floor = 0; +int desired_floor = 0; +int floor_period; // for period detection +int keyPressed = 0; +int count = 0; +int test_readings = 0; // need 4 consecutive readings to declare cur_floor +int test_floor = 0; int main() { - + + // start button checker thread + bpc_thread = new Thread(bpc_func); + + // init interrupt handler + IRSensor.fall(&get_period); + // set period (constant) dc_motor.period(period); - // servo1.period(period); - // servo2.period(period); - + servo1.period(period); + servo2.period(period); + + // start with elevator stationary + dc_motor.write(0); + // start with closed doors - // servo1.write(dty_servo_close); - //servo2.write(dty_servo_close); + servo1.write(dty_servo_close); + servo2.write(dty_servo_close); - // run elevator program while(1) { - - + + // get init floor + while (!cur_floor) { // wait for a floor to be detected + wait(0.25); + } + desired_floor = cur_floor; + led_update(); + // check for key press - find_keys(); - - // check if need to move - if (cur_floor != desired_floor) { - - // determine direction - if (cur_floor > desired_floor) { // move down - in1 = 0; - in2 = 1; - } - else { // move up - in1 = 1; - in2 = 0; + //find_keys(); + + // execute elevator alg + if (keyPressed) { + + // check if need to move + if (cur_floor != desired_floor) { + + // determine direction + if (cur_floor > desired_floor) { // move down + in1 = 1; + in2 = 0; + } else { // move up + in1 = 0; + in2 = 1; + } + + // start car + dc_motor.write(dty_dc); + + // check IR sensors + while (cur_floor != desired_floor) { + wait(0.2); + } + + // stop car + dc_motor.write(0); } - - // start car - dc_motor.write(dty_dc); - - // check IR sensors - - while (cur_floor != desired_floor) { - // check IR emitter here! - - while(!ADC_Voltage) - ADC_Voltage = IRSensor.read(); - - t.start(); - - while(ADC_Voltage) - ADC_Voltage = IRSensor.read(); - - t.stop(); - - tperiod = 2 * t.read_us(); - - t.reset(); - - if(tperiod > 9900 && tperiod < 10100) - cur_floor = 1; - else if(tperiod > 3900 && tperiod < 4100) - cur_floor = 2; - else if(tperiod > 1900 && tperiod < 2100) - cur_floor = 3; - else if(tperiod > 1300 && tperiod < 1500) - cur_floor = 4; - else if(tperiod > 900 && tperiod < 1100) - cur_floor = 5; - - //pc.printf("%d", cur_floor); - //wait_ms(1000); - } + + // open door + servo1.write(dty_servo_open); + servo2.write(dty_servo_open); + keyPressed = 0; } - - // stop car - dc_motor.write(0); - - // } - - // open door - - // servo1.write(dty_servo_open); - // servo2.write(dty_servo_open); - - - } - - + } + } +/* void find_keys() { - + // check row 1 row1 = 0; row2 = 1; if (!col1) { - desired_floor = 1; + desired_floor = 1; + keyPressed = 1; } if (!col2) { - desired_floor = 2; + desired_floor = 2; + keyPressed = 1; } if (!col3) { - desired_floor = 3; + desired_floor = 3; + keyPressed = 1; } - + // check row 2 row1 = 1; row2 = 0; if (!col1) { desired_floor = 4; + keyPressed = 1; } - + if (!col2) { + desired_floor = 5; + keyPressed = 1; + } +} +*/ + +void get_floor() { + if(floor_period > 9900 && floor_period < 10100) { + if (test_floor == 1) { + test_readings++; + } else { + test_readings = 0; + } + test_floor = 1; + if (test_readings > 3) cur_floor = 1; + } else if(floor_period > 3900 && floor_period < 4100) { + if (test_floor == 2) { + test_readings++; + } else { + test_readings = 0; + } + test_floor = 2; + if (test_readings > 3) cur_floor = 2; + } else if(floor_period > 1900 && floor_period < 2100) { + if (test_floor == 3) { + test_readings++; + } else { + test_readings = 0; + } + test_floor = 3; + if (test_readings > 3) cur_floor = 3; + } else if(floor_period > 1300 && floor_period < 1500) { + if (test_floor == 4) { + test_readings++; + } else { + test_readings = 0; + } + test_floor = 4; + if (test_readings > 3) cur_floor = 4; + } else if(floor_period > 900 && floor_period < 1100) { + if (test_floor == 5) { + test_readings++; + } else { + test_readings = 0; + } + test_floor = 5; + if (test_readings > 3) cur_floor = 5; + } } + +void get_period() { + count++; + if (count == 1) + t.start(); + else if (count == 2) { + t.stop(); + floor_period = t.read_us(); + t.reset(); + get_floor(); + count = 0; + } +} + +void led_update() { + if (cur_floor == 1) { + led1 = 1; + led2 = 0; + led3 = 0; + led4 = 0; + } + if (cur_floor == 2) { + led2 = 1; + led1 = 0; + led3 = 0; + led4 = 0; + } + if (cur_floor == 3) { + led3 = 1; + led2 = 0; + led1 = 0; + led4 = 0; + } + if (cur_floor == 4) { + led4 = 1; + led2 = 0; + led3 = 0; + led1 = 0; + } + if (cur_floor == 5) { + led4 = 1; + led1 = 1; + led2 = 0; + led3 = 0; + } +} + +void bpc_func(void const *args) { + while (1) { + float ADC_val = Button.read(); + int val = ADC_val*10; + if(val == 2) { + desired_floor = 1; + keyPressed = 1; + } else if(val == 4) { + desired_floor = 2; + keyPressed = 1; + } else if(val == 6) { + desired_floor = 3; + keyPressed = 1; + } else if(val == 8) { + desired_floor = 4; + keyPressed = 1; + } else if(val == 10) { + desired_floor = 5; + keyPressed = 1; + } + wait(0.1); + while (keyPressed); // wait for elevator to arrive at desired floor + } +} \ No newline at end of file