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@18:1dc7fdc248aa, 2015-05-29 (annotated)

Committer:
morganrose402
Date:
Fri May 29 13:41:58 2015 +0000
Revision:
18:1dc7fdc248aa
Parent:
17:b9608f93ea06 
more organization  fun

Who changed what in which revision?

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