ese519
Lab3
Fork of
ESE519_Lab3_v3
by 
ese519
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