Blake Babbs / RobotB

one lap kind of works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@17:5eb563904361, 2015-06-10 (annotated)

Committer:
bbabbs
Date:
Wed Jun 10 13:20:43 2015 +0000
Revision:
17:5eb563904361
Parent:
16:caa77287cc25 
one lap kind of works

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 8:bd2f012e2f57 11
charwhit 8:bd2f012e2f57 12
charwhit 8:bd2f012e2f57 13 MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
charwhit 9:030b7e4ff7be 14 //Serial pc(USBTX,USBRX);
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
nbtavis 2:80a1ed62c307 19 Timer timer;
nbtavis 2:80a1ed62c307 20 Timer time_wait;
charwhit 16:caa77287cc25 21 #define MAX .9
charwhit 5:c38929c0fd95 22 #define MIN 0
charwhit 12:0422156f83f6 23 #define PI 3.14159265
bbabbs 0:17669460c6b1 24
charwhit 4:acd0f86ed832 25 //#define P_TERM 5
charwhit 4:acd0f86ed832 26 //#define I_TERM 0
charwhit 5:c38929c0fd95 27 //#define D_TERM 20
nbtavis 2:80a1ed62c307 28
nbtavis 2:80a1ed62c307 29
bbabbs 0:17669460c6b1 30
charwhit 5:c38929c0fd95 31 int main()
charwhit 5:c38929c0fd95 32 {
charwhit 16:caa77287cc25 33 float P_TERM = 3;
charwhit 16:caa77287cc25 34 float I_TERM = 0;
bbabbs 17:5eb563904361 35 float D_TERM = 27;
bbabbs 17:5eb563904361 36 int last_lap = 0;
bbabbs 17:5eb563904361 37 int position = 0;
bbabbs 17:5eb563904361 38 float llpp = 0; //last lap previous point
bbabbs 17:5eb563904361 39 float llnp = 0; //last lap next point
bbabbs 17:5eb563904361 40 float llcp = 0; //last lap current point
charwhit 5:c38929c0fd95 41 btbee.reset();
charwhit 5:c38929c0fd95 42 robot.sensor_auto_calibrate();
charwhit 5:c38929c0fd95 43 wait(2.0);
charwhit 5:c38929c0fd95 44 float right;
charwhit 5:c38929c0fd95 45 float left;
charwhit 5:c38929c0fd95 46 //float current_pos[5];
charwhit 5:c38929c0fd95 47 float current_pos = 0.0;
charwhit 5:c38929c0fd95 48 float previous_pos =0.0;
charwhit 5:c38929c0fd95 49 float derivative, proportional, integral = 0;
charwhit 5:c38929c0fd95 50 float power;
charwhit 5:c38929c0fd95 51 float speed = MAX;
charwhit 5:c38929c0fd95 52
charwhit 5:c38929c0fd95 53 int lap = 0;
charwhit 5:c38929c0fd95 54 float lap_time = 0.0;
charwhit 5:c38929c0fd95 55 float total_time = 0.0;
charwhit 5:c38929c0fd95 56 float average_time = 0.0;
charwhit 5:c38929c0fd95 57 int y =1;
charwhit 5:c38929c0fd95 58 int count = 0;
charwhit 7:7d491b51665e 59 float paramChange[3];
charwhit 8:bd2f012e2f57 60 bool passed = false;
charwhit 12:0422156f83f6 61 char sweepValue[100];
charwhit 16:caa77287cc25 62
charwhit 16:caa77287cc25 63
charwhit 5:c38929c0fd95 64
charwhit 4:acd0f86ed832 65 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
charwhit 4:acd0f86ed832 66 int chars_read;
bbabbs 17:5eb563904361 67 vector<float> Uvalue, lineposval, setPointVals, Currentvec;
charwhit 12:0422156f83f6 68 vector<float> sweepData;
charwhit 16:caa77287cc25 69 FILE *fp = fopen( "/" FSNAME "/exper1.txt", "w");
charwhit 16:caa77287cc25 70 //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");
charwhit 16:caa77287cc25 71
charwhit 16:caa77287cc25 72
charwhit 16:caa77287cc25 73
charwhit 16:caa77287cc25 74 /*if (sweep == NULL) {
charwhit 12:0422156f83f6 75 robot.printf("Nope.");
charwhit 12:0422156f83f6 76 y = 0;
charwhit 15:3e3f1b42cc23 77 }*/
charwhit 9:030b7e4ff7be 78
nbtavis 2:80a1ed62c307 79
charwhit 5:c38929c0fd95 80 /* for (int i = 0; i <5; ++i)
charwhit 5:c38929c0fd95 81 current_pos[i] = 0.0; */
charwhit 16:caa77287cc25 82
charwhit 16:caa77287cc25 83
charwhit 9:030b7e4ff7be 84 //wait(8);
bbabbs 17:5eb563904361 85 // btbee.printf("Battery: %f\n", robot.battery());
charwhit 16:caa77287cc25 86
charwhit 16:caa77287cc25 87
charwhit 16:caa77287cc25 88 //btbee.printf("C: %d", sweepData.size());
charwhit 5:c38929c0fd95 89
charwhit 5:c38929c0fd95 90
charwhit 5:c38929c0fd95 91 time_wait.start();
charwhit 16:caa77287cc25 92 int x [5];
charwhit 16:caa77287cc25 93 //timer.start();
charwhit 5:c38929c0fd95 94 while(y) {
charwhit 5:c38929c0fd95 95 time_wait.reset();
charwhit 16:caa77287cc25 96 //timer.reset();
charwhit 5:c38929c0fd95 97 //Get raw sensor values
charwhit 16:caa77287cc25 98
charwhit 5:c38929c0fd95 99 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 100
charwhit 5:c38929c0fd95 101
charwhit 5:c38929c0fd95 102
charwhit 5:c38929c0fd95 103 //Check to make sure battery isn't low
charwhit 5:c38929c0fd95 104 if (robot.battery() < 2.4) {
charwhit 5:c38929c0fd95 105 timer.stop();
charwhit 16:caa77287cc25 106 robot.printf("LowBatt");
charwhit 6:99d09e88924b 107 btbee.printf("Battery too low\n");
charwhit 5:c38929c0fd95 108 break;
charwhit 5:c38929c0fd95 109 }
charwhit 5:c38929c0fd95 110
charwhit 5:c38929c0fd95 111 //else if (m3pi_IN [0] == 0)
charwhit 5:c38929c0fd95 112 //{break;}
charwhit 16:caa77287cc25 113
charwhit 14:5b3cc7741bdf 114 if( (x[0] > 300 && x[2]>300 && x[4]>300 & !passed)) {
charwhit 5:c38929c0fd95 115 if (lap == 0) {
charwhit 8:bd2f012e2f57 116 /*while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 117 robot.calibrated_sensor(x);
charwhit 8:bd2f012e2f57 118 }*/
charwhit 5:c38929c0fd95 119 timer.start();
charwhit 5:c38929c0fd95 120 lap= lap +1;
charwhit 5:c38929c0fd95 121 }
charwhit 5:c38929c0fd95 122
charwhit 14:5b3cc7741bdf 123 else if (lap == 1) {
charwhit 16:caa77287cc25 124
charwhit 6:99d09e88924b 125 robot.stop();
charwhit 16:caa77287cc25 126
charwhit 16:caa77287cc25 127 //Print to file stuff
charwhit 16:caa77287cc25 128 /*if(fp != NULL) {
charwhit 12:0422156f83f6 129 for (int i = 0; i < Uvalue.size(); ++i)
charwhit 14:5b3cc7741bdf 130 fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
charwhit 10:83dfbc5e93ed 131 fclose(fp);
charwhit 15:3e3f1b42cc23 132 //fclose(sweep);
charwhit 10:83dfbc5e93ed 133 robot.cls();
charwhit 10:83dfbc5e93ed 134 robot.printf("Doner");
charwhit 11:bb90691e5166 135 }
charwhit 16:caa77287cc25 136 break;*/
charwhit 5:c38929c0fd95 137 lap_time = timer.read();
charwhit 5:c38929c0fd95 138 total_time += lap_time;
charwhit 5:c38929c0fd95 139 average_time = total_time/lap;
bbabbs 17:5eb563904361 140 robot.cls();
charwhit 5:c38929c0fd95 141 robot.printf("%f",average_time);
bbabbs 17:5eb563904361 142 // if (btbee.writeable()) {
bbabbs 17:5eb563904361 143 // btbee.printf("Lap %d time: %f\n", lap, lap_time);
bbabbs 17:5eb563904361 144 // btbee.printf("Avg Lap time: %f\n", average_time);
bbabbs 17:5eb563904361 145 // }
charwhit 16:caa77287cc25 146
charwhit 16:caa77287cc25 147 /*while (count < 3) {
charwhit 5:c38929c0fd95 148 btbee.read_line(arr_read, 30, &chars_read);
charwhit 7:7d491b51665e 149 paramChange[count] = atof(arr_read);
charwhit 5:c38929c0fd95 150 count++;
charwhit 5:c38929c0fd95 151 }
charwhit 5:c38929c0fd95 152 P_TERM = paramChange[0];
charwhit 5:c38929c0fd95 153 I_TERM = paramChange[1];
charwhit 5:c38929c0fd95 154 D_TERM = paramChange[2];
charwhit 6:99d09e88924b 155 btbee.printf("PTERM %f\n", P_TERM);
charwhit 6:99d09e88924b 156 btbee.printf("ITERM %f\n", I_TERM);
charwhit 6:99d09e88924b 157 btbee.printf("DTERM %f\n", D_TERM);
charwhit 5:c38929c0fd95 158 lap = 0;
charwhit 5:c38929c0fd95 159 total_time = 0;
charwhit 5:c38929c0fd95 160 count = 0;
charwhit 6:99d09e88924b 161 timer.stop();
charwhit 6:99d09e88924b 162 timer.reset();
charwhit 16:caa77287cc25 163 continue;*/
bbabbs 17:5eb563904361 164 // break;
bbabbs 17:5eb563904361 165
bbabbs 17:5eb563904361 166
charwhit 16:caa77287cc25 167
charwhit 5:c38929c0fd95 168 } else {
charwhit 8:bd2f012e2f57 169 /*while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 170 robot.calibrated_sensor(x);
charwhit 8:bd2f012e2f57 171 }*/
charwhit 5:c38929c0fd95 172 lap_time = timer.read();
bbabbs 17:5eb563904361 173 // if (btbee.writeable()) {
bbabbs 17:5eb563904361 174 // btbee.printf("Lap %d time: %f\n", lap, lap_time);
bbabbs 17:5eb563904361 175 // }
charwhit 5:c38929c0fd95 176 total_time += lap_time;
charwhit 5:c38929c0fd95 177 average_time = total_time/lap;
charwhit 5:c38929c0fd95 178 lap = lap +1;
charwhit 16:caa77287cc25 179 timer.reset();
bbabbs 17:5eb563904361 180 robot.cls();
bbabbs 17:5eb563904361 181 robot.print("%f", lap_time);
charwhit 5:c38929c0fd95 182 }
charwhit 8:bd2f012e2f57 183 passed = true;
charwhit 16:caa77287cc25 184 } else if (x[0] > 300 && x[2]>300 && x[4]>300)
charwhit 8:bd2f012e2f57 185 passed = true;
charwhit 8:bd2f012e2f57 186 else
charwhit 8:bd2f012e2f57 187 passed = false;
charwhit 16:caa77287cc25 188
charwhit 5:c38929c0fd95 189
nbtavis 2:80a1ed62c307 190
charwhit 5:c38929c0fd95 191 // Get the position of the line.
charwhit 5:c38929c0fd95 192 /* for (int i =0; i < 4; ++i)
charwhit 5:c38929c0fd95 193 current_pos[i] = current_pos[i+1];
charwhit 5:c38929c0fd95 194 current_pos[4] = robot.line_position();
charwhit 5:c38929c0fd95 195 proportional = current_pos[4];
charwhit 5:c38929c0fd95 196
charwhit 5:c38929c0fd95 197 // compute the derivative
charwhit 5:c38929c0fd95 198 derivative = 0;
charwhit 5:c38929c0fd95 199 for (int i =1; i<5;++i) {
charwhit 5:c38929c0fd95 200 if (i ==1)
charwhit 5:c38929c0fd95 201 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 202 else if (i == 2)
charwhit 5:c38929c0fd95 203 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 204 else if (i==3)
charwhit 5:c38929c0fd95 205 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 206 else
charwhit 5:c38929c0fd95 207 derivative += (current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 208 }
charwhit 5:c38929c0fd95 209
charwhit 5:c38929c0fd95 210 derivative = derivative; */
charwhit 5:c38929c0fd95 211
charwhit 5:c38929c0fd95 212
charwhit 5:c38929c0fd95 213 current_pos = robot.line_position();
charwhit 16:caa77287cc25 214
charwhit 16:caa77287cc25 215 //Performs the sweep
charwhit 16:caa77287cc25 216 /*if (fgets(sweepValue, 100, sweep) != NULL){
charwhit 16:caa77287cc25 217 proportional = atof(sweepValue) + current_pos;
charwhit 16:caa77287cc25 218 }
charwhit 16:caa77287cc25 219 else {
charwhit 16:caa77287cc25 220 robot.stop();
charwhit 16:caa77287cc25 221 //fclose(sweep);
charwhit 16:caa77287cc25 222 if(fp != NULL) {
charwhit 16:caa77287cc25 223 for (int i = 0; i < Uvalue.size(); ++i)
charwhit 16:caa77287cc25 224 fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
charwhit 16:caa77287cc25 225 fclose(fp);
charwhit 16:caa77287cc25 226 fclose(sweep);
charwhit 16:caa77287cc25 227 robot.cls();
charwhit 16:caa77287cc25 228 robot.printf("Doner");
charwhit 16:caa77287cc25 229 }
charwhit 16:caa77287cc25 230 break;
charwhit 16:caa77287cc25 231 //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");
charwhit 16:caa77287cc25 232 //robot.cls();
charwhit 16:caa77287cc25 233 //robot.printf("Done.");
charwhit 16:caa77287cc25 234 // break;
charwhit 15:3e3f1b42cc23 235 }*/
charwhit 16:caa77287cc25 236
charwhit 16:caa77287cc25 237 proportional = current_pos;
charwhit 16:caa77287cc25 238
charwhit 16:caa77287cc25 239
charwhit 5:c38929c0fd95 240
charwhit 5:c38929c0fd95 241 derivative = current_pos - previous_pos;
charwhit 5:c38929c0fd95 242
charwhit 5:c38929c0fd95 243
charwhit 5:c38929c0fd95 244 //compute the integral
charwhit 5:c38929c0fd95 245 integral =+ proportional;
charwhit 5:c38929c0fd95 246
charwhit 5:c38929c0fd95 247 //remember the last position.
charwhit 5:c38929c0fd95 248 previous_pos = current_pos;
bbabbs 17:5eb563904361 249 if(last_lap != lap){
bbabbs 17:5eb563904361 250 position = 0;
bbabbs 17:5eb563904361 251 llpp = llcp = llnp = 0;
bbabbs 17:5eb563904361 252 }
bbabbs 17:5eb563904361 253 last_lap = lap;
charwhit 5:c38929c0fd95 254
charwhit 5:c38929c0fd95 255 // compute the power
bbabbs 17:5eb563904361 256 if(lap == 1){
bbabbs 17:5eb563904361 257 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
bbabbs 17:5eb563904361 258 Uvalue.push_back(power);
bbabbs 17:5eb563904361 259 Currentvec.push_back(current_pos);
bbabbs 17:5eb563904361 260 }
bbabbs 17:5eb563904361 261 else if(lap == 0){
bbabbs 17:5eb563904361 262 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
bbabbs 17:5eb563904361 263 }
bbabbs 17:5eb563904361 264 else{
bbabbs 17:5eb563904361 265 llpp = llcp;
bbabbs 17:5eb563904361 266 llcp = llnp;
bbabbs 17:5eb563904361 267 llnp = Currentvec[position + 1];
bbabbs 17:5eb563904361 268 power = Uvalue[position] + (P_TERM)*llcp + (D_TERM*(llpp + llnp) / .01);
bbabbs 17:5eb563904361 269 Uvalue[position] = power;
bbabbs 17:5eb563904361 270 Currentvec[position] = llcp;
bbabbs 17:5eb563904361 271 position++;
bbabbs 17:5eb563904361 272 }
bbabbs 17:5eb563904361 273
charwhit 16:caa77287cc25 274 /*if (fgets(sweepValue, 100, sweep) != NULL){
charwhit 16:caa77287cc25 275 power = atof(sweepValue) + power;
charwhit 16:caa77287cc25 276 }
charwhit 16:caa77287cc25 277 else {
charwhit 16:caa77287cc25 278 robot.stop();
charwhit 16:caa77287cc25 279 fclose(sweep);
charwhit 16:caa77287cc25 280 if(fp != NULL) {
charwhit 16:caa77287cc25 281 for (int i = 0; i < Uvalue.size(); ++i)
charwhit 16:caa77287cc25 282 fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
charwhit 16:caa77287cc25 283 fclose(fp);
charwhit 16:caa77287cc25 284 fclose(sweep);
charwhit 16:caa77287cc25 285 robot.cls();
charwhit 16:caa77287cc25 286 robot.printf("Doner");
charwhit 16:caa77287cc25 287 }
charwhit 16:caa77287cc25 288 break;
charwhit 16:caa77287cc25 289 }*/
charwhit 5:c38929c0fd95 290 //computer new speeds
charwhit 5:c38929c0fd95 291 right = speed+power;
charwhit 5:c38929c0fd95 292 left = speed-power;
charwhit 5:c38929c0fd95 293
charwhit 5:c38929c0fd95 294 //limit checks
charwhit 5:c38929c0fd95 295 if(right<MIN)
charwhit 5:c38929c0fd95 296 right = MIN;
charwhit 5:c38929c0fd95 297 else if (right > MAX)
charwhit 5:c38929c0fd95 298 right = MAX;
charwhit 5:c38929c0fd95 299
charwhit 5:c38929c0fd95 300 if(left<MIN)
charwhit 5:c38929c0fd95 301 left = MIN;
charwhit 5:c38929c0fd95 302 else if (left>MIN)
charwhit 5:c38929c0fd95 303 left = MAX;
charwhit 5:c38929c0fd95 304
charwhit 5:c38929c0fd95 305 //set speed
charwhit 5:c38929c0fd95 306
charwhit 5:c38929c0fd95 307 robot.left_motor(left);
charwhit 5:c38929c0fd95 308 robot.right_motor(right);
charwhit 16:caa77287cc25 309
charwhit 16:caa77287cc25 310
charwhit 16:caa77287cc25 311
bbabbs 17:5eb563904361 312 // Uvalue.push_back(power);
bbabbs 17:5eb563904361 313 // Currentvec.push_back(current_pos);
bbabbs 17:5eb563904361 314 // lineposval.push_back(robot.line_position());
bbabbs 17:5eb563904361 315 // setPointVals.push_back(0);
charwhit 16:caa77287cc25 316 //setPointVals.push_back(0.5);
charwhit 16:caa77287cc25 317
charwhit 16:caa77287cc25 318
charwhit 16:caa77287cc25 319
charwhit 5:c38929c0fd95 320 wait((5-time_wait.read_ms())/1000);
bbabbs 0:17669460c6b1 321 }
charwhit 5:c38929c0fd95 322
charwhit 5:c38929c0fd95 323
charwhit 5:c38929c0fd95 324
charwhit 5:c38929c0fd95 325 robot.stop();
charwhit 5:c38929c0fd95 326
charwhit 16:caa77287cc25 327 /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 5:c38929c0fd95 328 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 5:c38929c0fd95 329 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 5:c38929c0fd95 330 ,'G','8','E','8','D','8','C','4'
charwhit 5:c38929c0fd95 331 };
nbtavis 2:80a1ed62c307 332 int numb = 59;
charwhit 5:c38929c0fd95 333
charwhit 16:caa77287cc25 334 robot.playtune(hail,numb);*/
charwhit 5:c38929c0fd95 335
charwhit 5:c38929c0fd95 336
charwhit 5:c38929c0fd95 337
charwhit 5:c38929c0fd95 338
charwhit 5:c38929c0fd95 339 }