MEGACARAZ / Mbed 2 deprecated PID

PIDs and fun

Dependencies:   btbee m3pi_ng mbed FatFileSystemCpp

main.cpp@6:75a6f6b50188, 2015-05-28 (annotated)

Committer:
crmackey
Date:
Thu May 28 11:01:48 2015 +0000
Revision:
6:75a6f6b50188
Parent:
5:3f29267b18e3
Child:
7:74c9a0fbf2b1
Finished PID change via Bluetooth

Who changed what in which revision?

UserRevisionLine numberNew 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 6:75a6f6b50188 18 double P_TERM = 1.5;
crmackey 6:75a6f6b50188 19 double I_TERM = 0;
crmackey 6:75a6f6b50188 20 double D_TERM = 25;
crmackey 6:75a6f6b50188 21 char arr_read[30];
crmackey 6:75a6f6b50188 22 int chars_read;
crmackey 0:4d1ae7eb0070 23
crmackey 0:4d1ae7eb0070 24 int main() {
crmackey 0:4d1ae7eb0070 25
crmackey 1:936e329e840a 26
crmackey 0:4d1ae7eb0070 27
crmackey 0:4d1ae7eb0070 28 btbee.reset();
crmackey 0:4d1ae7eb0070 29 m3pi_pb.mode(PullUp);
crmackey 0:4d1ae7eb0070 30
crmackey 0:4d1ae7eb0070 31 m3pi.printf("Wait 4");
crmackey 0:4d1ae7eb0070 32 m3pi.locate(0,1);
crmackey 0:4d1ae7eb0070 33 m3pi.printf("PC");
crmackey 0:4d1ae7eb0070 34
crmackey 0:4d1ae7eb0070 35 while(m3pi_pb) {
crmackey 0:4d1ae7eb0070 36 m3pi_led[0]=!m3pi_led[0];
crmackey 0:4d1ae7eb0070 37 wait(3);
crmackey 0:4d1ae7eb0070 38 btbee.printf("\n");
crmackey 0:4d1ae7eb0070 39 btbee.printf("PC connected. Press the button \n");
crmackey 0:4d1ae7eb0070 40 }
crmackey 0:4d1ae7eb0070 41
crmackey 1:936e329e840a 42 m3pi.cls();
crmackey 0:4d1ae7eb0070 43
crmackey 0:4d1ae7eb0070 44 Timer LapTimer;
crmackey 5:3f29267b18e3 45 Timer LoopTime;
crmackey 0:4d1ae7eb0070 46 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:4d1ae7eb0070 47 m3pi.print(Bat,7);
crmackey 0:4d1ae7eb0070 48 wait(0.75);
crmackey 0:4d1ae7eb0070 49 m3pi.cls();
crmackey 0:4d1ae7eb0070 50 float batteryvoltage = m3pi.battery();
crmackey 0:4d1ae7eb0070 51 char* str = new char[30];
crmackey 0:4d1ae7eb0070 52 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:4d1ae7eb0070 53 m3pi.print(str,6);
crmackey 0:4d1ae7eb0070 54 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:4d1ae7eb0070 55 wait(0.75);
crmackey 0:4d1ae7eb0070 56 m3pi.cls();
crmackey 0:4d1ae7eb0070 57
crmackey 0:4d1ae7eb0070 58 if (batteryvoltage < 4.2)
crmackey 0:4d1ae7eb0070 59 {
crmackey 0:4d1ae7eb0070 60 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:4d1ae7eb0070 61 m3pi.print(low,7);
crmackey 0:4d1ae7eb0070 62 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:4d1ae7eb0070 63 m3pi.playtune(ExitSound,11);
crmackey 0:4d1ae7eb0070 64 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:4d1ae7eb0070 65 exit(1);
crmackey 0:4d1ae7eb0070 66 }
crmackey 0:4d1ae7eb0070 67
crmackey 0:4d1ae7eb0070 68 m3pi.locate(0,1);
crmackey 0:4d1ae7eb0070 69 m3pi.printf("Line PID");
crmackey 0:4d1ae7eb0070 70
crmackey 0:4d1ae7eb0070 71 wait(2.0);
crmackey 6:75a6f6b50188 72
crmackey 0:4d1ae7eb0070 73 btbee.printf("Now calibrating \n");
crmackey 0:4d1ae7eb0070 74
crmackey 0:4d1ae7eb0070 75 m3pi.sensor_auto_calibrate();
crmackey 0:4d1ae7eb0070 76
crmackey 0:4d1ae7eb0070 77 btbee.printf("Finished calibrating \n");
crmackey 0:4d1ae7eb0070 78
crmackey 0:4d1ae7eb0070 79
crmackey 0:4d1ae7eb0070 80 float right;
crmackey 0:4d1ae7eb0070 81 float left;
crmackey 0:4d1ae7eb0070 82 float current_pos_of_line = 0.0;
crmackey 0:4d1ae7eb0070 83 float previous_pos_of_line = 0.0;
crmackey 0:4d1ae7eb0070 84 float derivative,proportional,integral = 0;
crmackey 0:4d1ae7eb0070 85 float power;
crmackey 0:4d1ae7eb0070 86 float speed = MAX;
crmackey 0:4d1ae7eb0070 87 int LapTest[5];
crmackey 0:4d1ae7eb0070 88 int s1, s2, s3, s4, s5;
crmackey 0:4d1ae7eb0070 89 int counter = -1;
crmackey 0:4d1ae7eb0070 90 int checkvar = 0;
crmackey 0:4d1ae7eb0070 91 double Time1, Time2, Time3, Time4, Time5, TimeAve;
crmackey 3:ebe015c72bba 92 bool IR = 1;
crmackey 5:3f29267b18e3 93 double timer;
crmackey 0:4d1ae7eb0070 94 btbee.printf("\n");
crmackey 0:4d1ae7eb0070 95 btbee.printf("Now starting \n");
crmackey 0:4d1ae7eb0070 96
crmackey 0:4d1ae7eb0070 97
crmackey 1:936e329e840a 98
crmackey 0:4d1ae7eb0070 99
crmackey 1:936e329e840a 100 while (1) {
crmackey 1:936e329e840a 101
crmackey 5:3f29267b18e3 102 LoopTime.start();
crmackey 5:3f29267b18e3 103
crmackey 3:ebe015c72bba 104 IR = m3pi_IN [0];
crmackey 3:ebe015c72bba 105
crmackey 3:ebe015c72bba 106 if(m3pi_IN [0] == 0 and IR == 0) {
crmackey 3:ebe015c72bba 107 while (m3pi_IN [0] == 0 and IR == 0) {
crmackey 4:0bcd7a924140 108 m3pi.cls();
crmackey 4:0bcd7a924140 109 btbee.printf("Obstacle detected. Ending program");
crmackey 4:0bcd7a924140 110 m3pi.printf("ABORT");
crmackey 2:1e947dcc40cb 111 m3pi.stop();
crmackey 4:0bcd7a924140 112 exit(1);
crmackey 2:1e947dcc40cb 113 }
crmackey 2:1e947dcc40cb 114 }
crmackey 4:0bcd7a924140 115
crmackey 2:1e947dcc40cb 116
crmackey 2:1e947dcc40cb 117 // Get the position of the line.
crmackey 0:4d1ae7eb0070 118 current_pos_of_line = m3pi.line_position();
crmackey 0:4d1ae7eb0070 119 proportional = current_pos_of_line;
crmackey 0:4d1ae7eb0070 120
crmackey 0:4d1ae7eb0070 121 // Compute the derivative
crmackey 0:4d1ae7eb0070 122 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:4d1ae7eb0070 123
crmackey 0:4d1ae7eb0070 124 // Compute the integral
crmackey 0:4d1ae7eb0070 125 integral += proportional;
crmackey 0:4d1ae7eb0070 126
crmackey 0:4d1ae7eb0070 127 // Remember the last position.
crmackey 0:4d1ae7eb0070 128 previous_pos_of_line = current_pos_of_line;
crmackey 0:4d1ae7eb0070 129
crmackey 0:4d1ae7eb0070 130 // Compute the power
crmackey 0:4d1ae7eb0070 131 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:4d1ae7eb0070 132
crmackey 0:4d1ae7eb0070 133 // Compute new speeds
crmackey 0:4d1ae7eb0070 134 right = speed+power;
crmackey 0:4d1ae7eb0070 135 left = speed-power;
crmackey 0:4d1ae7eb0070 136
crmackey 0:4d1ae7eb0070 137 // limit checks
crmackey 0:4d1ae7eb0070 138 if (right < MIN)
crmackey 0:4d1ae7eb0070 139 right = MIN;
crmackey 0:4d1ae7eb0070 140 else if (right > MAX)
crmackey 0:4d1ae7eb0070 141 right = MAX;
crmackey 0:4d1ae7eb0070 142
crmackey 0:4d1ae7eb0070 143 if (left < MIN)
crmackey 0:4d1ae7eb0070 144 left = MIN;
crmackey 0:4d1ae7eb0070 145 else if (left > MAX)
crmackey 0:4d1ae7eb0070 146 left = MAX;
crmackey 0:4d1ae7eb0070 147
crmackey 0:4d1ae7eb0070 148 // set speed
crmackey 0:4d1ae7eb0070 149 m3pi.left_motor(left);
crmackey 0:4d1ae7eb0070 150 m3pi.right_motor(right);
crmackey 0:4d1ae7eb0070 151
crmackey 0:4d1ae7eb0070 152 m3pi.calibrated_sensor(LapTest);
crmackey 0:4d1ae7eb0070 153
crmackey 0:4d1ae7eb0070 154 s1 = LapTest[0];
crmackey 0:4d1ae7eb0070 155 s5 = LapTest[4];
crmackey 0:4d1ae7eb0070 156
crmackey 0:4d1ae7eb0070 157 if (s1 > 500 and s5 > 500) {
crmackey 0:4d1ae7eb0070 158 checkvar = 1;
crmackey 0:4d1ae7eb0070 159 }
crmackey 0:4d1ae7eb0070 160
crmackey 0:4d1ae7eb0070 161 if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
crmackey 6:75a6f6b50188 162 LapTimer.reset();
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 6:75a6f6b50188 220 btbee.printf("Average time per lap: %f \n", TimeAve);
crmackey 6:75a6f6b50188 221 btbee.printf("\n");
crmackey 6:75a6f6b50188 222 btbee.printf("Enter P value, I value, and D value separately \n");
crmackey 6:75a6f6b50188 223
crmackey 6:75a6f6b50188 224 while(1) {
crmackey 6:75a6f6b50188 225
crmackey 6:75a6f6b50188 226
crmackey 6:75a6f6b50188 227 if (btbee.readable()) {
crmackey 6:75a6f6b50188 228
crmackey 6:75a6f6b50188 229 btbee.printf("Enter P valure: \n");
crmackey 6:75a6f6b50188 230
crmackey 6:75a6f6b50188 231
crmackey 6:75a6f6b50188 232 btbee.read_all(arr_read, 30, &chars_read);
crmackey 6:75a6f6b50188 233 P_TERM = atof(arr_read);
crmackey 6:75a6f6b50188 234
crmackey 6:75a6f6b50188 235 btbee.printf("P term is %f \n", P_TERM);
crmackey 6:75a6f6b50188 236
crmackey 6:75a6f6b50188 237 wait(1.0);
crmackey 6:75a6f6b50188 238
crmackey 6:75a6f6b50188 239 break;
crmackey 6:75a6f6b50188 240
crmackey 6:75a6f6b50188 241 }
crmackey 6:75a6f6b50188 242
crmackey 6:75a6f6b50188 243 }
crmackey 6:75a6f6b50188 244
crmackey 6:75a6f6b50188 245 while(1) {
crmackey 6:75a6f6b50188 246
crmackey 6:75a6f6b50188 247 if(btbee.readable()) {
crmackey 6:75a6f6b50188 248
crmackey 6:75a6f6b50188 249 btbee.printf("Enter I value: \n");
crmackey 6:75a6f6b50188 250
crmackey 6:75a6f6b50188 251 btbee.read_all(arr_read, 30, &chars_read);
crmackey 6:75a6f6b50188 252 I_TERM = atof(arr_read);
crmackey 6:75a6f6b50188 253
crmackey 6:75a6f6b50188 254 btbee.printf("I term is %f \n", I_TERM);
crmackey 6:75a6f6b50188 255
crmackey 6:75a6f6b50188 256 wait(1.0);
crmackey 6:75a6f6b50188 257
crmackey 6:75a6f6b50188 258 break;
crmackey 6:75a6f6b50188 259
crmackey 6:75a6f6b50188 260 }
crmackey 6:75a6f6b50188 261
crmackey 6:75a6f6b50188 262 }
crmackey 6:75a6f6b50188 263
crmackey 6:75a6f6b50188 264 while(1) {
crmackey 6:75a6f6b50188 265
crmackey 6:75a6f6b50188 266 if(btbee.readable()) {
crmackey 6:75a6f6b50188 267
crmackey 6:75a6f6b50188 268 btbee.printf("Enter D value: \n");
crmackey 6:75a6f6b50188 269
crmackey 6:75a6f6b50188 270 btbee.read_all(arr_read, 30, &chars_read);
crmackey 6:75a6f6b50188 271 D_TERM = atof(arr_read);
crmackey 6:75a6f6b50188 272
crmackey 6:75a6f6b50188 273 btbee.printf("D term is %f \n", D_TERM);
crmackey 6:75a6f6b50188 274
crmackey 6:75a6f6b50188 275 wait(1);
crmackey 6:75a6f6b50188 276
crmackey 6:75a6f6b50188 277 btbee.printf("Starting again with new parameters");
crmackey 6:75a6f6b50188 278
crmackey 6:75a6f6b50188 279 counter = -1;
crmackey 6:75a6f6b50188 280
crmackey 6:75a6f6b50188 281
crmackey 6:75a6f6b50188 282 break;
crmackey 6:75a6f6b50188 283
crmackey 6:75a6f6b50188 284 }
crmackey 6:75a6f6b50188 285
crmackey 6:75a6f6b50188 286 }
crmackey 6:75a6f6b50188 287
crmackey 0:4d1ae7eb0070 288 }
crmackey 6:75a6f6b50188 289
crmackey 6:75a6f6b50188 290
crmackey 5:3f29267b18e3 291
crmackey 6:75a6f6b50188 292
crmackey 6:75a6f6b50188 293 //if(timer < 0.005) {
crmackey 6:75a6f6b50188 294 // wait(0.005 - timer);
crmackey 6:75a6f6b50188 295 // }
crmackey 6:75a6f6b50188 296 // timer = 0;
crmackey 6:75a6f6b50188 297 // LoopTime.reset();
crmackey 0:4d1ae7eb0070 298
crmackey 0:4d1ae7eb0070 299 }
crmackey 1:936e329e840a 300
crmackey 2:1e947dcc40cb 301
crmackey 2:1e947dcc40cb 302
crmackey 6:75a6f6b50188 303 }