File content as of revision 0:a8ef6edcfcd4:

// LAB 4 ES202
// 3/C Changdae Hahm and Charles Stabler
// 13FEB15
// Logical Control

#include "mbed.h"
#include "Motor.h"


Serial pc(USBTX, USBRX);   // serial comms
AnalogIn ted(p19);         // sensor pin
Timer t;
Motor m(p26,p29,p30);      // motor control

float read;
float height;
float difference;
float Desired_distance= 7.0;


int main() {
    pc.baud(9600);
    pc.format(7,SerialBase::None,1);
    t.start();
    int time_count=0;
    
    while(1) {
        while(time_count<300){
            read = ted.read();
            height = ((495.425114818726*pow(read,3)) - (734.715379566225*pow(read,2)) + (358.613836921017*read) - 35.5785628052144); // height calculation based on calibration
            pc.printf("%f,%f\n", height,t.read()); // printing height and time for TeraTerm 
        
            difference = Desired_distance - height;
        
            if (abs(difference)<=0.2){  // breaks loop when elevator reaches desired floor
                m.speed(0.0);
                break;
            }   
            else if (height > Desired_distance){ // transit to floor
                m.speed(.9);
            }
            else if (height < Desired_distance){ // transit to floor
                m.speed(-.9);
            }
            time_count=time_count+1;
        }
        if (Desired_distance!=18.0){ // changes desired floor once elevator reaches floor
            Desired_distance=18.0;
        }
        else if (Desired_distance!=7.0){ // changes desired floor once elevator reaches floor
            Desired_distance=7.0;
        }
    }
}