MEGACARAZ / Mbed 2 deprecated PID

PIDs and fun

Dependencies:   btbee m3pi_ng mbed FatFileSystemCpp

main.cpp@10:fd4ae3946c7e, 2015-05-28 (annotated)

Committer:
morganrose402
Date:
Thu May 28 13:47:23 2015 +0000
Revision:
10:fd4ae3946c7e
Parent:
9:43be0dea44f0
Child:
11:ac6112ca954f
morgan

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