course work

Dependencies:   LinkedList mbed

elevator.cpp

Committer:
sahabi
Date:
2015-10-08
Revision:
2:07f2fe858dbd
Parent:
1:28120714ad80
Child:
3:19106c977df1

File content as of revision 2:07f2fe858dbd:

#include "mbed.h"

PwmOut servo1(p21);
PwmOut servo2(p22);
PwmOut dc_motor(p23);
DigitalInOut in1(p5);
DigitalInOut in2(p6);
InterruptIn firstfloor(p7);
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); 
Timeout timeout;
int first = 0;
Timer time1;
int once = 0;
float stationary = 0.075;
int closed;
float ccw = 0.085;   // counter clock-wise
float cw = 0.065;    // clock-wise
int reached = 0;
int location = 1;
float frequency;
int 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 ==2){
            once = 0;
            if (frequency >= 900 && frequency <= 1100){currentfloor = 5;}
            else if (frequency >= 700 && frequency <= 800){currentfloor = 4;led4 = !led4;}
            else if (frequency >= 450 && frequency <= 550){currentfloor = 3;led3 = !led3;}
            else if (frequency >= 220 && frequency <= 290){currentfloor = 2;led2 = !led2;}
            else if (frequency >= 70 && frequency <= 150){currentfloor = 1;led1 = !led1;}
    
        if (currentfloor == requested_floor){
            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;
            }

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

void stop(){
    in1 = 0;
    in2 = 0;
    }

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){
        requested_floor = next;
        if (location > next){
            closeDoor();
            moveDown();
            busy = 1;
            }
            
        else if (location < next){
            closeDoor();
            moveUp();
            busy = 1;
            }
    }
}

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

    sensor.rise(&serve);

    while(1){    
 }
}