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@17:b9608f93ea06, 2015-05-29 (annotated)

Committer:
morganrose402
Date:
Fri May 29 13:40:56 2015 +0000
Revision:
17:b9608f93ea06
Parent:
16:b09cc5b75049
Child:
18:1dc7fdc248aa
fixed the organization a bit

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 16:b09cc5b75049 91 double 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 16:b09cc5b75049 98 vector < double > ZeTimes(2);
crmackey 16:b09cc5b75049 99 vector < vector < double > > Data;
crmackey 16:b09cc5b75049 100 double stuff;
crmackey 16:b09cc5b75049 101 double 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 }
morganrose402 17:b9608f93ea06 221 }
crmackey 14:46b19affc4b2 222 }
crmackey 14:46b19affc4b2 223
crmackey 15:62648eb5df9f 224 ZeTimes[0]= stuff;
crmackey 15:62648eb5df9f 225 ZeTimes[1] = power;
crmackey 16:b09cc5b75049 226 Data.push_back(ZeTimes);
crmackey 15:62648eb5df9f 227
crmackey 15:62648eb5df9f 228 loopcounter++;
crmackey 15:62648eb5df9f 229
crmackey 15:62648eb5df9f 230
crmackey 15:62648eb5df9f 231 timer = LoopTime.read();
crmackey 15:62648eb5df9f 232
crmackey 15:62648eb5df9f 233 if(timer < 0.005) {
morganrose402 17:b9608f93ea06 234 wait(0.005 - timer);
crmackey 15:62648eb5df9f 235 }
morganrose402 17:b9608f93ea06 236
crmackey 15:62648eb5df9f 237 timer = 0;
crmackey 15:62648eb5df9f 238 LoopTime.reset();
crmackey 15:62648eb5df9f 239
crmackey 16:b09cc5b75049 240 if (loopcounter == 10) {
crmackey 14:46b19affc4b2 241 m3pi.stop();
crmackey 14:46b19affc4b2 242 m3pi.cls();
crmackey 14:46b19affc4b2 243 m3pi.locate(0,0);
crmackey 14:46b19affc4b2 244 m3pi.printf("Ave time");
crmackey 14:46b19affc4b2 245 TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
crmackey 14:46b19affc4b2 246 m3pi.locate(0,1);
crmackey 14:46b19affc4b2 247 m3pi.printf("%f", TimeAve);
crmackey 14:46b19affc4b2 248 btbee.printf("Average time per lap: %f ", TimeAve);
crmackey 14:46b19affc4b2 249 btbee.printf("\n");
crmackey 14:46b19affc4b2 250 btbee.printf("Printing to USB");
crmackey 14:46b19affc4b2 251
morganrose402 17:b9608f93ea06 252 WriteTime.start();
crmackey 14:46b19affc4b2 253
morganrose402 17:b9608f93ea06 254 DIR *d;
morganrose402 17:b9608f93ea06 255 struct dirent *p;
morganrose402 17:b9608f93ea06 256 d = opendir("/" FSNAME);
morganrose402 17:b9608f93ea06 257 FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
morganrose402 17:b9608f93ea06 258 if ( fp == NULL ){
morganrose402 17:b9608f93ea06 259 m3pi.leds(1);
crmackey 14:46b19affc4b2 260 }
crmackey 14:46b19affc4b2 261
morganrose402 17:b9608f93ea06 262 for(int i = 0; i < 10; i++) {
morganrose402 17:b9608f93ea06 263 for( int j = 0; j < 2; j++) {
morganrose402 17:b9608f93ea06 264 junk = Data[i][j];
morganrose402 17:b9608f93ea06 265 fprintf(fp,"%f ", junk);
morganrose402 17:b9608f93ea06 266 }
morganrose402 17:b9608f93ea06 267 fprintf(fp,"\r\n");
morganrose402 17:b9608f93ea06 268 }
morganrose402 17:b9608f93ea06 269 m3pi.cls();
morganrose402 17:b9608f93ea06 270 TimeToWrite = WriteTime.read();
morganrose402 17:b9608f93ea06 271 btbee.printf("%f \n", TimeToWrite);
morganrose402 17:b9608f93ea06 272 WriteTime.reset();
crmackey 14:46b19affc4b2 273
morganrose402 17:b9608f93ea06 274 btbee.printf("Printed");
morganrose402 17:b9608f93ea06 275 m3pi.cls();
morganrose402 17:b9608f93ea06 276 m3pi.printf("Printed");
crmackey 15:62648eb5df9f 277
morganrose402 17:b9608f93ea06 278 fclose(fp);
crmackey 14:46b19affc4b2 279
crmackey 14:46b19affc4b2 280 exit(1);
morganrose402 17:b9608f93ea06 281 }
crmackey 14:46b19affc4b2 282
crmackey 14:46b19affc4b2 283
crmackey 15:62648eb5df9f 284
morganrose402 17:b9608f93ea06 285 }
crmackey 14:46b19affc4b2 286
crmackey 14:46b19affc4b2 287 }
crmackey 14:46b19affc4b2 288
crmackey 14:46b19affc4b2 289 }