File content as of revision 5:8e2b7caedd1d:

#include "mbed.h"
#include "PwmOut.h"

Timer freq;
unsigned int edge1=0;
unsigned short floorat = 0;
unsigned short edge = 2;
bool onFloor = false;
PwmOut servo1(p22);
PwmOut servo2(p23);
Serial pc(USBTX, USBRX); // tx, rx
PwmOut DCenable(p21);

void edgecnter() {
    edge--;
    if(edge > 0)
        freq.start();
    else {
        int diff = freq.read_us();
 //       pc.printf("edgediff: %d\r\n",diff);

        if(diff < 10050 && diff> 9900)
            floorat = 1;
        else if (diff < 4050 && diff > 3950)
            floorat = 2;
        else if (diff < 2050 && diff > 1950)
            floorat = 3;
        else if (diff < 1500 && diff > 1300)
            floorat = 4;
        else if (diff < 1100 && diff > 900)
            floorat = 5;
        edge = 2;
 //       if(floorat==0)
 //           pc.printf("edgediff: %d\r\n",diff);
        freq.reset();
    }
}

void open_door()
{
    servo1 = 0.03f; // right
    servo2 = 0.10f; // left
    wait(1);
}

void close_door()
{
    servo1 = 0.08f; //right
    servo2 = 0.05f; //left
    wait(1);
}

short get_requested(float voltage){
    if(voltage < 0.3f)
        return 1;
    else if (voltage < 0.5f)
        return 2;
    else if (voltage < 0.7f)
        return 3;
    else if (voltage < 0.7f)
        return 4;
    else
        return 5;
}

int main() {


    InterruptIn floor(p17);
    AnalogIn request(p18);
    servo1.period(0.020f);       //set 20ms period
    servo2.period(0.020f);       //set 20ms period
    DigitalOut motor1(p19);
    DigitalOut motor2(p20);
    //int floorat;
    DCenable.period(0.0002f);
    
    
    floor.rise(&edgecnter);

    
    close_door();
    //pc.print
    //go up until you find a floor
    motor1 = 0;
    motor2 = 1;
    DCenable = 0.8f;
    
    while(floorat==0);  

    short current_floor = floorat;
    pc.printf("at floor: %d\r\n",current_floor);
    pc.printf("go to floor: %d\r\n",get_requested(request));
    while(1) {
        while(get_requested(request) != floorat) {
            pc.printf("at floor: %d\r\n",current_floor);
            pc.printf("go to floor: %d\r\n",get_requested(request));
            if(get_requested(request) > current_floor) {
                pc.printf("go up\r\n");
                motor1 = 0;
                motor2 = 1;
                DCenable = 0.8f;
            } else if(get_requested(request) < current_floor) {
                pc.printf("go down\r\n");
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.8f;
            } else {
                motor1=0;
                motor2=0;
                DCenable = 0;

            }
        }
        pc.printf("at requested floor\r\n");
        open_door();
        wait(5);
        close_door();
        while(get_requested(request) == floorat);

    }
        


    
    return 0;
}