![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PIDs and fun
Dependencies: btbee m3pi_ng mbed FatFileSystemCpp
main.cpp@5:3f29267b18e3, 2015-05-27 (annotated)
- Committer:
- crmackey
- Date:
- Wed May 27 12:24:22 2015 +0000
- Revision:
- 5:3f29267b18e3
- Parent:
- 4:0bcd7a924140
- Child:
- 6:75a6f6b50188
Each loop now lasts exactly five miliseconds.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
crmackey | 0:4d1ae7eb0070 | 1 | #include "mbed.h" |
crmackey | 0:4d1ae7eb0070 | 2 | #include "m3pi_ng.h" |
crmackey | 0:4d1ae7eb0070 | 3 | #include "time.h" |
crmackey | 0:4d1ae7eb0070 | 4 | #include "btbee.h" |
crmackey | 0:4d1ae7eb0070 | 5 | |
crmackey | 0:4d1ae7eb0070 | 6 | m3pi m3pi; |
crmackey | 0:4d1ae7eb0070 | 7 | btbee btbee; |
crmackey | 0:4d1ae7eb0070 | 8 | DigitalIn m3pi_pb(p21); |
crmackey | 1:936e329e840a | 9 | DigitalIn m3pi_IN[] = {(p12)}; |
crmackey | 0:4d1ae7eb0070 | 10 | DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)}; |
crmackey | 0:4d1ae7eb0070 | 11 | DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)}; |
crmackey | 0:4d1ae7eb0070 | 12 | |
crmackey | 0:4d1ae7eb0070 | 13 | // Minimum and maximum motor speeds |
crmackey | 0:4d1ae7eb0070 | 14 | #define MAX 1.0 |
crmackey | 0:4d1ae7eb0070 | 15 | #define MIN 0 |
crmackey | 0:4d1ae7eb0070 | 16 | |
crmackey | 0:4d1ae7eb0070 | 17 | // PID terms |
crmackey | 0:4d1ae7eb0070 | 18 | #define P_TERM 1.5 |
crmackey | 0:4d1ae7eb0070 | 19 | #define I_TERM 0 |
crmackey | 0:4d1ae7eb0070 | 20 | #define D_TERM 25 |
crmackey | 0:4d1ae7eb0070 | 21 | |
crmackey | 0:4d1ae7eb0070 | 22 | int main() { |
crmackey | 0:4d1ae7eb0070 | 23 | |
crmackey | 1:936e329e840a | 24 | |
crmackey | 0:4d1ae7eb0070 | 25 | |
crmackey | 0:4d1ae7eb0070 | 26 | btbee.reset(); |
crmackey | 0:4d1ae7eb0070 | 27 | m3pi_pb.mode(PullUp); |
crmackey | 0:4d1ae7eb0070 | 28 | |
crmackey | 0:4d1ae7eb0070 | 29 | m3pi.printf("Wait 4"); |
crmackey | 0:4d1ae7eb0070 | 30 | m3pi.locate(0,1); |
crmackey | 0:4d1ae7eb0070 | 31 | m3pi.printf("PC"); |
crmackey | 0:4d1ae7eb0070 | 32 | |
crmackey | 0:4d1ae7eb0070 | 33 | while(m3pi_pb) { |
crmackey | 0:4d1ae7eb0070 | 34 | m3pi_led[0]=!m3pi_led[0]; |
crmackey | 0:4d1ae7eb0070 | 35 | wait(3); |
crmackey | 0:4d1ae7eb0070 | 36 | btbee.printf("\n"); |
crmackey | 0:4d1ae7eb0070 | 37 | btbee.printf("PC connected. Press the button \n"); |
crmackey | 0:4d1ae7eb0070 | 38 | } |
crmackey | 0:4d1ae7eb0070 | 39 | |
crmackey | 1:936e329e840a | 40 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 41 | |
crmackey | 0:4d1ae7eb0070 | 42 | Timer LapTimer; |
crmackey | 5:3f29267b18e3 | 43 | Timer LoopTime; |
crmackey | 0:4d1ae7eb0070 | 44 | char Bat[] = {'V','o','l','t',' ','i','s'}; |
crmackey | 0:4d1ae7eb0070 | 45 | m3pi.print(Bat,7); |
crmackey | 0:4d1ae7eb0070 | 46 | wait(0.75); |
crmackey | 0:4d1ae7eb0070 | 47 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 48 | float batteryvoltage = m3pi.battery(); |
crmackey | 0:4d1ae7eb0070 | 49 | char* str = new char[30]; |
crmackey | 0:4d1ae7eb0070 | 50 | sprintf(str, "%.4g", batteryvoltage); |
crmackey | 0:4d1ae7eb0070 | 51 | m3pi.print(str,6); |
crmackey | 0:4d1ae7eb0070 | 52 | btbee.printf("Battery voltage is %f \n", batteryvoltage); |
crmackey | 0:4d1ae7eb0070 | 53 | wait(0.75); |
crmackey | 0:4d1ae7eb0070 | 54 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 55 | |
crmackey | 0:4d1ae7eb0070 | 56 | if (batteryvoltage < 4.2) |
crmackey | 0:4d1ae7eb0070 | 57 | { |
crmackey | 0:4d1ae7eb0070 | 58 | char low[] = {'L','o','w',' ','b','a','t'}; |
crmackey | 0:4d1ae7eb0070 | 59 | m3pi.print(low,7); |
crmackey | 0:4d1ae7eb0070 | 60 | char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'}; |
crmackey | 0:4d1ae7eb0070 | 61 | m3pi.playtune(ExitSound,11); |
crmackey | 0:4d1ae7eb0070 | 62 | btbee.printf("Battery voltage is too low. Stopping program"); |
crmackey | 0:4d1ae7eb0070 | 63 | exit(1); |
crmackey | 0:4d1ae7eb0070 | 64 | } |
crmackey | 0:4d1ae7eb0070 | 65 | |
crmackey | 0:4d1ae7eb0070 | 66 | m3pi.locate(0,1); |
crmackey | 0:4d1ae7eb0070 | 67 | m3pi.printf("Line PID"); |
crmackey | 0:4d1ae7eb0070 | 68 | |
crmackey | 0:4d1ae7eb0070 | 69 | wait(2.0); |
crmackey | 0:4d1ae7eb0070 | 70 | |
crmackey | 0:4d1ae7eb0070 | 71 | btbee.printf("Now calibrating \n"); |
crmackey | 0:4d1ae7eb0070 | 72 | |
crmackey | 0:4d1ae7eb0070 | 73 | m3pi.sensor_auto_calibrate(); |
crmackey | 0:4d1ae7eb0070 | 74 | |
crmackey | 0:4d1ae7eb0070 | 75 | btbee.printf("Finished calibrating \n"); |
crmackey | 0:4d1ae7eb0070 | 76 | |
crmackey | 0:4d1ae7eb0070 | 77 | |
crmackey | 0:4d1ae7eb0070 | 78 | float right; |
crmackey | 0:4d1ae7eb0070 | 79 | float left; |
crmackey | 0:4d1ae7eb0070 | 80 | float current_pos_of_line = 0.0; |
crmackey | 0:4d1ae7eb0070 | 81 | float previous_pos_of_line = 0.0; |
crmackey | 0:4d1ae7eb0070 | 82 | float derivative,proportional,integral = 0; |
crmackey | 0:4d1ae7eb0070 | 83 | float power; |
crmackey | 0:4d1ae7eb0070 | 84 | float speed = MAX; |
crmackey | 0:4d1ae7eb0070 | 85 | int LapTest[5]; |
crmackey | 0:4d1ae7eb0070 | 86 | int s1, s2, s3, s4, s5; |
crmackey | 0:4d1ae7eb0070 | 87 | int counter = -1; |
crmackey | 0:4d1ae7eb0070 | 88 | int checkvar = 0; |
crmackey | 0:4d1ae7eb0070 | 89 | double Time1, Time2, Time3, Time4, Time5, TimeAve; |
crmackey | 3:ebe015c72bba | 90 | bool IR = 1; |
crmackey | 5:3f29267b18e3 | 91 | double timer; |
crmackey | 0:4d1ae7eb0070 | 92 | btbee.printf("\n"); |
crmackey | 0:4d1ae7eb0070 | 93 | btbee.printf("Now starting \n"); |
crmackey | 0:4d1ae7eb0070 | 94 | |
crmackey | 0:4d1ae7eb0070 | 95 | |
crmackey | 1:936e329e840a | 96 | |
crmackey | 0:4d1ae7eb0070 | 97 | |
crmackey | 1:936e329e840a | 98 | while (1) { |
crmackey | 1:936e329e840a | 99 | |
crmackey | 5:3f29267b18e3 | 100 | LoopTime.start(); |
crmackey | 5:3f29267b18e3 | 101 | |
crmackey | 3:ebe015c72bba | 102 | IR = m3pi_IN [0]; |
crmackey | 3:ebe015c72bba | 103 | |
crmackey | 3:ebe015c72bba | 104 | if(m3pi_IN [0] == 0 and IR == 0) { |
crmackey | 3:ebe015c72bba | 105 | while (m3pi_IN [0] == 0 and IR == 0) { |
crmackey | 4:0bcd7a924140 | 106 | m3pi.cls(); |
crmackey | 4:0bcd7a924140 | 107 | btbee.printf("Obstacle detected. Ending program"); |
crmackey | 4:0bcd7a924140 | 108 | m3pi.printf("ABORT"); |
crmackey | 2:1e947dcc40cb | 109 | m3pi.stop(); |
crmackey | 4:0bcd7a924140 | 110 | exit(1); |
crmackey | 2:1e947dcc40cb | 111 | } |
crmackey | 2:1e947dcc40cb | 112 | } |
crmackey | 4:0bcd7a924140 | 113 | |
crmackey | 2:1e947dcc40cb | 114 | |
crmackey | 2:1e947dcc40cb | 115 | // Get the position of the line. |
crmackey | 0:4d1ae7eb0070 | 116 | current_pos_of_line = m3pi.line_position(); |
crmackey | 0:4d1ae7eb0070 | 117 | proportional = current_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 118 | |
crmackey | 0:4d1ae7eb0070 | 119 | // Compute the derivative |
crmackey | 0:4d1ae7eb0070 | 120 | derivative = current_pos_of_line - previous_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 121 | |
crmackey | 0:4d1ae7eb0070 | 122 | // Compute the integral |
crmackey | 0:4d1ae7eb0070 | 123 | integral += proportional; |
crmackey | 0:4d1ae7eb0070 | 124 | |
crmackey | 0:4d1ae7eb0070 | 125 | // Remember the last position. |
crmackey | 0:4d1ae7eb0070 | 126 | previous_pos_of_line = current_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 127 | |
crmackey | 0:4d1ae7eb0070 | 128 | // Compute the power |
crmackey | 0:4d1ae7eb0070 | 129 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
crmackey | 0:4d1ae7eb0070 | 130 | |
crmackey | 0:4d1ae7eb0070 | 131 | // Compute new speeds |
crmackey | 0:4d1ae7eb0070 | 132 | right = speed+power; |
crmackey | 0:4d1ae7eb0070 | 133 | left = speed-power; |
crmackey | 0:4d1ae7eb0070 | 134 | |
crmackey | 0:4d1ae7eb0070 | 135 | // limit checks |
crmackey | 0:4d1ae7eb0070 | 136 | if (right < MIN) |
crmackey | 0:4d1ae7eb0070 | 137 | right = MIN; |
crmackey | 0:4d1ae7eb0070 | 138 | else if (right > MAX) |
crmackey | 0:4d1ae7eb0070 | 139 | right = MAX; |
crmackey | 0:4d1ae7eb0070 | 140 | |
crmackey | 0:4d1ae7eb0070 | 141 | if (left < MIN) |
crmackey | 0:4d1ae7eb0070 | 142 | left = MIN; |
crmackey | 0:4d1ae7eb0070 | 143 | else if (left > MAX) |
crmackey | 0:4d1ae7eb0070 | 144 | left = MAX; |
crmackey | 0:4d1ae7eb0070 | 145 | |
crmackey | 0:4d1ae7eb0070 | 146 | // set speed |
crmackey | 0:4d1ae7eb0070 | 147 | m3pi.left_motor(left); |
crmackey | 0:4d1ae7eb0070 | 148 | m3pi.right_motor(right); |
crmackey | 0:4d1ae7eb0070 | 149 | |
crmackey | 0:4d1ae7eb0070 | 150 | m3pi.calibrated_sensor(LapTest); |
crmackey | 0:4d1ae7eb0070 | 151 | |
crmackey | 0:4d1ae7eb0070 | 152 | s1 = LapTest[0]; |
crmackey | 0:4d1ae7eb0070 | 153 | s2 = LapTest[1]; |
crmackey | 0:4d1ae7eb0070 | 154 | s3 = LapTest[2]; |
crmackey | 0:4d1ae7eb0070 | 155 | s4 = LapTest[3]; |
crmackey | 0:4d1ae7eb0070 | 156 | s5 = LapTest[4]; |
crmackey | 0:4d1ae7eb0070 | 157 | |
crmackey | 0:4d1ae7eb0070 | 158 | if (s1 > 500 and s5 > 500) { |
crmackey | 0:4d1ae7eb0070 | 159 | checkvar = 1; |
crmackey | 0:4d1ae7eb0070 | 160 | } |
crmackey | 0:4d1ae7eb0070 | 161 | |
crmackey | 0:4d1ae7eb0070 | 162 | if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) { |
crmackey | 0:4d1ae7eb0070 | 163 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 164 | } |
crmackey | 0:4d1ae7eb0070 | 165 | |
crmackey | 0:4d1ae7eb0070 | 166 | if (s1 < 500 and s5 < 500 and checkvar == 1) { |
crmackey | 0:4d1ae7eb0070 | 167 | counter = counter + 1; |
crmackey | 0:4d1ae7eb0070 | 168 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 169 | m3pi.printf("%d", counter); |
crmackey | 0:4d1ae7eb0070 | 170 | checkvar = 0; |
crmackey | 0:4d1ae7eb0070 | 171 | switch (counter) { |
crmackey | 0:4d1ae7eb0070 | 172 | case 1: { |
crmackey | 0:4d1ae7eb0070 | 173 | Time1 = LapTimer.read(); |
crmackey | 0:4d1ae7eb0070 | 174 | LapTimer.reset(); |
crmackey | 0:4d1ae7eb0070 | 175 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 176 | btbee.printf("Lap 1: %f \n", Time1); |
crmackey | 0:4d1ae7eb0070 | 177 | break; |
crmackey | 0:4d1ae7eb0070 | 178 | } |
crmackey | 0:4d1ae7eb0070 | 179 | case 2: { |
crmackey | 0:4d1ae7eb0070 | 180 | Time2 = LapTimer.read(); |
crmackey | 0:4d1ae7eb0070 | 181 | LapTimer.reset(); |
crmackey | 0:4d1ae7eb0070 | 182 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 183 | btbee.printf("Lap 2: %f \n", Time2); |
crmackey | 0:4d1ae7eb0070 | 184 | break; |
crmackey | 0:4d1ae7eb0070 | 185 | } |
crmackey | 0:4d1ae7eb0070 | 186 | case 3: { |
crmackey | 0:4d1ae7eb0070 | 187 | Time3 = LapTimer.read(); |
crmackey | 0:4d1ae7eb0070 | 188 | LapTimer.reset(); |
crmackey | 0:4d1ae7eb0070 | 189 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 190 | btbee.printf("Lap 3: %f \n", Time3); |
crmackey | 0:4d1ae7eb0070 | 191 | break; |
crmackey | 0:4d1ae7eb0070 | 192 | } |
crmackey | 0:4d1ae7eb0070 | 193 | case 4: { |
crmackey | 0:4d1ae7eb0070 | 194 | Time4 = LapTimer.read(); |
crmackey | 0:4d1ae7eb0070 | 195 | LapTimer.reset(); |
crmackey | 0:4d1ae7eb0070 | 196 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 197 | btbee.printf("Lap 4: %f \n", Time4); |
crmackey | 0:4d1ae7eb0070 | 198 | break; |
crmackey | 0:4d1ae7eb0070 | 199 | } |
crmackey | 0:4d1ae7eb0070 | 200 | case 5: { |
crmackey | 0:4d1ae7eb0070 | 201 | Time5 = LapTimer.read(); |
crmackey | 0:4d1ae7eb0070 | 202 | LapTimer.reset(); |
crmackey | 0:4d1ae7eb0070 | 203 | LapTimer.start(); |
crmackey | 0:4d1ae7eb0070 | 204 | btbee.printf("Lap 5: %f \n", Time5); |
crmackey | 0:4d1ae7eb0070 | 205 | break; |
crmackey | 0:4d1ae7eb0070 | 206 | } |
crmackey | 0:4d1ae7eb0070 | 207 | } |
crmackey | 0:4d1ae7eb0070 | 208 | |
crmackey | 0:4d1ae7eb0070 | 209 | |
crmackey | 0:4d1ae7eb0070 | 210 | } |
crmackey | 0:4d1ae7eb0070 | 211 | |
crmackey | 0:4d1ae7eb0070 | 212 | if (counter == 5) { |
crmackey | 0:4d1ae7eb0070 | 213 | m3pi.stop(); |
crmackey | 0:4d1ae7eb0070 | 214 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 215 | m3pi.locate(0,0); |
crmackey | 0:4d1ae7eb0070 | 216 | m3pi.printf("Ave time"); |
crmackey | 0:4d1ae7eb0070 | 217 | TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0; |
crmackey | 0:4d1ae7eb0070 | 218 | m3pi.locate(0,1); |
crmackey | 0:4d1ae7eb0070 | 219 | m3pi.printf("%f", TimeAve); |
crmackey | 0:4d1ae7eb0070 | 220 | btbee.printf("Average time per lap: %f ", TimeAve); |
crmackey | 0:4d1ae7eb0070 | 221 | exit(1); |
crmackey | 0:4d1ae7eb0070 | 222 | } |
crmackey | 5:3f29267b18e3 | 223 | |
crmackey | 5:3f29267b18e3 | 224 | timer = LoopTime.read(); |
crmackey | 5:3f29267b18e3 | 225 | btbee.printf("%f", timer); |
crmackey | 5:3f29267b18e3 | 226 | btbee.printf("\n"); |
crmackey | 5:3f29267b18e3 | 227 | if(timer < 0.005) { |
crmackey | 5:3f29267b18e3 | 228 | wait(0.005 - timer); |
crmackey | 5:3f29267b18e3 | 229 | } |
crmackey | 5:3f29267b18e3 | 230 | timer = 0; |
crmackey | 5:3f29267b18e3 | 231 | LoopTime.reset(); |
crmackey | 0:4d1ae7eb0070 | 232 | |
crmackey | 0:4d1ae7eb0070 | 233 | } |
crmackey | 0:4d1ae7eb0070 | 234 | |
crmackey | 1:936e329e840a | 235 | } |
crmackey | 1:936e329e840a | 236 | |
crmackey | 2:1e947dcc40cb | 237 | |
crmackey | 2:1e947dcc40cb | 238 | |
crmackey | 4:0bcd7a924140 | 239 |