Drew Moore / Mbed 2 deprecated ElevatorLabP2Logic

Logic Control

Dependencies:   Motor mbed

main.cpp@0:c102c15cb7ad, 2016-05-02 (annotated)

Committer:
TheDoctor822
Date:
Mon May 02 19:06:13 2016 +0000
Revision:
0:c102c15cb7ad
Logic Control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TheDoctor822 0:c102c15cb7ad 1 #include "mbed.h"
TheDoctor822 0:c102c15cb7ad 2 #include "Motor.h"
TheDoctor822 0:c102c15cb7ad 3
TheDoctor822 0:c102c15cb7ad 4 Serial pc(USBTX,USBRX);
TheDoctor822 0:c102c15cb7ad 5
TheDoctor822 0:c102c15cb7ad 6 AnalogIn sensor(p19); //PINNUMBER);
TheDoctor822 0:c102c15cb7ad 7 Motor motor(p26, p30, p29); //change pins
TheDoctor822 0:c102c15cb7ad 8
TheDoctor822 0:c102c15cb7ad 9 int trigger; // trigger variable for while loop
TheDoctor822 0:c102c15cb7ad 10 int loop;
TheDoctor822 0:c102c15cb7ad 11 float stop;
TheDoctor822 0:c102c15cb7ad 12 float usrHeight;
TheDoctor822 0:c102c15cb7ad 13 float sensorVolts;
TheDoctor822 0:c102c15cb7ad 14 float height;
TheDoctor822 0:c102c15cb7ad 15 float dc;
TheDoctor822 0:c102c15cb7ad 16 float realSpeed;
TheDoctor822 0:c102c15cb7ad 17 float heightErr;
TheDoctor822 0:c102c15cb7ad 18 float coeff1;
TheDoctor822 0:c102c15cb7ad 19 float coeff2;
TheDoctor822 0:c102c15cb7ad 20 float coeff3;
TheDoctor822 0:c102c15cb7ad 21 float coeff4;
TheDoctor822 0:c102c15cb7ad 22
TheDoctor822 0:c102c15cb7ad 23 int main()
TheDoctor822 0:c102c15cb7ad 24 {
TheDoctor822 0:c102c15cb7ad 25 loop = 1;
TheDoctor822 0:c102c15cb7ad 26 trigger = 1;
TheDoctor822 0:c102c15cb7ad 27 realSpeed = 0.0;
TheDoctor822 0:c102c15cb7ad 28 //find good pwm value for going up (.45)
TheDoctor822 0:c102c15cb7ad 29 //going down(-.45)
TheDoctor822 0:c102c15cb7ad 30 //stopping(0)
TheDoctor822 0:c102c15cb7ad 31 pc.baud(115200);
TheDoctor822 0:c102c15cb7ad 32 motor.speed(0);
TheDoctor822 0:c102c15cb7ad 33
TheDoctor822 0:c102c15cb7ad 34 while(1) {
TheDoctor822 0:c102c15cb7ad 35
TheDoctor822 0:c102c15cb7ad 36 trigger = 1;
TheDoctor822 0:c102c15cb7ad 37 pc.scanf("%d,%f", &trigger, &usrHeight);
TheDoctor822 0:c102c15cb7ad 38
TheDoctor822 0:c102c15cb7ad 39 wait(2.0);
TheDoctor822 0:c102c15cb7ad 40
TheDoctor822 0:c102c15cb7ad 41
TheDoctor822 0:c102c15cb7ad 42 coeff1 = 355.519794247593;
TheDoctor822 0:c102c15cb7ad 43 coeff2 = -502.781241203052;
TheDoctor822 0:c102c15cb7ad 44 coeff3 = 251.402617938315;
TheDoctor822 0:c102c15cb7ad 45 coeff4 = -23.6439180749985;
TheDoctor822 0:c102c15cb7ad 46
TheDoctor822 0:c102c15cb7ad 47 while( trigger==1) {
TheDoctor822 0:c102c15cb7ad 48
TheDoctor822 0:c102c15cb7ad 49 //motor.speed(motorPWM);
TheDoctor822 0:c102c15cb7ad 50 sensorVolts = sensor.read();
TheDoctor822 0:c102c15cb7ad 51 height = coeff1*(sensorVolts * sensorVolts * sensorVolts) + coeff2*(sensorVolts * sensorVolts) + coeff3*(sensorVolts) + coeff4;
TheDoctor822 0:c102c15cb7ad 52 pc.printf("%f\n",height);
TheDoctor822 0:c102c15cb7ad 53 heightErr = usrHeight - height;
TheDoctor822 0:c102c15cb7ad 54 //pc.printf("Input Height: %f\n", usrHeight);
TheDoctor822 0:c102c15cb7ad 55 //pc.printf("Height: %f\n", height);
TheDoctor822 0:c102c15cb7ad 56 //pc.printf("Error: %f\n", heightErr);
TheDoctor822 0:c102c15cb7ad 57 if (heightErr > 0.5){
TheDoctor822 0:c102c15cb7ad 58 realSpeed = 0.6;
TheDoctor822 0:c102c15cb7ad 59 motor.speed(realSpeed);
TheDoctor822 0:c102c15cb7ad 60 }
TheDoctor822 0:c102c15cb7ad 61 if (heightErr < -0.5){
TheDoctor822 0:c102c15cb7ad 62 realSpeed = -0.6;
TheDoctor822 0:c102c15cb7ad 63 motor.speed(realSpeed);
TheDoctor822 0:c102c15cb7ad 64 }
TheDoctor822 0:c102c15cb7ad 65 //pc.printf("%d, %f, %f\n",loop, height, realSpeed);
TheDoctor822 0:c102c15cb7ad 66 pc.printf("%f\n", realSpeed);
TheDoctor822 0:c102c15cb7ad 67 wait(0.1);
TheDoctor822 0:c102c15cb7ad 68 }
TheDoctor822 0:c102c15cb7ad 69 } //need to send over loop, measured height, duty cycle value
TheDoctor822 0:c102c15cb7ad 70
TheDoctor822 0:c102c15cb7ad 71 }