Joseph Bradshaw / Mbed 2 deprecated mbed_elevator_PID_test

Test program for Vertical Control Experiment (Elevator) for ES202/SY202 in the Weapons and Systems Engineering Department.

Dependencies:   MotCon PID mbed

main.cpp@2:36cffabd0c01, 2018-09-20 (annotated)

Committer:
jebradshaw
Date:
Thu Sep 20 18:16:16 2018 +0000
Revision:
2:36cffabd0c01
Parent:
0:dafd17dd0807 
ES20X Board Example with the WSE Elevator and the GP2D12 range finder for feedback.  Crudely configured gain values.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:dafd17dd0807 1 //mbed ES201 Dev Board (V1.1) Elevator Tester
jebradshaw 2:36cffabd0c01 2 // Pololu Motor - https://www.pololu.com/product/2821
jebradshaw 2:36cffabd0c01 3 // 12 V: 11,000 RPM and 300 mA free-run, 5 oz-in (0.3 kg-cm) and 5 A stall
jebradshaw 2:36cffabd0c01 4
jebradshaw 0:dafd17dd0807 5 #include "mbed.h"
jebradshaw 0:dafd17dd0807 6 #include "MotCon.h"
jebradshaw 0:dafd17dd0807 7 #include "PID.h"
jebradshaw 0:dafd17dd0807 8
jebradshaw 0:dafd17dd0807 9 #define VCC 5.0f
jebradshaw 0:dafd17dd0807 10
jebradshaw 2:36cffabd0c01 11 PID controller(0.0, 0.0, 0.0, .01);
jebradshaw 0:dafd17dd0807 12 Serial pc(USBTX, USBRX);
jebradshaw 2:36cffabd0c01 13 AnalogIn ir_range(p20);
jebradshaw 0:dafd17dd0807 14 AnalogIn sonar_range(p16);
jebradshaw 0:dafd17dd0807 15 InterruptIn sonar_pulse(p17);
jebradshaw 0:dafd17dd0807 16 DigitalIn sonar_pulse_pin(p17);
jebradshaw 2:36cffabd0c01 17 MotCon mot(p25, p27, p28);
jebradshaw 0:dafd17dd0807 18 DigitalOut led1(LED1);
jebradshaw 0:dafd17dd0807 19 DigitalOut led2(LED2);
jebradshaw 0:dafd17dd0807 20
jebradshaw 0:dafd17dd0807 21 Timer t;
jebradshaw 0:dafd17dd0807 22 Timer pulse_t;
jebradshaw 0:dafd17dd0807 23 volatile float pulse_dist=0.0;
jebradshaw 0:dafd17dd0807 24
jebradshaw 0:dafd17dd0807 25 volatile float co;
jebradshaw 2:36cffabd0c01 26 volatile float Tdelay = .001;
jebradshaw 2:36cffabd0c01 27 volatile float T1_Pk =1.6f;
jebradshaw 2:36cffabd0c01 28 volatile float T1_Ik =0.45f;
jebradshaw 0:dafd17dd0807 29 volatile float T1_Dk =0.0;
jebradshaw 0:dafd17dd0807 30 volatile float T1_set_point1 = 0.0;
jebradshaw 0:dafd17dd0807 31 volatile float dist, ir_volts;
jebradshaw 0:dafd17dd0807 32
jebradshaw 0:dafd17dd0807 33 float get_range(void){
jebradshaw 0:dafd17dd0807 34 double v_avg=0.0;
jebradshaw 0:dafd17dd0807 35 for(int i=0;i<100;i++){
jebradshaw 0:dafd17dd0807 36 v_avg += ir_range * 3.3;
jebradshaw 0:dafd17dd0807 37 }
jebradshaw 0:dafd17dd0807 38 double volts = v_avg/100.0;
jebradshaw 0:dafd17dd0807 39 //Employ range equation here
jebradshaw 0:dafd17dd0807 40 double distance = (4.5*pow(volts,3))-(27.0*pow(volts, 2))+(57.0*volts)-19.0;
jebradshaw 0:dafd17dd0807 41
jebradshaw 0:dafd17dd0807 42 //double distance = volts;
jebradshaw 0:dafd17dd0807 43 return distance;
jebradshaw 0:dafd17dd0807 44 }
jebradshaw 0:dafd17dd0807 45
jebradshaw 0:dafd17dd0807 46 void pulse_measure(void){
jebradshaw 0:dafd17dd0807 47 if(sonar_pulse_pin){ //if pulse pin is high
jebradshaw 0:dafd17dd0807 48 pulse_t.start(); //start the pulse timer
jebradshaw 0:dafd17dd0807 49 sonar_pulse.fall(&pulse_measure); //set next interrupt for falling edge
jebradshaw 0:dafd17dd0807 50 }
jebradshaw 0:dafd17dd0807 51 else{ //if pulse pin is low
jebradshaw 0:dafd17dd0807 52 pulse_dist = pulse_t.read_us(); //read the pulse timer
jebradshaw 0:dafd17dd0807 53 pulse_t.stop(); //stop the timer
jebradshaw 0:dafd17dd0807 54 pulse_t.reset(); //reset timer to 0
jebradshaw 0:dafd17dd0807 55 sonar_pulse.rise(&pulse_measure); //reset interrupt for rising edge
jebradshaw 0:dafd17dd0807 56 }
jebradshaw 0:dafd17dd0807 57 }
jebradshaw 2:36cffabd0c01 58
jebradshaw 0:dafd17dd0807 59 int main() {
jebradshaw 2:36cffabd0c01 60 wait(.2);
jebradshaw 0:dafd17dd0807 61 pc.baud(921600);
jebradshaw 2:36cffabd0c01 62
jebradshaw 0:dafd17dd0807 63 sonar_pulse.rise(&pulse_measure);
jebradshaw 0:dafd17dd0807 64 t.start();
jebradshaw 0:dafd17dd0807 65
jebradshaw 0:dafd17dd0807 66 //resets the controllers internals
jebradshaw 0:dafd17dd0807 67 controller.reset();
jebradshaw 0:dafd17dd0807 68 pc.printf("Waiting 2 seconds\r\n");
jebradshaw 0:dafd17dd0807 69 wait(2.0);
jebradshaw 0:dafd17dd0807 70 //Distance sensor limits
jebradshaw 0:dafd17dd0807 71 controller.setInputLimits(6.0, 24.0); //0 inches to 26 inches
jebradshaw 0:dafd17dd0807 72 //Pwm output from -1.0 to 1.0
jebradshaw 2:36cffabd0c01 73 controller.setOutputLimits(-.72, 0.72);
jebradshaw 0:dafd17dd0807 74 //If there's a bias.
jebradshaw 0:dafd17dd0807 75 controller.setBias(0.0);
jebradshaw 0:dafd17dd0807 76 controller.setMode(AUTO_MODE);
jebradshaw 0:dafd17dd0807 77
jebradshaw 0:dafd17dd0807 78 controller.setInterval(Tdelay);
jebradshaw 0:dafd17dd0807 79
jebradshaw 0:dafd17dd0807 80 //We want the process variable to be 0.0 at start
jebradshaw 0:dafd17dd0807 81 controller.setSetPoint(T1_set_point1);
jebradshaw 0:dafd17dd0807 82 controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
jebradshaw 0:dafd17dd0807 83
jebradshaw 2:36cffabd0c01 84 int state = 0;
jebradshaw 2:36cffabd0c01 85 float sp = 10.0;
jebradshaw 0:dafd17dd0807 86
jebradshaw 0:dafd17dd0807 87 while(1){
jebradshaw 2:36cffabd0c01 88
jebradshaw 0:dafd17dd0807 89 switch(state){
jebradshaw 0:dafd17dd0807 90 case 0:
jebradshaw 0:dafd17dd0807 91 sp = 10.0; //10 inches
jebradshaw 0:dafd17dd0807 92 state = 1;
jebradshaw 0:dafd17dd0807 93 break;
jebradshaw 0:dafd17dd0807 94 case 1:
jebradshaw 0:dafd17dd0807 95 sp = 13.0; //13 inches
jebradshaw 0:dafd17dd0807 96 state = 2;
jebradshaw 0:dafd17dd0807 97 break;
jebradshaw 0:dafd17dd0807 98 case 2:
jebradshaw 0:dafd17dd0807 99 sp = 16.0; //16 inches
jebradshaw 0:dafd17dd0807 100 state = 3;
jebradshaw 0:dafd17dd0807 101 break;
jebradshaw 0:dafd17dd0807 102 case 3:
jebradshaw 2:36cffabd0c01 103 sp = 20.0; //22 inches
jebradshaw 0:dafd17dd0807 104 state = 4;
jebradshaw 0:dafd17dd0807 105 break;
jebradshaw 0:dafd17dd0807 106 case 4:
jebradshaw 2:36cffabd0c01 107 sp = 13.0; //13 inches
jebradshaw 0:dafd17dd0807 108 state = 0;
jebradshaw 0:dafd17dd0807 109 break;
jebradshaw 0:dafd17dd0807 110 default:
jebradshaw 0:dafd17dd0807 111 sp = 0.0;
jebradshaw 0:dafd17dd0807 112 state = 0;
jebradshaw 0:dafd17dd0807 113 break;
jebradshaw 2:36cffabd0c01 114 } //switch
jebradshaw 0:dafd17dd0807 115
jebradshaw 0:dafd17dd0807 116 //set the set point
jebradshaw 0:dafd17dd0807 117 controller.setSetPoint(sp);
jebradshaw 0:dafd17dd0807 118
jebradshaw 0:dafd17dd0807 119 float timeLapse = t.read() + 5.0;
jebradshaw 0:dafd17dd0807 120
jebradshaw 0:dafd17dd0807 121 while(t.read() < timeLapse){
jebradshaw 0:dafd17dd0807 122 if(t.read() < 0.0) //if timer goes negative, reset to 0.0
jebradshaw 0:dafd17dd0807 123 {
jebradshaw 0:dafd17dd0807 124 t.reset();
jebradshaw 0:dafd17dd0807 125 timeLapse = t.read() + 5.0;
jebradshaw 0:dafd17dd0807 126 }
jebradshaw 0:dafd17dd0807 127
jebradshaw 0:dafd17dd0807 128 if(pc.readable())
jebradshaw 0:dafd17dd0807 129 {
jebradshaw 0:dafd17dd0807 130 char c = pc.getc();
jebradshaw 0:dafd17dd0807 131 if(c == 'S' || c == 's')
jebradshaw 0:dafd17dd0807 132 {
jebradshaw 0:dafd17dd0807 133 pc.printf("Enter value for set point\r\n");
jebradshaw 0:dafd17dd0807 134 pc.scanf("%f",&sp);
jebradshaw 0:dafd17dd0807 135 controller.setSetPoint(sp);
jebradshaw 0:dafd17dd0807 136 pc.printf("%f\r\n",sp);
jebradshaw 0:dafd17dd0807 137 }
jebradshaw 0:dafd17dd0807 138 if(c == 'P' || c == 'p')
jebradshaw 0:dafd17dd0807 139 {
jebradshaw 0:dafd17dd0807 140 pc.printf("Enter value for Pk\r\n");
jebradshaw 0:dafd17dd0807 141 pc.scanf("%f",&T1_Pk);
jebradshaw 0:dafd17dd0807 142 pc.printf("%f\r\n",T1_Pk);
jebradshaw 0:dafd17dd0807 143 }
jebradshaw 0:dafd17dd0807 144 if(c == 'I' || c == 'i')
jebradshaw 0:dafd17dd0807 145 {
jebradshaw 0:dafd17dd0807 146 pc.printf("Enter value for Ik\r\n");
jebradshaw 0:dafd17dd0807 147 pc.scanf("%f",&T1_Ik);
jebradshaw 0:dafd17dd0807 148 pc.printf("%f\r\n",T1_Ik);
jebradshaw 0:dafd17dd0807 149 wait(1.0);
jebradshaw 0:dafd17dd0807 150 }
jebradshaw 2:36cffabd0c01 151 if(c == 'D' || c == 'd')
jebradshaw 2:36cffabd0c01 152 {
jebradshaw 2:36cffabd0c01 153 pc.printf("Enter value for Dk\r\n");
jebradshaw 2:36cffabd0c01 154 pc.scanf("%f",&T1_Dk);
jebradshaw 2:36cffabd0c01 155 pc.printf("%f\r\n",T1_Dk);
jebradshaw 2:36cffabd0c01 156 wait(1.0);
jebradshaw 2:36cffabd0c01 157 }
jebradshaw 0:dafd17dd0807 158 if(c == 'T' || c == 't')
jebradshaw 0:dafd17dd0807 159 {
jebradshaw 0:dafd17dd0807 160 pc.printf("Enter value for Tdelay\r\n");
jebradshaw 0:dafd17dd0807 161 pc.scanf("%f",&Tdelay);
jebradshaw 0:dafd17dd0807 162 controller.setInterval(Tdelay);
jebradshaw 0:dafd17dd0807 163 pc.printf("%f\r\n",Tdelay);
jebradshaw 0:dafd17dd0807 164 }
jebradshaw 0:dafd17dd0807 165
jebradshaw 0:dafd17dd0807 166 //Just read the sensors
jebradshaw 0:dafd17dd0807 167 if(c == 'R' || c == 'r')
jebradshaw 0:dafd17dd0807 168 {
jebradshaw 0:dafd17dd0807 169 mot= 0.0;
jebradshaw 0:dafd17dd0807 170 sp = 0.0;
jebradshaw 0:dafd17dd0807 171 state = 0;
jebradshaw 0:dafd17dd0807 172 while(1){
jebradshaw 0:dafd17dd0807 173 float dist = get_range();
jebradshaw 0:dafd17dd0807 174 float ir_volts = ir_range * 3.3;
jebradshaw 0:dafd17dd0807 175
jebradshaw 0:dafd17dd0807 176 pc.printf("%.2f,%.2f\r\n", dist, ir_volts);
jebradshaw 0:dafd17dd0807 177 wait(.05);
jebradshaw 2:36cffabd0c01 178 if(pc.readable()){
jebradshaw 2:36cffabd0c01 179 char c = pc.getc();
jebradshaw 0:dafd17dd0807 180 break;
jebradshaw 2:36cffabd0c01 181 }
jebradshaw 0:dafd17dd0807 182 }
jebradshaw 0:dafd17dd0807 183 char c = pc.getc();
jebradshaw 0:dafd17dd0807 184 c = 0;
jebradshaw 0:dafd17dd0807 185 }
jebradshaw 0:dafd17dd0807 186 controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
jebradshaw 0:dafd17dd0807 187 //T1_P = 0.0;
jebradshaw 0:dafd17dd0807 188 //T1_I = 0.0; //reset integral
jebradshaw 0:dafd17dd0807 189 }
jebradshaw 2:36cffabd0c01 190
jebradshaw 0:dafd17dd0807 191 dist = get_range();
jebradshaw 0:dafd17dd0807 192 ir_volts = ir_range * 3.3;
jebradshaw 0:dafd17dd0807 193
jebradshaw 0:dafd17dd0807 194 //Update the process variable.
jebradshaw 0:dafd17dd0807 195 controller.setProcessValue(dist);
jebradshaw 0:dafd17dd0807 196 //Set the new output.
jebradshaw 0:dafd17dd0807 197 co = controller.compute();
jebradshaw 0:dafd17dd0807 198
jebradshaw 2:36cffabd0c01 199 mot.mot_control(-co); //-co?
jebradshaw 0:dafd17dd0807 200
jebradshaw 0:dafd17dd0807 201 led2 = 1;
jebradshaw 0:dafd17dd0807 202
jebradshaw 0:dafd17dd0807 203 wait(Tdelay);
jebradshaw 2:36cffabd0c01 204 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);
jebradshaw 0:dafd17dd0807 205 //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);
jebradshaw 0:dafd17dd0807 206 led2 = 0;
jebradshaw 0:dafd17dd0807 207
jebradshaw 0:dafd17dd0807 208 if(dist > 23.0){
jebradshaw 0:dafd17dd0807 209 mot= 0.0;
jebradshaw 0:dafd17dd0807 210
jebradshaw 0:dafd17dd0807 211 pc.printf("Caution: Elevator too high. Lowering for 2 sec.\r\n");
jebradshaw 0:dafd17dd0807 212 for(int i=2;i>=0;i--){
jebradshaw 0:dafd17dd0807 213 pc.printf("%d sec left \r", i);
jebradshaw 0:dafd17dd0807 214 wait(1);
jebradshaw 0:dafd17dd0807 215 if(pc.readable()){
jebradshaw 0:dafd17dd0807 216 char c = pc.getc();
jebradshaw 0:dafd17dd0807 217 if(c=='r')
jebradshaw 0:dafd17dd0807 218 c='r';
jebradshaw 0:dafd17dd0807 219 else
jebradshaw 0:dafd17dd0807 220 c=0;
jebradshaw 0:dafd17dd0807 221 break;
jebradshaw 0:dafd17dd0807 222 }
jebradshaw 2:36cffabd0c01 223 }
jebradshaw 2:36cffabd0c01 224 state=0;
jebradshaw 0:dafd17dd0807 225 pc.printf("\r\n");
jebradshaw 0:dafd17dd0807 226 }
jebradshaw 0:dafd17dd0807 227 } //while(t.read() < timeLapse)
jebradshaw 0:dafd17dd0807 228 } //while(1)
jebradshaw 0:dafd17dd0807 229 }//main