File content as of revision 8:ae5df9496557:

#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();
        freq.reset();
    } else {
        int diff = freq.read_us();
    //    pc.printf("edgediff: %d\r\n",diff);

        if(diff < 10100 && diff> 9900)
            floorat = 1;
        else if (diff < 4050 && diff > 3950)
            floorat = 2;
        else if (diff < 2100 && diff > 1900)
            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.stop();
    }
}

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){
 //   pc.printf("voltage: %f\r\n",voltage);
    if(voltage < 0.30f)
        return 1;
    else if (voltage < 0.55f)
        return 2;
    else if (voltage < 0.75f)
        return 3;
    else if (voltage < 0.9f)
        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.02f);
    
    
    floor.rise(&edgecnter);

//    while(1) { 
//        pc.printf("floor: %d\r\n",floorat);
//    }
    close_door();
    //pc.print
    //go up until you find a floor
    motor1 = 0;
    motor2 = 1;
    DCenable = 0.5f;
    

    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) {
            if(floorat!= 0)
              current_floor = floorat;
            pc.printf("at floor: %d\r\n",floorat);
            pc.printf("\t\tgo 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.5f;
            } else if(get_requested(request) < current_floor) {
                pc.printf("go down\r\n");
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.5f;
            } else {
                motor1=0;
                motor2=0;
                DCenable = 0;
            }
            
        }
        pc.printf("at requested floor\r\n");
        DCenable = 0;
        open_door();
        wait(5);
        close_door();
        while(get_requested(request) == floorat);

    }
        


    
    return 0;
}