File content as of revision 1:d4b4c89e4a43:

// Test

#include "mbed.h"
#include "Motor.h"
#include <vector>

PwmOut servo1(p21);        // Left motor
PwmOut servo2(p22);         // Right motor

AnalogIn a_pin(p20);

using namespace std;
Timer period_t;

InterruptIn pin(p24);

Motor m(p23, p6, p7);

Serial pc(USBTX, USBRX);
vector<int> request;
// Scanning Keypad
DigitalOut row2(p11);
//DigitalOut row3(p12);
//DigitalOut row4(p14);
DigitalOut row1(p18);

InterruptIn column1(p13);
InterruptIn column2(p15);
InterruptIn column3(p16);
//DigitalIn column4(p17);
Ticker t_scan;

Ticker rowsTick;

int flag=0;
float freq,period;
int count = 0;
int i=0;
int numfloor,diff;                           // Floor number from keypad (input)
int curfloor;

int move;                       // Up = 1 , down = 0

void sort(){
    int num;
    int n = request.size();
    for(int l=0; l<n; l++){
        for(int m=0; m < n-l-1; m++){
               if(request[m] > request[m+1]){
                   num = request[m];
                   request[m] = request[m+1];
                   request[m+1] = num;
                   }
            }        
        }    

void door_oc(){
    pc.printf("On floor : %d \r\n",numfloor); 
    pc.printf("Door Opening\r\n");
    servo1.pulsewidth(0.00075);
    servo2.pulsewidth(0.00075);
    
    wait(2); 
    
    pc.printf("Door Closing\r\n");
    servo1.pulsewidth(0.00225);
    servo2.pulsewidth(0.00225);        
    
    }


int exist(int number)
{
    
    for(int t=0;t<request.size();t++)
    {
        if(number == request[t])
        {
            return 1;
        }
    }
    return 0;
}

// Scanning Keypad

void scan(){
    flag++;
    if(flag>2){
        flag = 1;
        }
    switch(flag){
        case 1 : row2 = 1;row1 = 0; break;
        case 2 : row1 = 1;row2 = 0; break;
        default : break;        
        }    
    
    }
void fall1(){
    if(flag == 1)
    {
        int c1 = exist(1);
        if( c1 == 0 && request.size()<4)
        {
        request.push_back(1);
        sort();
        //pc.printf("Buff size %d\r\n",request.size());
        pc.printf("You typed : 1\r\n");
        }
    }
    else if(flag == 2)
    {
        int c4 = exist(4);
        if( c4 == 0 && request.size()<4)
        {
        request.push_back(4);
        sort();
        //pc.printf("Buff size %d\r\n",request.size());
        pc.printf("You typed : 4\r\n");
        }
    }
    }
void fall2(){
    if(flag == 1)
    {
        int c2 = exist(2);
        if( c2 == 0 && request.size()<4)
        {
        request.push_back(2);
        sort();
        //pc.printf("Buff size %d\r\n",request.size());
        pc.printf("You typed : 2\r\n");
        }
    }
    else if(flag == 2)
    {
        int c5 = exist(5);
        if( c5 == 0 && request.size()<4)
        {
        request.push_back(5);
        sort();
        //pc.printf("Buff size %d\r\n",request.size());
        pc.printf("You typed : 5\r\n");
        }
    }
    }
void fall3(){
    if(flag == 1)
    {
        int c3 = exist(3);
        if( c3 == 0 && request.size()<4)
        {
        request.push_back(3);
        sort();
        //pc.printf("Buff size %d\r\n",request.size());
        pc.printf("You typed : 3\r\n");
        }
    }
    }


void direction(){
    for(int k = 0;k<request.size();k++){
        if(request[k] == curfloor){
            numfloor = curfloor;
            door_oc();
            request.erase(request.begin()+k);
            break;
            }
        }
    if(request.size()>0){
        int max = request[request.size()-1];
        if(max > curfloor){
            move = 1;
            }
        else if(max < curfloor){
            move = 0;
            }  
        }  
    }


int floor_detect(){

                    if(freq > 90.0 && freq < 110.0){
                         curfloor = 1;  
                         pc.printf("Floor : %d \r\n",curfloor);                          
                         //wait(2);
                        }
                    else if(freq > 190.0 && freq < 210.0){
                         curfloor = 2;
                         pc.printf("Floor : %d \r\n",curfloor);
                         //wait(2);
                        }
                    else if(freq > 290.0 && freq < 310.0){
                         curfloor = 3;
                         pc.printf("Floor : %d \r\n",curfloor);
                         //wait(2);
                        }
                    else if(freq > 390.0 && freq < 410.0){
                         curfloor = 4;
                         pc.printf("Floor : %d \r\n",curfloor);
                         //wait(2);
                        }
                    else if(freq > 490.0 && freq < 510.0){
                         curfloor = 5;
                         pc.printf("Floor : %d \r\n",curfloor);
                         //wait(2);
                        }
                   
                
         
        return curfloor;   
    }



void rise_edge(){
    
    if(a_pin.read()>0.95){
        count ++;
         if(count == 1){
             period_t.start();
             }
         else if(count == 2){
             period_t.stop();
             period = period_t.read();
             freq = 1.0/period;         
             period_t.reset();
             count = 0;
             }
        }
        if(freq > 0.0){
        //pc.printf("Freq: %3.2f\r\n",freq);
        }
    }

int get_index(int floor){
         for(int k = 0;k<request.size();k++){
          if(request[k]==floor){
              return k;
              }
          } 
          return -1;          
          }
          
int main() {
    
    t_scan.attach(&scan,0.004);
    column1.fall(&fall1);
    column2.fall(&fall2);
    column3.fall(&fall3);
    
    servo1.period(0.020);   
    
    request.push_back(1); 
    
    
    pc.printf("Start freq : %d\r\n", freq);
    pin.rise(&rise_edge);
    
    pc.printf("Start Now ............................. \r\n");
    while(1){   
          
        //sort(); 
        while(request.size()!=0){
          sort();
          curfloor = floor_detect();            
          direction();
          if(move == 1){                              
                for(int k = 0;k<request.size();k++){
                   sort();
                   numfloor = request[k];
                   diff = numfloor - curfloor;
                   while(diff > 0){
                       //pc.printf("DC ON +1 \r\n");
                       m.speed(1.0);
                    
                       curfloor = floor_detect();
                       if(exist(curfloor)){
                           numfloor = curfloor;
                           }
                       int f_index = get_index(curfloor);
                       diff = numfloor - curfloor; 
                       if(diff == 0 && f_index!= -1){
                           //wait(2);
                           pc.printf("DC OFF \r\n");
                           m.speed(0.0);
                           request.erase(request.begin()+f_index);
                           k--;
                           door_oc();
                           }
                   }              
                } 
                
                              
              }
          else if(move == 0){
              for(int k = request.size()-1;k >= 0;k--){
                   sort();
                   numfloor = request[k];
                   diff = numfloor - curfloor;
                   while(diff < 0){
                       m.speed(-1.0);
                       
                       curfloor = floor_detect();
                       if(exist(curfloor)){
                           numfloor = curfloor;
                           }
                       int f_index = get_index(curfloor);
                       diff = numfloor - curfloor; 
                       if(diff == 0 && f_index!= -1){
                           pc.printf("DC OFF \r\n");
                           //wait(2);
                           m.speed(0.0);
                           request.erase(request.begin()+k);
                           k++;
                           door_oc();
                           }
                   }              
                }                    
              }     
          }  
    }
}