Test program for Vertical Control Experiment (Elevator) for ES202/SY202 in the Weapons and Systems Engineering Department.
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?
User | Revision | Line number | New 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 |