File content as of revision 1:28120714ad80:

#include "mbed.h"

PwmOut servo1(p21);
PwmOut servo2(p22);
PwmOut dc_motor(p23);
DigitalInOut in1(p5);
DigitalInOut in2(p6);
InterruptIn firstfloor(p7);
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); 
Timeout timeout;
 
Timer time1;
 
float stationary = 0.075;
int closed;
float ccw = 0.085;   // counter clock-wise
float cw = 0.065;    // clock-wise
int reached = 0;
int location = 1;
int frequency;
int requested_floor;
int busy = 0;

void startTimer(){ 
    if(busy == 1){
    reached = 0;
    time1.start();
    }
}

void serve(){

    if(busy == 1){
    time1.stop();

    frequency = 1/(time1.read()*2);
    int floor;
    if (frequency >= 900 && frequency <= 1100){floor = 5;}
    else if (frequency >= 700 && frequency <= 800){floor = 4;led4 = !led4;}
    else if (frequency >= 450 && frequency <= 550){floor = 3;led3 = !led3;}
    else if (frequency >= 220 && frequency <= 290){floor = 2;led2 = !led2;}
    else if (frequency >= 70 && frequency <= 150){floor = 1;led1 = !led1;}
    
    if (floor == requested_floor){
        reached = 1;     
        }
        else{
            reached = 0;
        }
    if (reached == 1){
        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(0.6f);
    in1 = 1;
    in2 = 0;
    }
    
void moveDown(){
    dc_motor.write(0.6f);
    in1 = 0;
    in2 = 1;
    }

void stop(){
    in1 = 0;
    in2 = 0;
    }

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();
            moveDown();
            busy = 1;
            }
            
        else if (location < next){
            closeDoor();
            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);
    dc_motor.period(0.02f);
    servo1.write(stationary);
    servo2.write(stationary);
    dc_motor.write(0);
    in1.output();
    in1 = 0;
    in2.output();
    in2 = 0;
    firstfloor.rise(&call1); 
    secondfloor.rise(&call2); 
    thirdfloor.rise(&call3); 
    fourthfloor.rise(&call4); 
    fifthfloor.rise(&call5);
    sensor.rise(&startTimer);
    sensor.fall(&serve);

    while(1){    
 }
}