File content as of revision 3:19106c977df1:

#include "mbed.h"

Serial pc(USBTX,USBRX);
PwmOut dc_motor(p23);
PwmOut servo1(p21);
PwmOut servo2(p22);
DigitalInOut in1(p5);
DigitalInOut in2(p6);
AnalogIn an(A0);
InterruptIn firstfloor(p15);
InterruptIn secondfloor(p8);
InterruptIn thirdfloor(p9);
InterruptIn fourthfloor(p10);
InterruptIn fifthfloor(p11);
InterruptIn sensor(p12);
DigitalOut led4(LED4);
DigitalOut led3(LED3); 
DigitalOut led2(LED2); 
DigitalOut led1(LED1); 
int first = 0;
Timer time1;
int once = 0;
float stationary = 0.075;
float ccw = 0.085;   // counter clock-wise
float cw = 0.065;    // clock-wise
int reached = 0;
int location;
float frequency;
int requested_floor;
int busy = 0;
int currentfloor;

void serve(){
        
    if(busy == 1){ 
        if (first == 0){
            time1.reset();
            time1.start();
            first = 1;
        }
            
        else if (first == 1){
            time1.stop();
            first = 0;
            once++;
            frequency = 1./time1.read();
            
            if (once == 2){
                once = 0;
                
                if (frequency >= 900 && frequency <= 1100){currentfloor = 5;}
                else if (frequency >= 600 && frequency <= 800){currentfloor = 4;led4 = 1;}
                else if (frequency >= 400 && frequency <= 590){currentfloor = 3;led3 = 1;}
                else if (frequency >= 200 && frequency <= 300){currentfloor = 2;led2 = 1;}
                else if (frequency >= 50 && frequency <= 150){currentfloor = 1;led1 = 1;}
        
                if (currentfloor == requested_floor){
                    dc_motor.write(0.0f);
                    led1 = 0;
                    led2 = 0;
                    led3 = 0;
                    led4 = 0;
                    in1 = 0;
                    in2 = 0;
                    servo1.write(cw);
                    servo2.write(ccw); 
                    wait(0.33); 
                    servo1.write(stationary);
                    servo2.write(stationary);
                    busy = 0;
                    location = requested_floor;
                    }

        }
 
}
}
    }
    
void moveUp(){
    dc_motor.write(1.0f);
    in1 = 1;
    in2 = 0;
    }
    
void moveDown(){
    
    dc_motor.write(1.0f);
    pc.printf("A");
    in1 = 0;
    in2 = 1;
    }


void closeDoor(){
    servo1.write(ccw);
    servo2.write(cw); 
    wait(0.33); 
    servo1.write(stationary);
    servo2.write(stationary);   
}

void openDoor(){
    servo1.write(cw);
    servo2.write(ccw); 
    wait(0.33); 
    servo1.write(stationary);
    servo2.write(stationary);
}

void call(int next) {
    if (busy == 0){
        requested_floor = next;
        if (location > next){
            closeDoor();
            pc.printf("%d",next);
            moveDown();
            busy = 1;
            }
            
        else if (location < next){
            closeDoor();
            pc.printf("C");
            moveUp();
            busy = 1;
            }
    }
}

void call1(void) { call(1); }
void call2(void) { call(2); }
void call3(void) { call(3); }
void call4(void) { call(4); }
void call5(void) { call(5); }


int main() {
    servo1.period(0.02f);
    servo2.period(0.02f);
    servo1.write(stationary);
    servo2.write(stationary);
    in1.output();
    in2.output();
    in2 = 0;
    in1 = 0;
    firstfloor.rise(&call1); 
    secondfloor.rise(&call2); 
    thirdfloor.rise(&call3); 
    fourthfloor.rise(&call4); 
    fifthfloor.rise(&call5);
    location = 2;
    sensor.fall(&serve);

    while(1){
   //     if (an > 0.59 && an < 0.7 ){requested_floor = 1;}    
   //     else if (an > 1.19 && an < 1.3 ){requested_floor = 2;} 
   //     else if (an > 1.79 && an < 1.9 ){requested_floor = 3;} 
   //     else if (an > 2.39 && an < 2.5 ){requested_floor = 4;} 
   //     else if (an > 2.99 && an < 3.1 ){requested_floor = 5;} 
 }
}