File content as of revision 9:95cbc05bd81d:

#include "mbed.h"
#include "LinkedList.h"

int serving;
LinkedList<node>list;
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 open = 0.085;   // counter clock-wise
float closed = 0.040;    // clock-wise
float open2 = 0.100;
float closed2 = 0.055;
int reached = 0;
int location;
float frequency;
int requested_floor1, requested_floor2, requested_floor;
int busy = 0;
int currentfloor;
int globedir = 2;


int inqueue(int request){
    int i;
    int exist = 0;
    node *tmp;
    for (i = 1; i<= list.length();i++){
        tmp = list.pop(i);
        if (request == (int) tmp->data)
            exist = 1;
            }
    return exist;
}

int dir(int current, int next){
    int direction;
    if (current > next)
        direction = 0;
    if (current <= next)
        direction = 1;
return direction;
}
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 == 50){
                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 == serving){
                    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;
                    busy = 0;
                    servo1.write(open);
                    servo2.write(open2);
                    wait(1); 
                    location = serving;
                    pc.printf("at:   %d \n",location);
                    sensor.fall(0);
                    }

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


void closeDoor(){
    servo1.write(closed);
    servo2.write(closed2); 
}

void openDoor(){
    servo1.write(open);
    servo2.write(open2); 
}

void call(int next) {
    node *tmp;
    int i;
    int assigned = 0;
    
    if (list.length() < 4 && next > 0 && inqueue(next) == 0){
        list.append((int *)next);
        }
    for (i = 0; i<= list.length();i++){
        tmp = list.pop(i);    
        pc.printf(" %d \n",(int)tmp->data);
        }       
    if (busy == 0 && (int)list.pop(1)->data < 6){
        
        if (globedir == 2){
                    tmp = list.pop(1);
                    serving = (int)tmp->data;
                    list.remove(1);
                    assigned = 1;
                    }
                    
        else if (globedir == 1){
            for (i = 1; i<= list.length();i++){
                tmp = list.pop(i);
                if (dir(location,(int) tmp->data) == 1){
                    serving = (int)tmp->data;
                    list.remove(i);
                    assigned = 1;
                    break;
                }
                }
            }
            
        else if (globedir == 0){
            for (i = 1; i<= list.length();i++){
                tmp = list.pop(i);
                if (dir(location,(int) tmp->data) == 0){
                    serving = (int)tmp->data;
                    list.remove(i);
                    assigned = 1;
                    break;
            }
        }        
            }
        if (assigned != 1){
            tmp = list.pop(1);
            serving = (int)tmp->data;
            list.remove(1);
            }
            
            
        if (location > serving){
            pc.printf("serving:   %d \n",serving);
            globedir = 0;
            closeDoor();
            moveDown();
            busy = 1;
            sensor.fall(&serve);
            }
            
        else if (location < serving ){
            pc.printf("serving:   %d \n",serving);
            globedir = 1;
            closeDoor();
            moveUp();
            busy = 1;
            sensor.fall(&serve);
            }
            
    }
    }
    
int main() {
    servo1.period(0.02f);
    servo2.period(0.02f);
    in1.output();
    in2.output();
    in2 = 0;
    in1 = 0; 
    location = 2;
    while(1){
        
        while(list.length() <= 4){
        
            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;}
            if ((inqueue(requested_floor) == 0 && requested_floor != 0) || list.length() > 0){
                call(requested_floor); 
                }
}
}
}