MEGACARAZ / Mbed 2 deprecated PID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   btbee m3pi_ng mbed FatFileSystemCpp

USBTest.cpp@14:46b19affc4b2, 2015-05-29 (annotated)

Committer:
crmackey
Date:
Fri May 29 11:47:52 2015 +0000
Revision:
14:46b19affc4b2
Child:
15:62648eb5df9f
Added USBTest.cpp

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 14:46b19affc4b2 50 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 14:46b19affc4b2 51 m3pi.print(Bat,7);
crmackey 14:46b19affc4b2 52 wait(0.75);
crmackey 14:46b19affc4b2 53 m3pi.cls();
crmackey 14:46b19affc4b2 54 float batteryvoltage = m3pi.battery();
crmackey 14:46b19affc4b2 55 char* str = new char[30];
crmackey 14:46b19affc4b2 56 sprintf(str, "%.4g", batteryvoltage);
crmackey 14:46b19affc4b2 57 m3pi.print(str,6);
crmackey 14:46b19affc4b2 58 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 14:46b19affc4b2 59 wait(0.75);
crmackey 14:46b19affc4b2 60 m3pi.cls();
crmackey 14:46b19affc4b2 61
crmackey 14:46b19affc4b2 62 if (batteryvoltage < 4.2)
crmackey 14:46b19affc4b2 63 {
crmackey 14:46b19affc4b2 64 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 14:46b19affc4b2 65 m3pi.print(low,7);
crmackey 14:46b19affc4b2 66 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 14:46b19affc4b2 67 m3pi.playtune(ExitSound,11);
crmackey 14:46b19affc4b2 68 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 14:46b19affc4b2 69 exit(1);
crmackey 14:46b19affc4b2 70 }
crmackey 14:46b19affc4b2 71
crmackey 14:46b19affc4b2 72 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 73 m3pi.printf("Line PID");
crmackey 14:46b19affc4b2 74
crmackey 14:46b19affc4b2 75 wait(2.0);
crmackey 14:46b19affc4b2 76
crmackey 14:46b19affc4b2 77 btbee.printf("Now calibrating \n");
crmackey 14:46b19affc4b2 78
crmackey 14:46b19affc4b2 79 m3pi.sensor_auto_calibrate();
crmackey 14:46b19affc4b2 80
crmackey 14:46b19affc4b2 81 btbee.printf("Finished calibrating \n");
crmackey 14:46b19affc4b2 82
crmackey 14:46b19affc4b2 83
crmackey 14:46b19affc4b2 84 float right;
crmackey 14:46b19affc4b2 85 float left;
crmackey 14:46b19affc4b2 86 float current_pos_of_line = 0.0;
crmackey 14:46b19affc4b2 87 float previous_pos_of_line = 0.0;
crmackey 14:46b19affc4b2 88 float derivative,proportional,integral = 0;
crmackey 14:46b19affc4b2 89 float power;
crmackey 14:46b19affc4b2 90 float speed = MAX;
crmackey 14:46b19affc4b2 91 int LapTest[5];
crmackey 14:46b19affc4b2 92 int s1, s2, s3, s4, s5;
crmackey 14:46b19affc4b2 93 int counter = -1;
crmackey 14:46b19affc4b2 94 int checkvar = 0;
crmackey 14:46b19affc4b2 95 double Time1, Time2, Time3, Time4, Time5, TimeAve;
crmackey 14:46b19affc4b2 96 vector < float > ZeTimes;
crmackey 14:46b19affc4b2 97 vector < vector < float > > Data;
crmackey 14:46b19affc4b2 98 float stuff;
crmackey 14:46b19affc4b2 99
crmackey 14:46b19affc4b2 100
crmackey 14:46b19affc4b2 101 btbee.printf("\n");
crmackey 14:46b19affc4b2 102 btbee.printf("Now starting \n");
crmackey 14:46b19affc4b2 103
crmackey 14:46b19affc4b2 104 while (1) {
crmackey 14:46b19affc4b2 105
crmackey 14:46b19affc4b2 106 //if(m3pi_IN [0] == 0) {
crmackey 14:46b19affc4b2 107 // while (m3pi_IN [0] == 0) {
crmackey 14:46b19affc4b2 108 // btbee.printf("Obstacle detected. Please remove to continue");
crmackey 14:46b19affc4b2 109 // m3pi.stop();
crmackey 14:46b19affc4b2 110 // wait(3.0);
crmackey 14:46b19affc4b2 111 // }
crmackey 14:46b19affc4b2 112 //}
crmackey 14:46b19affc4b2 113
crmackey 14:46b19affc4b2 114
crmackey 14:46b19affc4b2 115 ZeTimes.clear();
crmackey 14:46b19affc4b2 116
crmackey 14:46b19affc4b2 117 // Get the position of the line.
crmackey 14:46b19affc4b2 118 current_pos_of_line = m3pi.line_position();
crmackey 14:46b19affc4b2 119
crmackey 14:46b19affc4b2 120 ZeTimes.push_back(current_pos_of_line);
crmackey 14:46b19affc4b2 121
crmackey 14:46b19affc4b2 122 proportional = current_pos_of_line;
crmackey 14:46b19affc4b2 123 //fprintf(fp, "%f", current_pos_of_line);
crmackey 14:46b19affc4b2 124 //fprintf(fp, " ");
crmackey 14:46b19affc4b2 125
crmackey 14:46b19affc4b2 126 // Compute the derivative
crmackey 14:46b19affc4b2 127 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 14:46b19affc4b2 128
crmackey 14:46b19affc4b2 129 // Compute the integral
crmackey 14:46b19affc4b2 130 integral += proportional;
crmackey 14:46b19affc4b2 131
crmackey 14:46b19affc4b2 132 // Remember the last position.
crmackey 14:46b19affc4b2 133 previous_pos_of_line = current_pos_of_line;
crmackey 14:46b19affc4b2 134
crmackey 14:46b19affc4b2 135 // Compute the power
crmackey 14:46b19affc4b2 136 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 14:46b19affc4b2 137
crmackey 14:46b19affc4b2 138 // Compute new speeds
crmackey 14:46b19affc4b2 139 right = speed+power;
crmackey 14:46b19affc4b2 140 left = speed-power;
crmackey 14:46b19affc4b2 141
crmackey 14:46b19affc4b2 142 // limit checks
crmackey 14:46b19affc4b2 143 if (right < MIN)
crmackey 14:46b19affc4b2 144 right = MIN;
crmackey 14:46b19affc4b2 145 else if (right > MAX)
crmackey 14:46b19affc4b2 146 right = MAX;
crmackey 14:46b19affc4b2 147
crmackey 14:46b19affc4b2 148 if (left < MIN)
crmackey 14:46b19affc4b2 149 left = MIN;
crmackey 14:46b19affc4b2 150 else if (left > MAX)
crmackey 14:46b19affc4b2 151 left = MAX;
crmackey 14:46b19affc4b2 152
crmackey 14:46b19affc4b2 153 // set speed
crmackey 14:46b19affc4b2 154 m3pi.left_motor(left);
crmackey 14:46b19affc4b2 155 m3pi.right_motor(right);
crmackey 14:46b19affc4b2 156
crmackey 14:46b19affc4b2 157 ZeTimes.push_back(left);
crmackey 14:46b19affc4b2 158 ZeTimes.push_back(right);
crmackey 14:46b19affc4b2 159
crmackey 14:46b19affc4b2 160 //fprintf(fp, "%f", left);
crmackey 14:46b19affc4b2 161 // fprintf(fp, " ");
crmackey 14:46b19affc4b2 162 //
crmackey 14:46b19affc4b2 163 // fprintf(fp, "%f", right);
crmackey 14:46b19affc4b2 164 // fprintf(fp, "\n");
crmackey 14:46b19affc4b2 165
crmackey 14:46b19affc4b2 166 m3pi.calibrated_sensor(LapTest);
crmackey 14:46b19affc4b2 167
crmackey 14:46b19affc4b2 168 Data.push_back(ZeTimes);
crmackey 14:46b19affc4b2 169
crmackey 14:46b19affc4b2 170 s1 = LapTest[0];
crmackey 14:46b19affc4b2 171 s2 = LapTest[1];
crmackey 14:46b19affc4b2 172 s3 = LapTest[2];
crmackey 14:46b19affc4b2 173 s4 = LapTest[3];
crmackey 14:46b19affc4b2 174 s5 = LapTest[4];
crmackey 14:46b19affc4b2 175
crmackey 14:46b19affc4b2 176 if (s1 > 500 and s5 > 500) {
crmackey 14:46b19affc4b2 177 checkvar = 1;
crmackey 14:46b19affc4b2 178 }
crmackey 14:46b19affc4b2 179
crmackey 14:46b19affc4b2 180 if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
crmackey 14:46b19affc4b2 181 LapTimer.start();
crmackey 14:46b19affc4b2 182 }
crmackey 14:46b19affc4b2 183
crmackey 14:46b19affc4b2 184 if (s1 < 500 and s5 < 500 and checkvar == 1) {
crmackey 14:46b19affc4b2 185 counter = counter + 1;
crmackey 14:46b19affc4b2 186 m3pi.cls();
crmackey 14:46b19affc4b2 187 m3pi.printf("%d", counter);
crmackey 14:46b19affc4b2 188 checkvar = 0;
crmackey 14:46b19affc4b2 189 switch (counter) {
crmackey 14:46b19affc4b2 190 case 1: {
crmackey 14:46b19affc4b2 191 Time1 = LapTimer.read();
crmackey 14:46b19affc4b2 192 LapTimer.reset();
crmackey 14:46b19affc4b2 193 LapTimer.start();
crmackey 14:46b19affc4b2 194 btbee.printf("Lap 1: %f \n", Time1);
crmackey 14:46b19affc4b2 195
crmackey 14:46b19affc4b2 196 break;
crmackey 14:46b19affc4b2 197 }
crmackey 14:46b19affc4b2 198 case 2: {
crmackey 14:46b19affc4b2 199 Time2 = LapTimer.read();
crmackey 14:46b19affc4b2 200 LapTimer.reset();
crmackey 14:46b19affc4b2 201 LapTimer.start();
crmackey 14:46b19affc4b2 202 btbee.printf("Lap 2: %f \n", Time2);
crmackey 14:46b19affc4b2 203
crmackey 14:46b19affc4b2 204 break;
crmackey 14:46b19affc4b2 205 }
crmackey 14:46b19affc4b2 206 case 3: {
crmackey 14:46b19affc4b2 207 Time3 = LapTimer.read();
crmackey 14:46b19affc4b2 208 LapTimer.reset();
crmackey 14:46b19affc4b2 209 LapTimer.start();
crmackey 14:46b19affc4b2 210 btbee.printf("Lap 3: %f \n", Time3);
crmackey 14:46b19affc4b2 211
crmackey 14:46b19affc4b2 212 break;
crmackey 14:46b19affc4b2 213 }
crmackey 14:46b19affc4b2 214 case 4: {
crmackey 14:46b19affc4b2 215 Time4 = LapTimer.read();
crmackey 14:46b19affc4b2 216 LapTimer.reset();
crmackey 14:46b19affc4b2 217 LapTimer.start();
crmackey 14:46b19affc4b2 218 btbee.printf("Lap 4: %f \n", Time4);
crmackey 14:46b19affc4b2 219
crmackey 14:46b19affc4b2 220 break;
crmackey 14:46b19affc4b2 221 }
crmackey 14:46b19affc4b2 222 case 5: {
crmackey 14:46b19affc4b2 223 Time5 = LapTimer.read();
crmackey 14:46b19affc4b2 224 LapTimer.reset();
crmackey 14:46b19affc4b2 225 LapTimer.start();
crmackey 14:46b19affc4b2 226 btbee.printf("Lap 5: %f \n", Time5);
crmackey 14:46b19affc4b2 227
crmackey 14:46b19affc4b2 228 break;
crmackey 14:46b19affc4b2 229 }
crmackey 14:46b19affc4b2 230 }
crmackey 14:46b19affc4b2 231
crmackey 14:46b19affc4b2 232
crmackey 14:46b19affc4b2 233 }
crmackey 14:46b19affc4b2 234
crmackey 14:46b19affc4b2 235 if (counter == 0) {
crmackey 14:46b19affc4b2 236 m3pi.stop();
crmackey 14:46b19affc4b2 237 m3pi.cls();
crmackey 14:46b19affc4b2 238 m3pi.locate(0,0);
crmackey 14:46b19affc4b2 239 m3pi.printf("Ave time");
crmackey 14:46b19affc4b2 240 TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
crmackey 14:46b19affc4b2 241 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 242 m3pi.printf("%f", TimeAve);
crmackey 14:46b19affc4b2 243 btbee.printf("Average time per lap: %f ", TimeAve);
crmackey 14:46b19affc4b2 244 btbee.printf("\n");
crmackey 14:46b19affc4b2 245 btbee.printf("Printing to USB");
crmackey 14:46b19affc4b2 246
crmackey 14:46b19affc4b2 247 DIR *d;
crmackey 14:46b19affc4b2 248 struct dirent *p;
crmackey 14:46b19affc4b2 249
crmackey 14:46b19affc4b2 250 d = opendir("/" FSNAME);
crmackey 14:46b19affc4b2 251
crmackey 14:46b19affc4b2 252 FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
crmackey 14:46b19affc4b2 253 if ( fp == NULL )
crmackey 14:46b19affc4b2 254 {
crmackey 14:46b19affc4b2 255 m3pi.leds(1);
crmackey 14:46b19affc4b2 256 }
crmackey 14:46b19affc4b2 257
crmackey 14:46b19affc4b2 258 for( int j = 0; j < 3; j++ ) {
crmackey 14:46b19affc4b2 259
crmackey 14:46b19affc4b2 260 for( int i = 0; i < Data.size(); i++) {
crmackey 14:46b19affc4b2 261
crmackey 14:46b19affc4b2 262 stuff = Data[i][j];
crmackey 14:46b19affc4b2 263
crmackey 14:46b19affc4b2 264 fprintf(fp,"%f \n", stuff); //message
crmackey 14:46b19affc4b2 265
crmackey 14:46b19affc4b2 266 }
crmackey 14:46b19affc4b2 267
crmackey 14:46b19affc4b2 268 fprintf(fp," ");
crmackey 14:46b19affc4b2 269
crmackey 14:46b19affc4b2 270 }
crmackey 14:46b19affc4b2 271
crmackey 14:46b19affc4b2 272 fclose(fp);
crmackey 14:46b19affc4b2 273
crmackey 14:46b19affc4b2 274 btbee.printf("Printed");
crmackey 14:46b19affc4b2 275
crmackey 14:46b19affc4b2 276
crmackey 14:46b19affc4b2 277 exit(1);
crmackey 14:46b19affc4b2 278 }
crmackey 14:46b19affc4b2 279
crmackey 14:46b19affc4b2 280
crmackey 14:46b19affc4b2 281 }
crmackey 14:46b19affc4b2 282
crmackey 14:46b19affc4b2 283 }
crmackey 14:46b19affc4b2 284
crmackey 14:46b19affc4b2 285 }