Charles Stabler / Mbed 2 deprecated LogicalMotorControl

Logical control algorithm for lab 4

Dependencies:   Motor mbed

main.cpp@0:a8ef6edcfcd4, 2015-03-02 (annotated)

Committer:
cstab94
Date:
Mon Mar 02 14:38:34 2015 +0000
Revision:
0:a8ef6edcfcd4
Logical control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cstab94 0:a8ef6edcfcd4 1 // LAB 4 ES202
cstab94 0:a8ef6edcfcd4 2 // 3/C Changdae Hahm and Charles Stabler
cstab94 0:a8ef6edcfcd4 3 // 13FEB15
cstab94 0:a8ef6edcfcd4 4 // Logical Control
cstab94 0:a8ef6edcfcd4 5
cstab94 0:a8ef6edcfcd4 6 #include "mbed.h"
cstab94 0:a8ef6edcfcd4 7 #include "Motor.h"
cstab94 0:a8ef6edcfcd4 8
cstab94 0:a8ef6edcfcd4 9
cstab94 0:a8ef6edcfcd4 10 Serial pc(USBTX, USBRX); // serial comms
cstab94 0:a8ef6edcfcd4 11 AnalogIn ted(p19); // sensor pin
cstab94 0:a8ef6edcfcd4 12 Timer t;
cstab94 0:a8ef6edcfcd4 13 Motor m(p26,p29,p30); // motor control
cstab94 0:a8ef6edcfcd4 14
cstab94 0:a8ef6edcfcd4 15 float read;
cstab94 0:a8ef6edcfcd4 16 float height;
cstab94 0:a8ef6edcfcd4 17 float difference;
cstab94 0:a8ef6edcfcd4 18 float Desired_distance= 7.0;
cstab94 0:a8ef6edcfcd4 19
cstab94 0:a8ef6edcfcd4 20
cstab94 0:a8ef6edcfcd4 21 int main() {
cstab94 0:a8ef6edcfcd4 22 pc.baud(9600);
cstab94 0:a8ef6edcfcd4 23 pc.format(7,SerialBase::None,1);
cstab94 0:a8ef6edcfcd4 24 t.start();
cstab94 0:a8ef6edcfcd4 25 int time_count=0;
cstab94 0:a8ef6edcfcd4 26
cstab94 0:a8ef6edcfcd4 27 while(1) {
cstab94 0:a8ef6edcfcd4 28 while(time_count<300){
cstab94 0:a8ef6edcfcd4 29 read = ted.read();
cstab94 0:a8ef6edcfcd4 30 height = ((495.425114818726*pow(read,3)) - (734.715379566225*pow(read,2)) + (358.613836921017*read) - 35.5785628052144); // height calculation based on calibration
cstab94 0:a8ef6edcfcd4 31 pc.printf("%f,%f\n", height,t.read()); // printing height and time for TeraTerm
cstab94 0:a8ef6edcfcd4 32
cstab94 0:a8ef6edcfcd4 33 difference = Desired_distance - height;
cstab94 0:a8ef6edcfcd4 34
cstab94 0:a8ef6edcfcd4 35 if (abs(difference)<=0.2){ // breaks loop when elevator reaches desired floor
cstab94 0:a8ef6edcfcd4 36 m.speed(0.0);
cstab94 0:a8ef6edcfcd4 37 break;
cstab94 0:a8ef6edcfcd4 38 }
cstab94 0:a8ef6edcfcd4 39 else if (height > Desired_distance){ // transit to floor
cstab94 0:a8ef6edcfcd4 40 m.speed(.9);
cstab94 0:a8ef6edcfcd4 41 }
cstab94 0:a8ef6edcfcd4 42 else if (height < Desired_distance){ // transit to floor
cstab94 0:a8ef6edcfcd4 43 m.speed(-.9);
cstab94 0:a8ef6edcfcd4 44 }
cstab94 0:a8ef6edcfcd4 45 time_count=time_count+1;
cstab94 0:a8ef6edcfcd4 46 }
cstab94 0:a8ef6edcfcd4 47 if (Desired_distance!=18.0){ // changes desired floor once elevator reaches floor
cstab94 0:a8ef6edcfcd4 48 Desired_distance=18.0;
cstab94 0:a8ef6edcfcd4 49 }
cstab94 0:a8ef6edcfcd4 50 else if (Desired_distance!=7.0){ // changes desired floor once elevator reaches floor
cstab94 0:a8ef6edcfcd4 51 Desired_distance=7.0;
cstab94 0:a8ef6edcfcd4 52 }
cstab94 0:a8ef6edcfcd4 53 }
cstab94 0:a8ef6edcfcd4 54 }