IESS / ILCRobot

This ILC code kinda works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed ILCRobot

Dependents:   ILCRobot

Fork of RobotB by Blake Babbs

main.cpp@22:1288bd38d682, 2015-06-18 (annotated)

Committer:
charwhit
Date:
Thu Jun 18 12:12:13 2015 +0000
Revision:
22:1288bd38d682
Parent:
21:df402d79b2ad 
Commented final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charwhit 12:0422156f83f6 1 #include "mbed.h"
charwhit 8:bd2f012e2f57 2 #include "MSCFileSystem.h"
bbabbs 0:17669460c6b1 3 #include "btbee.h"
bbabbs 0:17669460c6b1 4 #include "m3pi_ng.h"
charwhit 8:bd2f012e2f57 5 #include <fstream>
charwhit 8:bd2f012e2f57 6 #define FSNAME "msc"
charwhit 9:030b7e4ff7be 7 #include <string>
charwhit 9:030b7e4ff7be 8 #include <sstream>
charwhit 9:030b7e4ff7be 9 #include <vector>
charwhit 12:0422156f83f6 10 #include "math.h"
charwhit 20:262739736935 11 #include <algorithm>
charwhit 8:bd2f012e2f57 12
charwhit 8:bd2f012e2f57 13
charwhit 8:bd2f012e2f57 14 MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
charwhit 8:bd2f012e2f57 15
nbtavis 2:80a1ed62c307 16 m3pi robot;
charwhit 4:acd0f86ed832 17 btbee btbee;
nbtavis 3:bae8eb81a9d7 18 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
charwhit 20:262739736935 19 DigitalIn m3pi_pb(p21);
nbtavis 2:80a1ed62c307 20 Timer timer;
nbtavis 2:80a1ed62c307 21 Timer time_wait;
charwhit 22:1288bd38d682 22 #define MAX .96
charwhit 5:c38929c0fd95 23 #define MIN 0
charwhit 12:0422156f83f6 24 #define PI 3.14159265
bbabbs 0:17669460c6b1 25
charwhit 4:acd0f86ed832 26 //#define P_TERM 5
charwhit 4:acd0f86ed832 27 //#define I_TERM 0
charwhit 5:c38929c0fd95 28 //#define D_TERM 20
nbtavis 2:80a1ed62c307 29
nbtavis 2:80a1ed62c307 30
charwhit 22:1288bd38d682 31 //Struct that represents test points in Nedler-Mead. Of the form (P,D) i.e (P=5,D=7)
charwhit 20:262739736935 32 struct testPoint {
charwhit 20:262739736935 33 testPoint():Pvalue(0), Dvalue(0), time(1000) {}
charwhit 20:262739736935 34 testPoint(float a, float b, float c): Pvalue(a), Dvalue(b), time(c) {}
charwhit 20:262739736935 35
charwhit 20:262739736935 36 friend testPoint operator-(const testPoint& lhs, const testPoint& rhs) {
charwhit 20:262739736935 37 testPoint dummy;
charwhit 20:262739736935 38 dummy.Pvalue = lhs.Pvalue - rhs.Pvalue;
charwhit 20:262739736935 39 dummy.Dvalue = lhs.Dvalue - rhs.Dvalue;
charwhit 20:262739736935 40 dummy.time = 1000;
charwhit 20:262739736935 41 return dummy;
charwhit 20:262739736935 42 }
charwhit 20:262739736935 43
charwhit 20:262739736935 44 friend testPoint operator+(const testPoint& lhs, const testPoint& rhs) {
charwhit 20:262739736935 45 testPoint dummy;
charwhit 20:262739736935 46 dummy.Pvalue = lhs.Pvalue + rhs.Pvalue;
charwhit 20:262739736935 47 dummy.Dvalue = lhs.Dvalue + rhs.Dvalue;
charwhit 20:262739736935 48 dummy.time = 1000;
charwhit 20:262739736935 49 return dummy;
charwhit 20:262739736935 50 }
charwhit 20:262739736935 51
charwhit 20:262739736935 52 friend testPoint operator*(const float a, const testPoint& rhs) {
charwhit 20:262739736935 53 testPoint dummy;
charwhit 20:262739736935 54 dummy.Pvalue = a * rhs.Pvalue;
charwhit 20:262739736935 55 dummy.Dvalue = a * rhs.Dvalue;
charwhit 20:262739736935 56 dummy.time = 1000;
charwhit 20:262739736935 57 return dummy;
charwhit 20:262739736935 58
charwhit 20:262739736935 59 }
charwhit 20:262739736935 60
charwhit 20:262739736935 61
charwhit 20:262739736935 62
charwhit 20:262739736935 63 float Pvalue;
charwhit 20:262739736935 64 float Dvalue;
charwhit 20:262739736935 65 float time;
charwhit 20:262739736935 66 };
charwhit 20:262739736935 67
charwhit 20:262739736935 68
charwhit 22:1288bd38d682 69 //Comparator for testPoint struct sorting
charwhit 20:262739736935 70 bool myComp (testPoint i,testPoint j)
charwhit 20:262739736935 71 {
charwhit 20:262739736935 72 return (i.time<j.time);
charwhit 20:262739736935 73 }
charwhit 20:262739736935 74
charwhit 20:262739736935 75
charwhit 20:262739736935 76
bbabbs 0:17669460c6b1 77
charwhit 5:c38929c0fd95 78 int main()
charwhit 5:c38929c0fd95 79 {
charwhit 16:caa77287cc25 80 float P_TERM = 3;
charwhit 16:caa77287cc25 81 float I_TERM = 0;
bbabbs 17:5eb563904361 82 float D_TERM = 27;
charwhit 22:1288bd38d682 83
charwhit 20:262739736935 84
charwhit 20:262739736935 85 m3pi_pb.mode(PullUp);
charwhit 5:c38929c0fd95 86 btbee.reset();
charwhit 20:262739736935 87
charwhit 20:262739736935 88
charwhit 5:c38929c0fd95 89 robot.sensor_auto_calibrate();
charwhit 22:1288bd38d682 90 //Waiting for button to be pressed before starting for bluetooth comms
charwhit 20:262739736935 91 while(m3pi_pb) {
charwhit 20:262739736935 92
charwhit 20:262739736935 93 }
charwhit 5:c38929c0fd95 94 wait(2.0);
charwhit 5:c38929c0fd95 95 float right;
charwhit 5:c38929c0fd95 96 float left;
charwhit 5:c38929c0fd95 97 float current_pos = 0.0;
charwhit 5:c38929c0fd95 98 float previous_pos =0.0;
charwhit 5:c38929c0fd95 99 float derivative, proportional, integral = 0;
charwhit 5:c38929c0fd95 100 float power;
charwhit 5:c38929c0fd95 101 float speed = MAX;
charwhit 5:c38929c0fd95 102
charwhit 5:c38929c0fd95 103 int lap = 0;
charwhit 5:c38929c0fd95 104 float lap_time = 0.0;
charwhit 5:c38929c0fd95 105 int y =1;
charwhit 20:262739736935 106
charwhit 8:bd2f012e2f57 107 bool passed = false;
charwhit 20:262739736935 108
charwhit 20:262739736935 109 Timer crossProtect;
charwhit 20:262739736935 110
charwhit 20:262739736935 111
charwhit 20:262739736935 112 //Simplex algorithm stuff
charwhit 20:262739736935 113 vector< testPoint > triangle;
charwhit 20:262739736935 114 triangle.push_back(testPoint(20,2,1000));
charwhit 20:262739736935 115 triangle.push_back(testPoint(2,40,1000));
charwhit 20:262739736935 116 triangle.push_back(testPoint(3,27,1000));
charwhit 20:262739736935 117 int corners = 0;
charwhit 20:262739736935 118 P_TERM = triangle[0].Pvalue;
charwhit 20:262739736935 119 D_TERM = triangle[0].Dvalue;
charwhit 20:262739736935 120 testPoint centroid;
charwhit 20:262739736935 121 testPoint reflection;
charwhit 20:262739736935 122 testPoint expansion;
charwhit 20:262739736935 123 testPoint contraction;
charwhit 20:262739736935 124 testPoint reduction;
charwhit 20:262739736935 125 int step = 1;
charwhit 20:262739736935 126 bool contracted = false;
charwhit 20:262739736935 127 float reflecFactor = 1;
charwhit 20:262739736935 128 float expanFactor = 2;
charwhit 20:262739736935 129 float contrFactor = -1/2;
charwhit 20:262739736935 130 float reducFactor = 1/2;
charwhit 22:1288bd38d682 131 Timer backup; //Timer so robot doesnt count a lap when it swerves wierdly
charwhit 22:1288bd38d682 132
charwhit 22:1288bd38d682 133 //Max and min values for P and D parameters
charwhit 21:df402d79b2ad 134 int MINPVAL = 2;
charwhit 21:df402d79b2ad 135 int MAXPVAL = 30;
charwhit 22:1288bd38d682 136 int MAXDVAL= 40;
charwhit 16:caa77287cc25 137
charwhit 16:caa77287cc25 138
charwhit 5:c38929c0fd95 139
charwhit 5:c38929c0fd95 140 time_wait.start();
charwhit 20:262739736935 141 backup.start();
charwhit 16:caa77287cc25 142 int x [5];
charwhit 5:c38929c0fd95 143 while(y) {
charwhit 5:c38929c0fd95 144 time_wait.reset();
charwhit 22:1288bd38d682 145
charwhit 5:c38929c0fd95 146 //Get raw sensor values
charwhit 5:c38929c0fd95 147 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 148
charwhit 5:c38929c0fd95 149 //Check to make sure battery isn't low
charwhit 5:c38929c0fd95 150 if (robot.battery() < 2.4) {
charwhit 5:c38929c0fd95 151 timer.stop();
charwhit 16:caa77287cc25 152 robot.printf("LowBatt");
charwhit 6:99d09e88924b 153 btbee.printf("Battery too low\n");
charwhit 5:c38929c0fd95 154 break;
charwhit 5:c38929c0fd95 155 }
charwhit 5:c38929c0fd95 156
charwhit 22:1288bd38d682 157
charwhit 16:caa77287cc25 158
charwhit 20:262739736935 159 //running the four corners
charwhit 20:262739736935 160 if(x[0] > 300 && x[2]>300 && x[4]>300 && !passed && (backup.read() > 2 || lap == 0)) {
charwhit 20:262739736935 161 btbee.printf("Crossing\n");
charwhit 22:1288bd38d682 162 if (lap == 0) { //Allows for 10cm head start
charwhit 5:c38929c0fd95 163 timer.start();
charwhit 5:c38929c0fd95 164 lap= lap +1;
charwhit 22:1288bd38d682 165 } else if(corners < 2) { // This condition get lap times for the intial group of 3 points
charwhit 5:c38929c0fd95 166 lap_time = timer.read();
charwhit 20:262739736935 167 triangle[corners].time = lap_time;
charwhit 22:1288bd38d682 168
charwhit 20:262739736935 169 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 22:1288bd38d682 170
charwhit 20:262739736935 171
charwhit 20:262739736935 172 ++corners;
bbabbs 17:5eb563904361 173 robot.cls();
charwhit 20:262739736935 174 robot.printf("%f", lap_time);
charwhit 20:262739736935 175
charwhit 20:262739736935 176 ++lap;
charwhit 20:262739736935 177 P_TERM = triangle[corners].Pvalue;
charwhit 20:262739736935 178 D_TERM = triangle[corners].Dvalue;
charwhit 18:dcdf28fa4c9e 179 timer.reset();
charwhit 20:262739736935 180 } else {
charwhit 20:262739736935 181 if (corners == 2){
charwhit 20:262739736935 182 triangle[corners].time = timer.read();
charwhit 20:262739736935 183 ++corners;
charwhit 20:262739736935 184 }
charwhit 22:1288bd38d682 185 switch(step) { //Each step corresponds to a step in Nedler-Mead
charwhit 20:262739736935 186 case 1:
charwhit 22:1288bd38d682 187 if (triangle[1].time == 1000 || triangle[2].time == 1000){ //Getting new data if reduced
charwhit 20:262739736935 188 if (triangle[1].time == 1000){
charwhit 20:262739736935 189 triangle[1].time = timer.read();
charwhit 20:262739736935 190 P_TERM = triangle[2].Pvalue;
charwhit 20:262739736935 191 D_TERM = triangle[2].Dvalue;
charwhit 20:262739736935 192 timer.reset();
charwhit 20:262739736935 193 break;
charwhit 20:262739736935 194 }
charwhit 20:262739736935 195 else
charwhit 20:262739736935 196 triangle[2].time = timer.read();
charwhit 20:262739736935 197 }
charwhit 20:262739736935 198 sort(triangle.begin(), triangle.end(), myComp);
charwhit 20:262739736935 199 for (int i = 0; i < triangle.size(); ++i)
charwhit 20:262739736935 200 btbee.printf("Time %d: %f\n",i,triangle[i].time);
charwhit 21:df402d79b2ad 201 btbee.printf("Best so far: P: %f, D: %f\n", triangle[0].Pvalue, triangle[0].Dvalue);
charwhit 20:262739736935 202 centroid.Pvalue = (triangle[0].Pvalue + triangle[1].Pvalue) / 2;
charwhit 20:262739736935 203 centroid.Dvalue = (triangle[0].Dvalue + triangle[1].Dvalue) / 2;
charwhit 20:262739736935 204 reflection = centroid + (reflecFactor*(centroid - triangle[2]));
charwhit 20:262739736935 205 if (reflection.Pvalue < 0)
charwhit 21:df402d79b2ad 206 reflection.Pvalue = MINPVAL;
charwhit 20:262739736935 207 else if (reflection.Pvalue > 30)
charwhit 21:df402d79b2ad 208 reflection.Pvalue = MAXPVAL;
charwhit 20:262739736935 209
charwhit 20:262739736935 210 if (reflection.Dvalue < 0)
charwhit 20:262739736935 211 reflection.Dvalue = 1;
charwhit 21:df402d79b2ad 212 else if (reflection.Dvalue > 40)
charwhit 21:df402d79b2ad 213 reflection.Dvalue = MAXDVAL;
charwhit 20:262739736935 214 P_TERM = reflection.Pvalue;
charwhit 20:262739736935 215 D_TERM = reflection.Dvalue;
charwhit 20:262739736935 216 ++step;
charwhit 20:262739736935 217 btbee.printf("Reflecting\n");
charwhit 20:262739736935 218 btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
charwhit 20:262739736935 219 poop = true;
charwhit 20:262739736935 220 timer.reset();
charwhit 20:262739736935 221 break;
charwhit 20:262739736935 222 case 2: //finished running reflection
charwhit 20:262739736935 223 reflection.time = timer.read();
charwhit 20:262739736935 224 btbee.printf("Reflection Time: %f\n", reflection.time);
charwhit 20:262739736935 225 if (reflection.time < triangle[0].time) {
charwhit 20:262739736935 226 expansion = centroid + expanFactor*(centroid - triangle[2]);
charwhit 20:262739736935 227 if (expansion.Pvalue < 0)
charwhit 21:df402d79b2ad 228 expansion.Pvalue = MINPVAL;
charwhit 20:262739736935 229 else if (expansion.Pvalue > 30)
charwhit 21:df402d79b2ad 230 expansion.Pvalue = MAXPVAL;
charwhit 20:262739736935 231
charwhit 20:262739736935 232 if (expansion.Dvalue < 0)
charwhit 20:262739736935 233 expansion.Dvalue = 1;
charwhit 21:df402d79b2ad 234 else if (expansion.Dvalue > 40)
charwhit 21:df402d79b2ad 235 expansion.Dvalue = MAXDVAL;
charwhit 20:262739736935 236 P_TERM = expansion.Pvalue;
charwhit 20:262739736935 237 D_TERM = expansion.Dvalue;
charwhit 20:262739736935 238 ++step;
charwhit 20:262739736935 239 btbee.printf("Expanding\n");
charwhit 20:262739736935 240 btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
charwhit 20:262739736935 241 timer.reset();
charwhit 20:262739736935 242 } else if (reflection.time < triangle[1].time) {
charwhit 20:262739736935 243 triangle[2] = reflection;
charwhit 20:262739736935 244 step = 1;
charwhit 20:262739736935 245 btbee.printf("Going back to 1\n");
charwhit 20:262739736935 246 P_TERM = 3;
charwhit 20:262739736935 247 D_TERM = 27;
charwhit 20:262739736935 248 } else {
charwhit 20:262739736935 249
charwhit 20:262739736935 250 contraction = centroid + contrFactor*(centroid - triangle[2]);
charwhit 20:262739736935 251 if (contraction.Pvalue < 0)
charwhit 21:df402d79b2ad 252 contraction.Pvalue = MINPVAL;
charwhit 20:262739736935 253 else if (contraction.Pvalue > 30)
charwhit 21:df402d79b2ad 254 contraction.Pvalue = MAXPVAL;
charwhit 20:262739736935 255
charwhit 20:262739736935 256 if (contraction.Dvalue < 0)
charwhit 20:262739736935 257 contraction.Dvalue = 1;
charwhit 21:df402d79b2ad 258 else if (contraction.Dvalue > 40)
charwhit 21:df402d79b2ad 259 contraction.Dvalue = MAXDVAL;
charwhit 20:262739736935 260 P_TERM = contraction.Pvalue;
charwhit 20:262739736935 261 D_TERM = contraction.Dvalue;
charwhit 20:262739736935 262 ++step;
charwhit 20:262739736935 263 contracted = true;
charwhit 20:262739736935 264 btbee.printf("Contracting\n");
charwhit 20:262739736935 265 btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
charwhit 20:262739736935 266 timer.reset();
charwhit 20:262739736935 267 }
charwhit 20:262739736935 268 break;
charwhit 20:262739736935 269 case 3: //Finished running Expansion/Contraction
charwhit 20:262739736935 270 if (!contracted) {
charwhit 20:262739736935 271 expansion.time = timer.read();
charwhit 20:262739736935 272 btbee.printf("Expansion Time: %f\n", expansion.time);
charwhit 20:262739736935 273 if (expansion.time < reflection.time)
charwhit 20:262739736935 274 triangle[2] = expansion;
charwhit 20:262739736935 275 else
charwhit 20:262739736935 276 triangle[2] = reflection;
charwhit 20:262739736935 277 P_TERM = 3;
charwhit 20:262739736935 278 D_TERM = 27;
charwhit 20:262739736935 279 } else {
charwhit 20:262739736935 280 contraction.time = timer.read();
charwhit 20:262739736935 281 btbee.printf("Contraction Time: %f\n", contraction.time);
charwhit 20:262739736935 282 if (contraction.time < triangle[2].time){
charwhit 20:262739736935 283 triangle[2] = contraction;
charwhit 20:262739736935 284 P_TERM = 3;
charwhit 20:262739736935 285 D_TERM = 27;
charwhit 20:262739736935 286 }
charwhit 20:262739736935 287 else {
charwhit 20:262739736935 288 triangle[1] = triangle[0] + (reducFactor*(triangle[1] - triangle[0]));
charwhit 20:262739736935 289 triangle[2] = triangle[0] + (reducFactor*(triangle[2] - triangle[0]));
charwhit 20:262739736935 290 P_TERM = triangle[1].Pvalue;
charwhit 20:262739736935 291 D_TERM = triangle[1].Dvalue;
charwhit 20:262739736935 292 timer.reset();
charwhit 20:262739736935 293 }
charwhit 20:262739736935 294 }
charwhit 20:262739736935 295 step = 1;
charwhit 20:262739736935 296 contracted = false;
charwhit 20:262739736935 297 btbee.printf("Going back to 1\n");
charwhit 20:262739736935 298 break;
charwhit 20:262739736935 299 }//end of switch
charwhit 20:262739736935 300 btbee.printf("Step: %d\n", step);
charwhit 5:c38929c0fd95 301 }
charwhit 8:bd2f012e2f57 302 passed = true;
charwhit 20:262739736935 303 backup.reset();
charwhit 16:caa77287cc25 304 } else if (x[0] > 300 && x[2]>300 && x[4]>300)
charwhit 8:bd2f012e2f57 305 passed = true;
charwhit 8:bd2f012e2f57 306 else
charwhit 8:bd2f012e2f57 307 passed = false;
charwhit 16:caa77287cc25 308
charwhit 5:c38929c0fd95 309
nbtavis 2:80a1ed62c307 310
charwhit 5:c38929c0fd95 311 current_pos = robot.line_position();
charwhit 16:caa77287cc25 312
charwhit 16:caa77287cc25 313
charwhit 16:caa77287cc25 314 proportional = current_pos;
charwhit 16:caa77287cc25 315
charwhit 16:caa77287cc25 316
charwhit 5:c38929c0fd95 317 derivative = current_pos - previous_pos;
charwhit 5:c38929c0fd95 318
charwhit 5:c38929c0fd95 319
charwhit 5:c38929c0fd95 320 //compute the integral
charwhit 5:c38929c0fd95 321 integral =+ proportional;
charwhit 5:c38929c0fd95 322
charwhit 5:c38929c0fd95 323 //remember the last position.
charwhit 5:c38929c0fd95 324 previous_pos = current_pos;
charwhit 20:262739736935 325
charwhit 5:c38929c0fd95 326
charwhit 5:c38929c0fd95 327 // compute the power
charwhit 20:262739736935 328 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 22:1288bd38d682 329
charwhit 20:262739736935 330
charwhit 5:c38929c0fd95 331 //computer new speeds
charwhit 5:c38929c0fd95 332 right = speed+power;
charwhit 5:c38929c0fd95 333 left = speed-power;
charwhit 5:c38929c0fd95 334
charwhit 5:c38929c0fd95 335 //limit checks
charwhit 5:c38929c0fd95 336 if(right<MIN)
charwhit 5:c38929c0fd95 337 right = MIN;
charwhit 5:c38929c0fd95 338 else if (right > MAX)
charwhit 5:c38929c0fd95 339 right = MAX;
charwhit 5:c38929c0fd95 340
charwhit 5:c38929c0fd95 341 if(left<MIN)
charwhit 5:c38929c0fd95 342 left = MIN;
charwhit 5:c38929c0fd95 343 else if (left>MIN)
charwhit 5:c38929c0fd95 344 left = MAX;
charwhit 5:c38929c0fd95 345
charwhit 5:c38929c0fd95 346 //set speed
charwhit 5:c38929c0fd95 347 robot.left_motor(left);
charwhit 5:c38929c0fd95 348 robot.right_motor(right);
charwhit 16:caa77287cc25 349
charwhit 16:caa77287cc25 350
charwhit 5:c38929c0fd95 351 wait((5-time_wait.read_ms())/1000);
bbabbs 0:17669460c6b1 352 }
charwhit 5:c38929c0fd95 353
charwhit 5:c38929c0fd95 354
charwhit 5:c38929c0fd95 355
charwhit 5:c38929c0fd95 356 robot.stop();
charwhit 5:c38929c0fd95 357
charwhit 5:c38929c0fd95 358
charwhit 5:c38929c0fd95 359 }