File content as of revision 2:d7103c7e0671:

// ESE 519 Lab 3 Code //

#include "mbed.h"
#include "rtos.h"

Serial pc(USBTX,USBRX);

// functions
//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(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);
*/

// LEDs for testing
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

// init vars
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 = 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);

    // start with elevator stationary
    dc_motor.write(0);

    // start with closed doors
    servo1.write(dty_servo_close);
    servo2.write(dty_servo_close);

    while(1) {
       
        // get init floor
        while (!cur_floor) { // wait for a floor to be detected
            wait(0.25);
        }
        if (!desired_floor) desired_floor = cur_floor;
        led_update();
       
        // check for key press
        //find_keys();

        // execute elevator alg
        if (keyPressed) {

            led1 = 1;
            led2 = 1;
            led3 = 1;
            led4 = 1;

            // 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);
            }

            // open door
            servo1.write(dty_servo_open);
            servo2.write(dty_servo_open);
            keyPressed = 0;
        }
    }

}

/*
void find_keys() {

    // check row 1
    row1 = 0;
    row2 = 1;
    if (!col1) {
        desired_floor = 1;
        keyPressed = 1;
    }
    if (!col2) {
        desired_floor = 2;
        keyPressed = 1;
    }
    if (!col3) {
        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
            wait(.2);
        }
    }
}