File content as of revision 2:27a9092dcc6c:

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

unsigned int freqcnt=0;
unsigned short floorsPassed = 0;
bool onFloor = false;
PwmOut servo1(p22);
PwmOut servo2(p23);
Serial pc(USBTX, USBRX); // tx, rx
PwmOut DCenable(p21);

void freq_counter() {
    freqcnt++;
    //DCenable = 0;
}

void timerfunc() {  // what it does
    if (freqcnt > 10) {
        onFloor = true;
        
    } else {
        if(onFloor)
            floorsPassed++;
        onFloor = false;
    }
    freqcnt = 0;   
} 

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_floor(int i) {
    int floorat=0;
    wait(0.099f);
    if (freqcnt>90) {    //fifth floor
        floorat = 5;
    } else if (freqcnt>60) {    //fourth floor
        floorat = 4;
    } else if (freqcnt>40) {    //third floor
        floorat = 3;
    } else if (freqcnt>20) {    //second floor
        floorat = 2;
    } else if(freqcnt > 0) { //at first floor
        floorat = 1;
    }  
    if(i>0) {
        int floor2 = get_floor(i-1);
        if (floor2 == floorat)
            return floorat;
        else
            return floor2;
    }
    return floorat;
}  

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);
    
    
    
    Ticker check_floor; //?
    check_floor.attach(&timerfunc,0.1f);
    floor.fall(&freq_counter);
    
   close_door();
   //pc.print

    while(!onFloor) {    //go to the first floor below us
        motor1 = 1;
        motor2 = 0;
        DCenable = 0.8f;
    }
    
    pc.printf("at floor\r\n");
    floorat = get_floor(1);
    
    pc.printf("floor %d\r\n",floorat);
    
    DCenable = 0;
    wait(5);
    open_door();
    close_door();
    
  
    while(1){
        pc.printf("%d floor\n\r",floorat);
        pc.printf("%f requested\n\r",request.read());
        if(request <= 0.2f) { //first floor
            pc.printf("go to the first floor\r\n");
            if(floorat > 1) { //if we're above the first floor
                //set DC motor to go down
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.8f;
            } else if (floorat == 1) {
                DCenable = 0;
                open_door();
                wait(10);
                close_door();
            }
            
        } else if (request <= 0.4f) { // second floor
            pc.printf("go to the second floor\r\n");
            if (floorat > 2) {
                //set DC motor to go down
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.8f;
            } else if (floorat < 2) {
                //set DC motor to go up
                motor1 = 0;
                motor2 = 1;
                DCenable = 0.8f;
            } else {
                DCenable = 0;
                open_door();
                wait(10);
                close_door();
            }
        } else if (request*5 <= 0.6f) { // third floor
            pc.printf("go to the third floor\r\n");
            if (floorat > 3) {
                //set DC motor to go down
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.8f;
            } else if (floorat < 3) {
                //set DC motor to go up
                motor1 = 0;
                motor2 = 1;
                DCenable = 0.8f;
            } else {
                DCenable = 0;
                open_door();
                wait(10);
                close_door();
            }
        } else if (request <= 0.8f) { // fourth floor
            pc.printf("go to the fourth floor\r\n");
            if (floorat > 4) {
                //set DC motor to go down
                motor1 = 1;
                motor2 = 0;
                DCenable = 0.8f;
            } else if (floorat < 4) {
                //set DC motor to go up
                motor1 = 0;
                motor2 = 1;
                DCenable = 0.8f;
            } else {
                DCenable = 0;
                open_door();
                wait(10);
                close_door();
            }
        } else { //fifth floor
            pc.printf("go to the fifth floor\r\n");
            if (floorat < 5) {
                //set DC motor to go up
                motor1 = 0;
                motor2 = 1;
                DCenable = 0.8f;
            } else {
                DCenable = 0;
                open_door();
                wait(10);
                close_door();
            }
        }   //end floor checks
        while(!onFloor) {} //wait til we get to the next floor
        floorat = get_floor(1);
    }


    
    return 0;
}