File content as of revision 4:a5a7611a8d25:

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

unsigned int freqcnt=0;
unsigned short floorat = 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;
        //pc.printf("on floor\r\n");
    }
    else
        onFloor = false;
//    if(DCenable.read() > 0.0f && freqcnt< 200) {
//        if (freqcnt>90 && freqcnt<110) {    //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;
//        } 
//        pc.printf("floor: %d\r\n",floorat);
//    }
//    pc.printf("freqcnt: %d\r\n",freqcnt);
//    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_requested(float voltage){
    if(voltage < 0.25f)
        return 1;
    else if (voltage < 0.45f)
        return 2;
    else if (voltage < 0.65f)
        return 3;
    else if (voltage < 0.85f)
        return 4;
    else
        return 5;
}
  
short get_floor() {
    DCenable = 0;
    wait(2);
    float avg=0;
    short floorat=0;
    freqcnt=0;
    wait(0.1f);
    avg+=freqcnt;
    freqcnt=0;
    wait(0.1f);
    avg+=freqcnt;
    freqcnt=0;
    wait(0.1f);
    avg+=freqcnt;
    pc.printf("%f\r\n",avg);
    if (avg>270 && avg < 310) {    //fifth floor
        floorat = 5;
    } else if (avg>180) {    //fourth floor
        floorat = 4;
    } else if (avg>120) {    //third floor
        floorat = 3;
    } else if (avg>60) {    //second floor
        floorat = 2;
    } else if(avg > 0) { //at first floor
        floorat = 1;
    }  

    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;
    }
    wait(0.5f);
    DCenable = 0;
    short current_floor;
    pc.printf("at floor: %d\r\n",get_floor());
    pc.printf("go to floor: %d\r\n",get_requested(request));
    while(1) {
        if(get_floor() !=0)
            current_floor = get_floor();
    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;
        }
    
    while(onFloor) {
        wait(1);
        if(get_floor()!=0)
            current_floor = get_floor();
        pc.printf("at floor: %d\r\n",current_floor);
        DCenable = 0.8f;
        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("off current floor\r\n");
    
    while(!onFloor) {    //go to the first floor below us
        DCenable = 0.8f;
    }
    wait(0.5f);
    DCenable = 0;
    
    wait(2);
    
    pc.printf("at floor: %d\r\n",current_floor);
    pc.printf("go to floor: %d\r\n",get_requested(request));
    
    }
        
 //   floorat = get_floor();
    
    //pc.printf("floor %d\r\n",floorat);
    
    
    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.25f) { //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.41f) { // 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 <= 0.62f) { // 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.85f) { // 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
//        
//        wait(1);
//        if(freqcnt > 120){
//            DCenable = 0;
//            wait(2);
//        }
        //while(1) {
//            //DCenable = 0;
//            wait(5);
//            pc.printf("floor %d\r\n",floorat);
//        } //wait til we get to the next floor
        
        
    //}


    
    return 0;
}