File content as of revision 6:1f9baf8d833e:

#include "mbed.h"

Serial pc(USBTX,USBRX);
PwmOut dc_motor(p23);
PwmOut servo1(p21);
PwmOut servo2(p22);
DigitalInOut in1(p5);
DigitalInOut in2(p6);
AnalogIn an(p16);
InterruptIn firstfloor(p15);
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); 
int first = 0;
Timer time1;
int once = 0;
float stationary = 0.075;
float ccw = 0.085;   // counter clock-wise
float cw = 0.065;    // clock-wise
int reached = 0;
int location;
float frequency;
int requested_floor1, requested_floor2, requested_floor;
int busy = 0;
int currentfloor;

void serve(){
        
    if(busy == 1){ 
        if (first == 0){
            time1.reset();
            time1.start();
            first = 1;
        }
            
        else if (first == 1){
            time1.stop();
            first = 0;
            once++;
            frequency = 1./time1.read();

            
            if (once == 200){
                once = 0;
                
                if (frequency >= 900.0f && frequency <= 1100.0f){currentfloor = 5;}
                else if (frequency >= 600.0f && frequency <= 800.0f){currentfloor = 4;}
                else if (frequency >= 400.0f && frequency <= 590.0f){currentfloor = 3;}
                else if (frequency >= 200.0f && frequency <= 300.0f){currentfloor = 2;}
                else if (frequency >= 50.0f && frequency <= 150.0f){currentfloor = 1;}
        
                if (currentfloor == requested_floor){
                    pc.printf("served:   %d floor frequency:  %f \n",currentfloor,frequency);
            
                    dc_motor.write(0.0f);
                    led1 = 0;
                    led2 = 0;
                    led3 = 0;
                    led4 = 0;
                    in1 = 0;
                    in2 = 0;
                    servo1.write(cw);
                    servo2.write(ccw); 
                    wait(0.33); 
                    servo1.write(stationary);
                    servo2.write(stationary);
                    busy = 0;
                    location = requested_floor;
                    pc.printf("at:   %d \n",location);
                    sensor.fall(0);
                    }

        }
 
}
}
    }
    
void moveUp(){
    dc_motor.write(0.9f);
    in1 = 1;
    in2 = 0;
    }
    
void moveDown(){
    
    dc_motor.write(0.9f);
    in1 = 0;
    in2 = 1;
    }


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 && next > 0.5){
        requested_floor = next;
        if (location > next){
            pc.printf("serving:   %d \n",next);
            closeDoor();
            moveDown();
            busy = 1;
            sensor.fall(&serve);
            }
            
        else if (location < next ){
            pc.printf("serving:   %d \n",next);
            closeDoor();
            moveUp();
            busy = 1;
            sensor.fall(&serve);
            }
    }
}

//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);
    servo1.write(stationary);
    servo2.write(stationary);
    in1.output();
    in2.output();
    in2 = 0;
    in1 = 0;
 //   firstfloor.rise(&call1); 
 //   secondfloor.rise(&call2); 
 //   thirdfloor.rise(&call3); 
 //   fourthfloor.rise(&call4); 
 //   fifthfloor.rise(&call5);
    location = 1;
 //   

    while(1){
        
        while(busy == 0){
        
            if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor1 = 1;}      //1
            else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor1 = 2;}//2
            else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor1 = 3;} //3
            else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor1 = 4;}  //4
            else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor1 = 5;}  //5
            else{requested_floor1 = 0;}
            
            if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor2 = 1;led1 = 1;}    
            else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor2 = 2;led2 = 1;} 
            else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor2 = 3;led3 = 1;} 
            else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor2 = 4; led4 = 1;} 
            else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor2 = 5;}
            else{requested_floor2 = 0;}
            
            if (requested_floor1 != requested_floor2) {requested_floor = 0;}
            else{requested_floor = requested_floor1;}
            
            call(requested_floor); 
            }
 }
}