File content as of revision 2:dcd904176b27:

#include "mbed.h"
#include <map>

//elevator I/O
BusOut leds(LED1,LED2,LED3,LED4);
PwmOut EN(p23);
DigitalOut IN1(p24);
DigitalOut IN2(p25);


//p23 LEFT p24 RIGHT
PwmOut leftDoor(p21);
PwmOut rightDoor(p22);

InterruptIn  irIN(p16);

Serial pc(USBTX, USBRX);


//itizilize digital I/O for keypad
DigitalOut row1(p27);
DigitalOut row2(p28);
DigitalIn col1(p20);
DigitalIn col2(p19);
DigitalIn col3(p18);

//timer for row switch
Timer t1;
Timer irT;
//init keypad map
map<int, int> keypadMap;

int curFloor = 1;  //initialize at 1
int floorDesired = 1; //init floor buffer
bool floors[5];

int count = 0;
int nextFloor;

//direction 1:up; 2: down;
int direction = 0;

float first = 10000.0;
float second = 5000.0;
float third = 3333.0;
float fourth = 2500.0;
float fifth = 2000.0;

float tol = 250.0;

void init(){
        /*
        keypadMap[0x11]= 0x1;
        keypadMap[0x12]= 0x2;
        keypadMap[0x14]= 0x4;
        keypadMap[0x18]= 0x0;
        keypadMap[0x21]= 0x8;
        keypadMap[0x22]= 0x0;
        keypadMap[0x24]= 0x0;
        keypadMap[0x28]= 0x0;
        */
        
        keypadMap[0x11]= 1;
        keypadMap[0x12]= 2;
        keypadMap[0x14]= 3;
        keypadMap[0x21]= 4;
        keypadMap[0x22]= 5;
        }

void openDoors(){
    
    leftDoor= 0.075;                  //init door closed
    rightDoor= 0.125;                  //itit door closed]
    
    }
void closeDoors(){
    
    leftDoor= 0.125;                  //init door closed
    rightDoor= 0.075;                  //itit door closed
    }

void up(){
    EN = 0.2;
    IN1 = 1;
    IN2 = 0.0;
    }
void down(){
    EN = 0.2;
    IN1 = 0.0;
    IN2 = 1;   
    }

void stop(){
    EN = 0.0;
    IN1 = 0.0;
    IN2 = 0.0;
    }
    
int getFloor(float period) {
        if (first - tol < period && first + tol >= period)
            return 1;
        else if (second - tol < period && second + tol >= period)
            return 2;
        else if (third - tol < period && third + tol >= period)
            return 3;
        else if (fourth - tol < period && fourth + tol >= period)
            return 4;
        else if (fifth - tol < period && fifth + tol >= period)
            return 5;
        else return 0;
    }

void irRise() {
    irT.start();
    }

int temp; 
void irFall() {
    irT.stop();
    float irPeriod = 2 * irT.read_us();
    
    temp = getFloor(irPeriod);
    if(temp != 0)
        curFloor = temp;
    irT.reset();
    }

int getNextFloor() {
        //up
        if (direction == 1){
                for (int i = curFloor; i< 4; ++i){
                    if(floors[i])
                        return i+1;
                    }
                for (int i = curFloor - 2; i >=0 ; --i){
                    if(floors[i])
                        return i+1;
                    }
            }
        //down
        if (direction == 2){
                for (int i = curFloor - 2; i >=0 ; --i){
                    if(floors[i])
                        return i+1;
                    }
                for (int i = curFloor; i< 4; ++i){
                    if(floors[i])
                        return i+1;
                    }
            }
        else return floorDesired;
    }

int main() {
        
    init();
    //Initialize rows to 0 and keypad stuff
    row1 = 0;
    row2 = 0;
    t1.start();
    int cur_input = 0x00;
    int prev_input = 0x00;
    //float irPeriod = 0; 
    irIN.rise(&irRise);
    irIN.fall(&irFall);
    
    //ELEVATOR STATE
    //int nofloor = 0;   //set nofloor state to 0
    
    //int Serv = 0;
    //initialize servo and dc motor
    float freq = 50;    //set 50 HZ freq for PWM and motor
    EN.period(1/freq);   
                                    
                         
    leftDoor.period(1/freq);             
    leftDoor= 0.125;                  //init door closed
    rightDoor.period(1/freq);              
    rightDoor= 0.075;                  //itit door closed
    
    while(1) {
        pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor);
 
/////////////Below contains all code in order to detect current input from keypad////////////////////////////       
        if(prev_input == 0){

            row1 = 0;
            row2 = 0;
            //Keep each pin on for 4ms
            //"turn on" appropriat bit in map when on.
           switch(t1.read_ms()%8){
                            case 0:
                            row1 = 1;
                            cur_input |= 0x10;   
                            break;
                            case 4:
                            row2 = 1;
                            cur_input |= 0x20;
                            break;
                            }
                }
        
            //Check each colum to see if it is high
            //if it is "turn on" that respective bit
             if(col1 == 1)
             cur_input |= 0x01;
             else if(col2 == 1)
             cur_input |= 0x02;
             else if(col3 == 1)
             cur_input |= 0x04;
             else cur_input = 0;    //Detect button release or no input set input to 0
            
            //leds = keypadMap[cur_input];             //KEYPAD DEBUG
            if(cur_input != prev_input && cur_input != 0){
                    floorDesired = keypadMap[cur_input];
                    floors[floorDesired - 1] = true;
                    nextFloor = getNextFloor();
                    //Serv++;
                }
            //Maintain the past input
            prev_input = cur_input;
            
////////////////////////MOVE FLOORS//////////////////////////////////////////////////   
         
    
        if(curFloor == nextFloor){
            stop();
            floors[nextFloor-1] = false;
            nextFloor = getNextFloor();
            }
        if(curFloor < nextFloor){
            up();
            direction = 1;
            }
        if(curFloor > nextFloor){
            down();
            direction = 2;
            }
                        
            
            } 
       
    
}