MEGACARAZ / Mbed 2 deprecated PID

PIDs and fun

Dependencies:   btbee m3pi_ng mbed FatFileSystemCpp

USBTest.cpp@15:62648eb5df9f, 2015-05-29 (annotated)

Committer:
crmackey
Date:
Fri May 29 13:19:37 2015 +0000
Revision:
15:62648eb5df9f
Parent:
14:46b19affc4b2
Child:
16:b09cc5b75049
New Version. Still broken

Who changed what in which revision?

UserRevisionLine numberNew contents of line
crmackey 14:46b19affc4b2 1 #include "mbed.h"
crmackey 14:46b19affc4b2 2 #include "m3pi_ng.h"
crmackey 14:46b19affc4b2 3 #include "time.h"
crmackey 14:46b19affc4b2 4 #include "btbee.h"
crmackey 14:46b19affc4b2 5 #include "MSCFileSystem.h"
crmackey 14:46b19affc4b2 6 #include <vector>
crmackey 14:46b19affc4b2 7
crmackey 14:46b19affc4b2 8 #define FSNAME "msc"
crmackey 14:46b19affc4b2 9 MSCFileSystem msc(FSNAME);
crmackey 14:46b19affc4b2 10
crmackey 14:46b19affc4b2 11 m3pi m3pi;
crmackey 14:46b19affc4b2 12 btbee btbee;
crmackey 14:46b19affc4b2 13 DigitalIn m3pi_pb(p21);
crmackey 14:46b19affc4b2 14 DigitalIn m3pi_IN[] = {(p12)};
crmackey 14:46b19affc4b2 15 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
crmackey 14:46b19affc4b2 16 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
crmackey 14:46b19affc4b2 17
crmackey 14:46b19affc4b2 18 // Minimum and maximum motor speeds
crmackey 14:46b19affc4b2 19 #define MAX 1.0
crmackey 14:46b19affc4b2 20 #define MIN 0
crmackey 14:46b19affc4b2 21
crmackey 14:46b19affc4b2 22 // PID terms
crmackey 14:46b19affc4b2 23 #define P_TERM 1.5
crmackey 14:46b19affc4b2 24 #define I_TERM 0
crmackey 14:46b19affc4b2 25 #define D_TERM 25
crmackey 14:46b19affc4b2 26
crmackey 14:46b19affc4b2 27 int main() {
crmackey 14:46b19affc4b2 28
crmackey 14:46b19affc4b2 29
crmackey 14:46b19affc4b2 30 while(1) {
crmackey 14:46b19affc4b2 31
crmackey 14:46b19affc4b2 32
crmackey 14:46b19affc4b2 33 btbee.reset();
crmackey 14:46b19affc4b2 34 m3pi_pb.mode(PullUp);
crmackey 14:46b19affc4b2 35
crmackey 14:46b19affc4b2 36 m3pi.printf("Wait 4");
crmackey 14:46b19affc4b2 37 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 38 m3pi.printf("PC");
crmackey 14:46b19affc4b2 39
crmackey 14:46b19affc4b2 40 while(m3pi_pb) {
crmackey 14:46b19affc4b2 41 m3pi_led[0]=!m3pi_led[0];
crmackey 14:46b19affc4b2 42 wait(3);
crmackey 14:46b19affc4b2 43 btbee.printf("\n");
crmackey 14:46b19affc4b2 44 btbee.printf("PC connected. Press the button \n");
crmackey 14:46b19affc4b2 45 }
crmackey 14:46b19affc4b2 46
crmackey 14:46b19affc4b2 47 m3pi.cls();
crmackey 14:46b19affc4b2 48
crmackey 14:46b19affc4b2 49 Timer LapTimer;
crmackey 15:62648eb5df9f 50 Timer LoopTime;
crmackey 15:62648eb5df9f 51 Timer WriteTime;
crmackey 14:46b19affc4b2 52 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 14:46b19affc4b2 53 m3pi.print(Bat,7);
crmackey 14:46b19affc4b2 54 wait(0.75);
crmackey 14:46b19affc4b2 55 m3pi.cls();
crmackey 14:46b19affc4b2 56 float batteryvoltage = m3pi.battery();
crmackey 14:46b19affc4b2 57 char* str = new char[30];
crmackey 14:46b19affc4b2 58 sprintf(str, "%.4g", batteryvoltage);
crmackey 14:46b19affc4b2 59 m3pi.print(str,6);
crmackey 14:46b19affc4b2 60 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 14:46b19affc4b2 61 wait(0.75);
crmackey 14:46b19affc4b2 62 m3pi.cls();
crmackey 14:46b19affc4b2 63
crmackey 14:46b19affc4b2 64 if (batteryvoltage < 4.2)
crmackey 14:46b19affc4b2 65 {
crmackey 14:46b19affc4b2 66 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 14:46b19affc4b2 67 m3pi.print(low,7);
crmackey 14:46b19affc4b2 68 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 14:46b19affc4b2 69 m3pi.playtune(ExitSound,11);
crmackey 14:46b19affc4b2 70 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 14:46b19affc4b2 71 exit(1);
crmackey 14:46b19affc4b2 72 }
crmackey 14:46b19affc4b2 73
crmackey 14:46b19affc4b2 74 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 75 m3pi.printf("Line PID");
crmackey 14:46b19affc4b2 76
crmackey 14:46b19affc4b2 77 wait(2.0);
crmackey 14:46b19affc4b2 78
crmackey 14:46b19affc4b2 79 btbee.printf("Now calibrating \n");
crmackey 14:46b19affc4b2 80
crmackey 14:46b19affc4b2 81 m3pi.sensor_auto_calibrate();
crmackey 14:46b19affc4b2 82
crmackey 14:46b19affc4b2 83 btbee.printf("Finished calibrating \n");
crmackey 14:46b19affc4b2 84
crmackey 14:46b19affc4b2 85
crmackey 14:46b19affc4b2 86 float right;
crmackey 14:46b19affc4b2 87 float left;
crmackey 14:46b19affc4b2 88 float current_pos_of_line = 0.0;
crmackey 14:46b19affc4b2 89 float previous_pos_of_line = 0.0;
crmackey 14:46b19affc4b2 90 float derivative,proportional,integral = 0;
crmackey 14:46b19affc4b2 91 float power;
crmackey 14:46b19affc4b2 92 float speed = MAX;
crmackey 14:46b19affc4b2 93 int LapTest[5];
crmackey 14:46b19affc4b2 94 int s1, s2, s3, s4, s5;
crmackey 14:46b19affc4b2 95 int counter = -1;
crmackey 14:46b19affc4b2 96 int checkvar = 0;
crmackey 14:46b19affc4b2 97 double Time1, Time2, Time3, Time4, Time5, TimeAve;
crmackey 15:62648eb5df9f 98 vector < float > ZeTimes(2);
crmackey 15:62648eb5df9f 99 vector < vector < float > > Data(500);
crmackey 14:46b19affc4b2 100 float stuff;
crmackey 15:62648eb5df9f 101 float junk;
crmackey 15:62648eb5df9f 102 double timer;
crmackey 15:62648eb5df9f 103 double TimeToWrite;
crmackey 15:62648eb5df9f 104 int loopcounter = 0;
crmackey 14:46b19affc4b2 105
crmackey 14:46b19affc4b2 106
crmackey 14:46b19affc4b2 107 btbee.printf("\n");
crmackey 14:46b19affc4b2 108 btbee.printf("Now starting \n");
crmackey 14:46b19affc4b2 109
crmackey 14:46b19affc4b2 110 while (1) {
crmackey 14:46b19affc4b2 111
crmackey 15:62648eb5df9f 112 LoopTime.start();
crmackey 14:46b19affc4b2 113
crmackey 14:46b19affc4b2 114
crmackey 14:46b19affc4b2 115 // Get the position of the line.
crmackey 15:62648eb5df9f 116 current_pos_of_line = m3pi.line_position();
crmackey 14:46b19affc4b2 117
crmackey 15:62648eb5df9f 118 stuff = current_pos_of_line;
crmackey 14:46b19affc4b2 119
crmackey 14:46b19affc4b2 120 proportional = current_pos_of_line;
crmackey 14:46b19affc4b2 121 //fprintf(fp, "%f", current_pos_of_line);
crmackey 14:46b19affc4b2 122 //fprintf(fp, " ");
crmackey 14:46b19affc4b2 123
crmackey 14:46b19affc4b2 124 // Compute the derivative
crmackey 14:46b19affc4b2 125 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 14:46b19affc4b2 126
crmackey 14:46b19affc4b2 127 // Compute the integral
crmackey 14:46b19affc4b2 128 integral += proportional;
crmackey 14:46b19affc4b2 129
crmackey 14:46b19affc4b2 130 // Remember the last position.
crmackey 14:46b19affc4b2 131 previous_pos_of_line = current_pos_of_line;
crmackey 14:46b19affc4b2 132
crmackey 14:46b19affc4b2 133 // Compute the power
crmackey 14:46b19affc4b2 134 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 14:46b19affc4b2 135
crmackey 14:46b19affc4b2 136 // Compute new speeds
crmackey 14:46b19affc4b2 137 right = speed+power;
crmackey 14:46b19affc4b2 138 left = speed-power;
crmackey 14:46b19affc4b2 139
crmackey 14:46b19affc4b2 140 // limit checks
crmackey 14:46b19affc4b2 141 if (right < MIN)
crmackey 14:46b19affc4b2 142 right = MIN;
crmackey 14:46b19affc4b2 143 else if (right > MAX)
crmackey 14:46b19affc4b2 144 right = MAX;
crmackey 14:46b19affc4b2 145
crmackey 14:46b19affc4b2 146 if (left < MIN)
crmackey 14:46b19affc4b2 147 left = MIN;
crmackey 14:46b19affc4b2 148 else if (left > MAX)
crmackey 14:46b19affc4b2 149 left = MAX;
crmackey 14:46b19affc4b2 150
crmackey 14:46b19affc4b2 151 // set speed
crmackey 14:46b19affc4b2 152 m3pi.left_motor(left);
crmackey 14:46b19affc4b2 153 m3pi.right_motor(right);
crmackey 14:46b19affc4b2 154
crmackey 14:46b19affc4b2 155
crmackey 14:46b19affc4b2 156
crmackey 14:46b19affc4b2 157 m3pi.calibrated_sensor(LapTest);
crmackey 14:46b19affc4b2 158
crmackey 15:62648eb5df9f 159
crmackey 14:46b19affc4b2 160
crmackey 14:46b19affc4b2 161 s1 = LapTest[0];
crmackey 14:46b19affc4b2 162 s2 = LapTest[1];
crmackey 14:46b19affc4b2 163 s3 = LapTest[2];
crmackey 14:46b19affc4b2 164 s4 = LapTest[3];
crmackey 14:46b19affc4b2 165 s5 = LapTest[4];
crmackey 14:46b19affc4b2 166
crmackey 14:46b19affc4b2 167 if (s1 > 500 and s5 > 500) {
crmackey 14:46b19affc4b2 168 checkvar = 1;
crmackey 14:46b19affc4b2 169 }
crmackey 14:46b19affc4b2 170
crmackey 14:46b19affc4b2 171 if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
crmackey 14:46b19affc4b2 172 LapTimer.start();
crmackey 14:46b19affc4b2 173 }
crmackey 14:46b19affc4b2 174
crmackey 14:46b19affc4b2 175 if (s1 < 500 and s5 < 500 and checkvar == 1) {
crmackey 14:46b19affc4b2 176 counter = counter + 1;
crmackey 14:46b19affc4b2 177 m3pi.cls();
crmackey 14:46b19affc4b2 178 m3pi.printf("%d", counter);
crmackey 14:46b19affc4b2 179 checkvar = 0;
crmackey 14:46b19affc4b2 180 switch (counter) {
crmackey 14:46b19affc4b2 181 case 1: {
crmackey 14:46b19affc4b2 182 Time1 = LapTimer.read();
crmackey 14:46b19affc4b2 183 LapTimer.reset();
crmackey 14:46b19affc4b2 184 LapTimer.start();
crmackey 14:46b19affc4b2 185 btbee.printf("Lap 1: %f \n", Time1);
crmackey 14:46b19affc4b2 186
crmackey 14:46b19affc4b2 187 break;
crmackey 14:46b19affc4b2 188 }
crmackey 14:46b19affc4b2 189 case 2: {
crmackey 14:46b19affc4b2 190 Time2 = LapTimer.read();
crmackey 14:46b19affc4b2 191 LapTimer.reset();
crmackey 14:46b19affc4b2 192 LapTimer.start();
crmackey 14:46b19affc4b2 193 btbee.printf("Lap 2: %f \n", Time2);
crmackey 14:46b19affc4b2 194
crmackey 14:46b19affc4b2 195 break;
crmackey 14:46b19affc4b2 196 }
crmackey 14:46b19affc4b2 197 case 3: {
crmackey 14:46b19affc4b2 198 Time3 = LapTimer.read();
crmackey 14:46b19affc4b2 199 LapTimer.reset();
crmackey 14:46b19affc4b2 200 LapTimer.start();
crmackey 14:46b19affc4b2 201 btbee.printf("Lap 3: %f \n", Time3);
crmackey 14:46b19affc4b2 202
crmackey 14:46b19affc4b2 203 break;
crmackey 14:46b19affc4b2 204 }
crmackey 14:46b19affc4b2 205 case 4: {
crmackey 14:46b19affc4b2 206 Time4 = LapTimer.read();
crmackey 14:46b19affc4b2 207 LapTimer.reset();
crmackey 14:46b19affc4b2 208 LapTimer.start();
crmackey 14:46b19affc4b2 209 btbee.printf("Lap 4: %f \n", Time4);
crmackey 14:46b19affc4b2 210
crmackey 14:46b19affc4b2 211 break;
crmackey 14:46b19affc4b2 212 }
crmackey 14:46b19affc4b2 213 case 5: {
crmackey 14:46b19affc4b2 214 Time5 = LapTimer.read();
crmackey 14:46b19affc4b2 215 LapTimer.reset();
crmackey 14:46b19affc4b2 216 LapTimer.start();
crmackey 14:46b19affc4b2 217 btbee.printf("Lap 5: %f \n", Time5);
crmackey 14:46b19affc4b2 218
crmackey 14:46b19affc4b2 219 break;
crmackey 14:46b19affc4b2 220 }
crmackey 14:46b19affc4b2 221 }
crmackey 14:46b19affc4b2 222
crmackey 14:46b19affc4b2 223
crmackey 14:46b19affc4b2 224 }
crmackey 14:46b19affc4b2 225
crmackey 15:62648eb5df9f 226 ZeTimes[0]= stuff;
crmackey 15:62648eb5df9f 227 ZeTimes[1] = power;
crmackey 15:62648eb5df9f 228 Data[loopcounter] = ZeTimes;
crmackey 15:62648eb5df9f 229
crmackey 15:62648eb5df9f 230 loopcounter++;
crmackey 15:62648eb5df9f 231
crmackey 15:62648eb5df9f 232
crmackey 15:62648eb5df9f 233 timer = LoopTime.read();
crmackey 15:62648eb5df9f 234
crmackey 15:62648eb5df9f 235 if(timer < 0.005) {
crmackey 15:62648eb5df9f 236 wait(0.005 - timer);
crmackey 15:62648eb5df9f 237 }
crmackey 15:62648eb5df9f 238 timer = 0;
crmackey 15:62648eb5df9f 239 LoopTime.reset();
crmackey 15:62648eb5df9f 240
crmackey 15:62648eb5df9f 241 if (loopcounter == 500) {
crmackey 14:46b19affc4b2 242 m3pi.stop();
crmackey 14:46b19affc4b2 243 m3pi.cls();
crmackey 14:46b19affc4b2 244 m3pi.locate(0,0);
crmackey 14:46b19affc4b2 245 m3pi.printf("Ave time");
crmackey 14:46b19affc4b2 246 TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
crmackey 14:46b19affc4b2 247 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 248 m3pi.printf("%f", TimeAve);
crmackey 14:46b19affc4b2 249 btbee.printf("Average time per lap: %f ", TimeAve);
crmackey 14:46b19affc4b2 250 btbee.printf("\n");
crmackey 14:46b19affc4b2 251 btbee.printf("Printing to USB");
crmackey 14:46b19affc4b2 252
crmackey 15:62648eb5df9f 253 WriteTime.start();
crmackey 15:62648eb5df9f 254
crmackey 15:62648eb5df9f 255 DIR *d;
crmackey 14:46b19affc4b2 256 struct dirent *p;
crmackey 14:46b19affc4b2 257
crmackey 14:46b19affc4b2 258 d = opendir("/" FSNAME);
crmackey 14:46b19affc4b2 259
crmackey 14:46b19affc4b2 260 FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
crmackey 14:46b19affc4b2 261 if ( fp == NULL )
crmackey 14:46b19affc4b2 262 {
crmackey 14:46b19affc4b2 263 m3pi.leds(1);
crmackey 14:46b19affc4b2 264 }
crmackey 15:62648eb5df9f 265
crmackey 15:62648eb5df9f 266 for(int i = 0; i < 500; i++) {
crmackey 14:46b19affc4b2 267
crmackey 15:62648eb5df9f 268 for( int j = 0; j < 2; j++) {
crmackey 14:46b19affc4b2 269
crmackey 15:62648eb5df9f 270 junk = Data[i][j];
crmackey 15:62648eb5df9f 271
crmackey 15:62648eb5df9f 272 fprintf(fp,"%f ", junk);
crmackey 14:46b19affc4b2 273
crmackey 14:46b19affc4b2 274 }
crmackey 15:62648eb5df9f 275
crmackey 15:62648eb5df9f 276 fprintf(fp,"\r\n");
crmackey 14:46b19affc4b2 277
crmackey 15:62648eb5df9f 278 }
crmackey 14:46b19affc4b2 279
crmackey 15:62648eb5df9f 280 TimeToWrite = WriteTime.read();
crmackey 15:62648eb5df9f 281 btbee.printf("%f \n", TimeToWrite);
crmackey 15:62648eb5df9f 282 WriteTime.reset();
crmackey 14:46b19affc4b2 283
crmackey 14:46b19affc4b2 284 btbee.printf("Printed");
crmackey 15:62648eb5df9f 285 m3pi.cls();
crmackey 15:62648eb5df9f 286 m3pi.printf("Printed");
crmackey 15:62648eb5df9f 287
crmackey 15:62648eb5df9f 288 fclose(fp);
crmackey 14:46b19affc4b2 289
crmackey 14:46b19affc4b2 290
crmackey 14:46b19affc4b2 291 exit(1);
crmackey 14:46b19affc4b2 292 }
crmackey 14:46b19affc4b2 293
crmackey 14:46b19affc4b2 294
crmackey 15:62648eb5df9f 295
crmackey 14:46b19affc4b2 296 }
crmackey 14:46b19affc4b2 297
crmackey 14:46b19affc4b2 298 }
crmackey 14:46b19affc4b2 299
crmackey 14:46b19affc4b2 300 }