Charles Stabler
/
LogicalMotorControl
Logical control algorithm for lab 4
main.cpp
- Committer:
- cstab94
- Date:
- 2015-03-02
- Revision:
- 0:a8ef6edcfcd4
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; } } }