File content as of revision 6:c6de60c953d2:

// ESE 519 Lab 3 Code //

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

Serial pc(USBTX,USBRX);

// functions
void check_floor();
void get_floor();   // uses floor period to set cur_floor
void get_period();
void led_update();
void update_q();
void el_alg();  // need sorted list
void bubbleSort();  // sorts list

// 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

// 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_close = 0.1125;  // PCT [3.75-11.25]
float dty_servo_open = 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 el_q [4];
int el_q_size = 0;
char cur_direction = 'S';   // S = stationary, U = up, D = down

// mutex
Mutex q_mutex; // protect el_q

int main() {
   
    // init interrupt handler
    IRSensor.fall(&get_period);
   
    // init queue
    for (int i=0;i<4;i++) el_q[i] = 0;
   
    // 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;
            bpc_thread = new Thread(bpc_func); // start button checker thread
        }
        led_update();
       
        // check for key press
        //find_keys();

        // execute elevator alg
        if (!el_q_size) {

            q_mutex.lock();
            desired_floor = el_q[0];
            update_q();
            q_mutex.unlock();

            // check if need to move
            if (cur_floor != desired_floor) {

                // determine direction
                if (cur_floor > desired_floor) {    // move down
                    in1 = 1;
                    in2 = 0;
                    cur_direction = 'U';
                } else {                            // move up
                    in1 = 0;
                    in2 = 1;
                    cur_direction = 'D';
                }

                // start car
                servo1.write(dty_servo_close);
                servo2.write(dty_servo_close);
                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);
            wait(2);
            //keyPressed = 0;
        }
    }

}

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;
        q_mutex.lock();
        if(val == 2) {
            el_q[el_q_size] = 1;
            el_q_size++;
            bubbleSort();
        } else if(val == 4) {
            el_q[el_q_size] = 2;
            el_q_size++;
            bubbleSort();
        } else if(val == 6) {
            el_q[el_q_size] = 3;
            el_q_size++;
            bubbleSort();
        } else if(val == 8) {
            el_q[el_q_size] = 4;
            el_q_size++;
            bubbleSort();
        } else if(val == 10) {
            el_q[el_q_size] = 5;
            el_q_size++;
            bubbleSort();
        }
        q_mutex.unlock();
        wait(0.1);
        while (el_q_size == 4) { // wait for queue to empty
            wait(0.2);
        }
    }
}

void update_q() {
    for (int i=1;i<el_q_size;i++) {
        el_q[i-1] = el_q[i];
    }
    el_q_size--;    
}

void el_alg() {
    int index = -1;
    if (cur_direction == 'U') {
        for (int i=0; i<el_q_size;i++) {
            if (el_q[i] > cur_floor) { 
                index = i;
                break;
            }
        }
        if (index != -1) {
            for (int i=index; i<el_q_size; i++) {
                int temp = el_q[i-index];
                el_q[i-index] = el_q[i];
                el_q[i] = temp;
            }
        }
    }
    else if (cur_direction == 'D') {
        for (int i=0;i<el_q_size;i++) {
            if (el_q[i] > cur_floor) { 
                index = i;
                break;
            }
        }
        if (index != -1) {
            for (int i=0; i<index; i--) {
                int temp = el_q[i];
                el_q[i] = el_q[index-1-i];
                el_q[index-1-i] = temp;
            }
        }
    }     
}

void bubbleSort() {
    bool swapped = true;
    int j = 0;
    int tmp;
    while (swapped) {
        swapped = false;
        j++;
        for (int i = 0; i < el_q_size - j; i++) {
            if (el_q[i] > el_q[i + 1]) {
                tmp = el_q[i];
                el_q[i] = el_q[i + 1];
                el_q[i + 1] = tmp;
                swapped = true;
            }
        }
    }
}