File content as of revision 9:d7a9c2345e9e:

// 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 bubble_sort();
void led_q();

// 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_dir = '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;
            led_update();
            bpc_thread = new Thread(bpc_func); // start button checker thread
        }
       
        // 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_dir = 'U';
                } else {                            // move up
                    in1 = 0;
                    in2 = 1;
                    cur_dir = '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);
            
            // update queue
            q_mutex.lock();
            update_q();
            q_mutex.unlock();
            wait(1);
            //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++;
            bubble_sort();
        } else if(val == 4) {
            el_q[el_q_size] = 2;
            el_q_size++;
            bubble_sort();
        } else if(val == 6) {
            el_q[el_q_size] = 3;
            el_q_size++;
            bubble_sort();
        } else if(val == 8) {
            el_q[el_q_size] = 4;
            el_q_size++;
            bubble_sort();
        } else if(val == 10) {
            el_q[el_q_size] = 5;
            el_q_size++;
            bubble_sort();
        }
        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 n = el_q_size;
    if (n>1) {
        int index;
        int temp;
        for (int i=0;i<n;i++) {
            if (el_q[i] > cur_floor) {
                index = i;
                break;
            }
        }
        if (cur_dir == 'U') {
            for (int i=index;i<n;i++) {
                temp = el_q[i-index];
                el_q[i-index] = el_q[i];
                el_q[i] = temp;
            }
        }
        if (cur_dir == 'D') {
            for (int i=0;i<index-1;i++) {
                temp = el_q[i];
                el_q[i] = el_q[index-i-1];
                el_q[index-i-1] = temp;
            }
        }
    }
    desired_floor = el_q[0];
    led_q();
}


void bubble_sort() {
    int n = el_q_size;
    bool sorted = false;
    while (!sorted) {
        sorted = true;
        for (int i=0;i<n-1;i++) {
            for (int j=i;j<n;j++) {
                if (el_q[i] > el_q[j]) {
                    int temp = el_q[j];
                    el_q[j] = el_q[i];
                    el_q[i] = temp;
                    sorted = false;
                }
            }
        }
    }
    el_alg();
}

void led_q() {
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    if (el_q_size >= 1) led1 = 1;
    if (el_q_size >= 2) led2 = 1;
    if (el_q_size >= 3) led3 = 1;
    if (el_q_size >= 4) led4 = 1;
}