Charles Stabler
/
LogicalMotorControl
Logical control algorithm for lab 4
Diff: main.cpp
- Revision:
- 0:a8ef6edcfcd4
diff -r 000000000000 -r a8ef6edcfcd4 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Mar 02 14:38:34 2015 +0000 @@ -0,0 +1,54 @@ +// 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; + } + } +}