Test program for Vertical Control Experiment (Elevator) for ES202/SY202 in the Weapons and Systems Engineering Department.
Diff: main.cpp
- Revision:
- 2:36cffabd0c01
- Parent:
- 0:dafd17dd0807
--- a/main.cpp Fri Mar 16 15:49:02 2018 +0000 +++ b/main.cpp Thu Sep 20 18:16:16 2018 +0000 @@ -1,17 +1,20 @@ //mbed ES201 Dev Board (V1.1) Elevator Tester +// Pololu Motor - https://www.pololu.com/product/2821 +// 12 V: 11,000 RPM and 300 mA free-run, 5 oz-in (0.3 kg-cm) and 5 A stall + #include "mbed.h" #include "MotCon.h" #include "PID.h" #define VCC 5.0f -PID controller(0.0, 0.0, 0.0, .1); +PID controller(0.0, 0.0, 0.0, .01); Serial pc(USBTX, USBRX); -AnalogIn ir_range(p19); +AnalogIn ir_range(p20); AnalogIn sonar_range(p16); InterruptIn sonar_pulse(p17); DigitalIn sonar_pulse_pin(p17); -MotCon mot(p25, p27); +MotCon mot(p25, p27, p28); DigitalOut led1(LED1); DigitalOut led2(LED2); @@ -20,9 +23,9 @@ volatile float pulse_dist=0.0; volatile float co; -volatile float Tdelay = .1; -volatile float T1_Pk =2.3; -volatile float T1_Ik =0.4; +volatile float Tdelay = .001; +volatile float T1_Pk =1.6f; +volatile float T1_Ik =0.45f; volatile float T1_Dk =0.0; volatile float T1_set_point1 = 0.0; volatile float dist, ir_volts; @@ -52,9 +55,11 @@ sonar_pulse.rise(&pulse_measure); //reset interrupt for rising edge } } - + int main() { + wait(.2); pc.baud(921600); + sonar_pulse.rise(&pulse_measure); t.start(); @@ -65,7 +70,7 @@ //Distance sensor limits controller.setInputLimits(6.0, 24.0); //0 inches to 26 inches //Pwm output from -1.0 to 1.0 - controller.setOutputLimits(-0.3, 0.4); + controller.setOutputLimits(-.72, 0.72); //If there's a bias. controller.setBias(0.0); controller.setMode(AUTO_MODE); @@ -76,10 +81,11 @@ controller.setSetPoint(T1_set_point1); controller.setTunings(T1_Pk, T1_Ik, T1_Dk); - int state = 1; - float sp = 0.0; + int state = 0; + float sp = 10.0; while(1){ + switch(state){ case 0: sp = 10.0; //10 inches @@ -94,18 +100,18 @@ state = 3; break; case 3: - sp = 20.0; //20 inches + sp = 20.0; //22 inches state = 4; break; case 4: - sp = 23.0; //23 inches + sp = 13.0; //13 inches state = 0; break; default: sp = 0.0; state = 0; break; - } //switch + } //switch //set the set point controller.setSetPoint(sp); @@ -142,6 +148,13 @@ pc.printf("%f\r\n",T1_Ik); wait(1.0); } + if(c == 'D' || c == 'd') + { + pc.printf("Enter value for Dk\r\n"); + pc.scanf("%f",&T1_Dk); + pc.printf("%f\r\n",T1_Dk); + wait(1.0); + } if(c == 'T' || c == 't') { pc.printf("Enter value for Tdelay\r\n"); @@ -162,8 +175,10 @@ pc.printf("%.2f,%.2f\r\n", dist, ir_volts); wait(.05); - if(pc.readable()) + if(pc.readable()){ + char c = pc.getc(); break; + } } char c = pc.getc(); c = 0; @@ -172,7 +187,7 @@ //T1_P = 0.0; //T1_I = 0.0; //reset integral } - + dist = get_range(); ir_volts = ir_range * 3.3; @@ -181,12 +196,12 @@ //Set the new output. co = controller.compute(); - mot=-co; //-co? + mot.mot_control(-co); //-co? led2 = 1; wait(Tdelay); - pc.printf("T=%.3f SP=%.3f Dist=%.2f co=%.4f Pk=%.3f Ik=%13.10f\r\n", t.read(), sp, dist, co, T1_Pk, T1_Ik); + pc.printf("T=%.3f SP=%.3f Dist=%5.2f co=%6.4f Pk=%6.3f Ik=%7.4f\r\n", t.read(), sp, dist, co, T1_Pk, T1_Ik); //pc.printf("dc=%7.3f ir range=%7.3f sonar analog=%7.3f pulse=%.1f\r\n", (float)mot, get_range(), sonar_An_range, pulse_dist/147.0f); led2 = 0; @@ -205,7 +220,8 @@ c=0; break; } - } + } + state=0; pc.printf("\r\n"); } } //while(t.read() < timeLapse)