File content as of revision 5:d5f2ff7ca039:

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

BusOut leds(LED1,LED2,LED3,LED4);

//Driving motors
PwmOut EN(p22);
DigitalOut IN1(p30);
DigitalOut IN2(p29);


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

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 for floor detection
Timer irT;

//init keypad map
map<int, int> keypadMap;


int curFloor;  //initialize at 2
int floorDesired = 1; //init floor buffer
bool floors[5] = {false, false, false,false,false};

int open = 0;
int count = 0;
int nextFloor = 1;

//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 = 150.0;

void init(){        
        keypadMap[0x11]= 1;
        keypadMap[0x12]= 2;
        keypadMap[0x14]= 3;
        keypadMap[0x21]= 4;
        keypadMap[0x24]= 5;
        }

//Elevator functions
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
    }

//Elevator movement
void up(){
    EN = 0.6;
    IN1 = 1;
    IN2 = 0.0;
    }
void down(){
    EN = 0.6;
    IN1 = 0.0;
    IN2 = 1;   
    }

void stop(){
    EN = 0.0;
    IN1 = 0.0;
    IN2 = 0.0;
    }
    
//Floor classification based on the time period    
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;
    }

//Interrupt In of IR wave
void irRise() {
    irT.start();
    }

//Mecahnism to detect floors with aggregation
int aggr;
int temp;
int prev;

void irFall() {
    irT.stop();
    float irPeriod = 2 * irT.read_us();
    
    temp = getFloor(irPeriod);
    if(temp != 0){
      if(temp == prev)
          aggr++;
      else aggr = 0;
      if(aggr == 15){
            curFloor = temp;
          aggr = 0;
          }
      prev = temp;
    }
    irT.reset();
    }

//Getting next floor based on Elevator Algorithm
int getNextFloor() {
        //up
        if (direction == 1){
                for (int i = curFloor; i< 5; ++i){
                    if(floors[i]==true)
                        return i+1;
                    }
                for (int i = curFloor - 2; i >=0 ; --i){
                    if(floors[i]==true)
                        return i+1;
                    }
                return curFloor;
            }
        //down
        else if (direction == 2){
                for (int i = curFloor - 2; i >=0 ; --i){

                    if(floors[i]==true)
                        return i+1;
                    }
                for (int i = curFloor; i< 5; ++i){
                    if(floors[i]==true)
                        return i+1;
                    }
                return curFloor;
            }
        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);
    
    //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
    
    int reach = 0;
    
    //go down until 1st floor detected as starting point
    pc.printf("Starting elevator \r");
    
    while(curFloor != 1)
    {
        down();
    }
    
    stop();
    
    
    
    while(1) {

 
/////////////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
            
            if(cur_input == 0x22)
                leds = 0x1;
            
            //leds = keypadMap[cur_input];             //KEYPAD DEBUG
            if(cur_input != prev_input && cur_input != 0){
                    floorDesired = keypadMap[cur_input];
                    floors[floorDesired - 1] = true;
                    nextFloor = getNextFloor();
                    pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);

                }
            //Maintain the past input
            prev_input = cur_input;
            //fix this error i don't know why it exsits

            
////////////////////////MOVE FLOORS//////////////////////////////////////////////////   
         
        
        if(curFloor == nextFloor){ //if correct stop moving and get the next floor
            stop();
            if(open == 0)
            { 
            openDoors();
            open = 1;
            }
            floors[nextFloor-1] = false;
            if(reach == 0 ){
                
                nextFloor = getNextFloor();
                wait(1);
                reach = 1;
                pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
                }
                reach = 1;
            }
        if(curFloor < nextFloor){  //if the cur floor is below the nextfloor move up
            reach = 0;
            closeDoors();
            up();
            direction = 1;
            }
        if(curFloor > nextFloor){ //if the cur floor is above the nextfloor move down
            if(open == 1)
            { 
            closeDoors();
            open = 0;
            }
            reach = 0;
            down();
            direction = 2;
            }
                        
            }     
}