Nick Tavis / Robot

basic code

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@14:9bb4ac4968d2, 2015-06-01 (annotated)

Committer:
charwhit
Date:
Mon Jun 01 08:10:10 2015 +0000
Revision:
14:9bb4ac4968d2
Parent:
13:0422156f83f6
Child:
15:5b3cc7741bdf
Reading off file and sweep working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charwhit 13:0422156f83f6 1 #include "mbed.h"
charwhit 9:bd2f012e2f57 2 #include "MSCFileSystem.h"
bbabbs 0:17669460c6b1 3 #include "btbee.h"
bbabbs 0:17669460c6b1 4 #include "m3pi_ng.h"
charwhit 9:bd2f012e2f57 5 #include <fstream>
charwhit 9:bd2f012e2f57 6 #define FSNAME "msc"
charwhit 10:030b7e4ff7be 7 #include <string>
charwhit 10:030b7e4ff7be 8 #include <sstream>
charwhit 10:030b7e4ff7be 9 #include <vector>
charwhit 13:0422156f83f6 10 #include "math.h"
charwhit 9:bd2f012e2f57 11
charwhit 9:bd2f012e2f57 12
charwhit 9:bd2f012e2f57 13 MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
charwhit 10:030b7e4ff7be 14 //Serial pc(USBTX,USBRX);
charwhit 9:bd2f012e2f57 15
nbtavis 2:80a1ed62c307 16 m3pi robot;
charwhit 5: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 12:bb90691e5166 21 #define MAX .15
charwhit 6:c38929c0fd95 22 #define MIN 0
charwhit 13:0422156f83f6 23 #define PI 3.14159265
bbabbs 0:17669460c6b1 24
charwhit 5:acd0f86ed832 25 //#define P_TERM 5
charwhit 5:acd0f86ed832 26 //#define I_TERM 0
charwhit 6:c38929c0fd95 27 //#define D_TERM 20
nbtavis 2:80a1ed62c307 28
nbtavis 2:80a1ed62c307 29
bbabbs 0:17669460c6b1 30
charwhit 6:c38929c0fd95 31 int main()
charwhit 6:c38929c0fd95 32 {
charwhit 12:bb90691e5166 33 float P_TERM = 1;
charwhit 12:bb90691e5166 34 float I_TERM = 0;
charwhit 13:0422156f83f6 35 float D_TERM = 2;
charwhit 6:c38929c0fd95 36
charwhit 6:c38929c0fd95 37 btbee.reset();
charwhit 6:c38929c0fd95 38 robot.sensor_auto_calibrate();
charwhit 6:c38929c0fd95 39 wait(2.0);
charwhit 6:c38929c0fd95 40 float right;
charwhit 6:c38929c0fd95 41 float left;
charwhit 6:c38929c0fd95 42 //float current_pos[5];
charwhit 6:c38929c0fd95 43 float current_pos = 0.0;
charwhit 6:c38929c0fd95 44 float previous_pos =0.0;
charwhit 6:c38929c0fd95 45 float derivative, proportional, integral = 0;
charwhit 6:c38929c0fd95 46 float power;
charwhit 6:c38929c0fd95 47 float speed = MAX;
charwhit 6:c38929c0fd95 48
charwhit 6:c38929c0fd95 49 int lap = 0;
charwhit 6:c38929c0fd95 50 float lap_time = 0.0;
charwhit 6:c38929c0fd95 51 float total_time = 0.0;
charwhit 6:c38929c0fd95 52 float average_time = 0.0;
charwhit 6:c38929c0fd95 53 int y =1;
charwhit 6:c38929c0fd95 54 int count = 0;
charwhit 8:7d491b51665e 55 float paramChange[3];
charwhit 9:bd2f012e2f57 56 bool passed = false;
charwhit 13:0422156f83f6 57 char sweepValue[100];
charwhit 13:0422156f83f6 58 int import = 0;
charwhit 10:030b7e4ff7be 59 //string hallo = "Idk what is going on...\n";
charwhit 10:030b7e4ff7be 60 //stringstream ss;
charwhit 6:c38929c0fd95 61
charwhit 5:acd0f86ed832 62 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
charwhit 5:acd0f86ed832 63 int chars_read;
charwhit 13:0422156f83f6 64 vector<float> Uvalue, lineposval;
charwhit 13:0422156f83f6 65 vector<float> sweepData;
charwhit 11:83dfbc5e93ed 66 FILE *fp = fopen( "/" FSNAME "/data.txt", "w");
charwhit 13:0422156f83f6 67 FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
charwhit 9:bd2f012e2f57 68
charwhit 13:0422156f83f6 69 //Manual implementation of sweep function
charwhit 13:0422156f83f6 70 float t = 0;
charwhit 13:0422156f83f6 71 float A = 0.1;
charwhit 13:0422156f83f6 72 float f = 19.8/20;
charwhit 13:0422156f83f6 73 float newSetPoint;
charwhit 9:bd2f012e2f57 74
charwhit 10:030b7e4ff7be 75
charwhit 13:0422156f83f6 76 if (sweep == NULL){
charwhit 13:0422156f83f6 77 robot.printf("Nope.");
charwhit 13:0422156f83f6 78 y = 0;
charwhit 13:0422156f83f6 79 }
charwhit 10:030b7e4ff7be 80
nbtavis 2:80a1ed62c307 81
charwhit 6:c38929c0fd95 82 /* for (int i = 0; i <5; ++i)
charwhit 6:c38929c0fd95 83 current_pos[i] = 0.0; */
charwhit 7:99d09e88924b 84
charwhit 7:99d09e88924b 85
charwhit 10:030b7e4ff7be 86 //wait(8);
charwhit 7:99d09e88924b 87 btbee.printf("Battery: %f\n", robot.battery());
charwhit 7:99d09e88924b 88 //timer.start();
charwhit 13:0422156f83f6 89
charwhit 13:0422156f83f6 90 /*while (fgets(sweepValue, 100, sweep) != NULL){
charwhit 13:0422156f83f6 91 sweepData.push_back(atof(sweepValue));
charwhit 13:0422156f83f6 92 }*/
charwhit 13:0422156f83f6 93 btbee.printf("C: %d", sweepData.size());
charwhit 6:c38929c0fd95 94
charwhit 6:c38929c0fd95 95
charwhit 6:c38929c0fd95 96 time_wait.start();
charwhit 6:c38929c0fd95 97 while(y) {
charwhit 6:c38929c0fd95 98 time_wait.reset();
charwhit 6:c38929c0fd95 99 //Get raw sensor values
charwhit 6:c38929c0fd95 100 int x [5];
charwhit 6:c38929c0fd95 101 robot.calibrated_sensor(x);
charwhit 6:c38929c0fd95 102
charwhit 6:c38929c0fd95 103
charwhit 6:c38929c0fd95 104
charwhit 6:c38929c0fd95 105 //Check to make sure battery isn't low
charwhit 6:c38929c0fd95 106 if (robot.battery() < 2.4) {
charwhit 6:c38929c0fd95 107 timer.stop();
charwhit 7:99d09e88924b 108 btbee.printf("Battery too low\n");
charwhit 6:c38929c0fd95 109 break;
charwhit 6:c38929c0fd95 110 }
charwhit 6:c38929c0fd95 111
charwhit 6:c38929c0fd95 112 //else if (m3pi_IN [0] == 0)
charwhit 6:c38929c0fd95 113 //{break;}
charwhit 12:bb90691e5166 114
charwhit 12:bb90691e5166 115 if( (x[0] > 300 && x[2]>300 && x[4]>300 & !passed) || timer.read() > 5) {
charwhit 6:c38929c0fd95 116 if (lap == 0) {
charwhit 9:bd2f012e2f57 117 /*while( x[0]> 300 && x[4] > 300) {
charwhit 6:c38929c0fd95 118 robot.calibrated_sensor(x);
charwhit 9:bd2f012e2f57 119 }*/
charwhit 6:c38929c0fd95 120 timer.start();
charwhit 6:c38929c0fd95 121 lap= lap +1;
charwhit 6:c38929c0fd95 122 }
charwhit 6:c38929c0fd95 123
charwhit 12:bb90691e5166 124 else if (lap == 1 || timer.read() > 5) {
charwhit 7:99d09e88924b 125 robot.stop();
charwhit 13:0422156f83f6 126 robot.printf("Size: %d", Uvalue.size());
charwhit 12:bb90691e5166 127 if(fp != NULL){
charwhit 13:0422156f83f6 128 for (int i = 0; i < Uvalue.size(); ++i)
charwhit 13:0422156f83f6 129 fprintf(fp,"%f %f\n",Uvalue[i], lineposval[i]);
charwhit 11:83dfbc5e93ed 130 fclose(fp);
charwhit 13:0422156f83f6 131 fclose(sweep);
charwhit 11:83dfbc5e93ed 132 robot.cls();
charwhit 11:83dfbc5e93ed 133 robot.locate(0,0);
charwhit 11:83dfbc5e93ed 134 robot.printf("Doner");
charwhit 12:bb90691e5166 135 }
charwhit 6:c38929c0fd95 136 lap_time = timer.read();
charwhit 6:c38929c0fd95 137 total_time += lap_time;
charwhit 6:c38929c0fd95 138 average_time = total_time/lap;
charwhit 6:c38929c0fd95 139 robot.printf("%f",average_time);
charwhit 6:c38929c0fd95 140 if (btbee.writeable()) {
charwhit 6:c38929c0fd95 141 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 6:c38929c0fd95 142 btbee.printf("Avg Lap time: %f\n", average_time);
charwhit 6:c38929c0fd95 143 }
charwhit 7:99d09e88924b 144
charwhit 6:c38929c0fd95 145 while (count < 3){
charwhit 6:c38929c0fd95 146 //btbee.printf("Input parameter\n");
charwhit 6:c38929c0fd95 147 btbee.read_line(arr_read, 30, &chars_read);
charwhit 8:7d491b51665e 148 paramChange[count] = atof(arr_read);
charwhit 7:99d09e88924b 149 //btbee.printf("%d", arr_read);
charwhit 6:c38929c0fd95 150 count++;
charwhit 6:c38929c0fd95 151 }
charwhit 6:c38929c0fd95 152 P_TERM = paramChange[0];
charwhit 6:c38929c0fd95 153 I_TERM = paramChange[1];
charwhit 6:c38929c0fd95 154 D_TERM = paramChange[2];
charwhit 7:99d09e88924b 155 btbee.printf("PTERM %f\n", P_TERM);
charwhit 7:99d09e88924b 156 btbee.printf("ITERM %f\n", I_TERM);
charwhit 7:99d09e88924b 157 btbee.printf("DTERM %f\n", D_TERM);
charwhit 6:c38929c0fd95 158 lap = 0;
charwhit 6:c38929c0fd95 159 total_time = 0;
charwhit 6:c38929c0fd95 160 count = 0;
charwhit 7:99d09e88924b 161 timer.stop();
charwhit 7:99d09e88924b 162 timer.reset();
charwhit 6:c38929c0fd95 163 continue;
charwhit 6:c38929c0fd95 164
charwhit 6:c38929c0fd95 165 } else {
charwhit 9:bd2f012e2f57 166 /*while( x[0]> 300 && x[4] > 300) {
charwhit 6:c38929c0fd95 167 robot.calibrated_sensor(x);
charwhit 9:bd2f012e2f57 168 }*/
charwhit 6:c38929c0fd95 169 lap_time = timer.read();
charwhit 6:c38929c0fd95 170 if (btbee.writeable()) {
charwhit 6:c38929c0fd95 171 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 6:c38929c0fd95 172 }
charwhit 6:c38929c0fd95 173 total_time += lap_time;
charwhit 6:c38929c0fd95 174 average_time = total_time/lap;
charwhit 6:c38929c0fd95 175 lap = lap +1;
charwhit 12:bb90691e5166 176 //timer.reset();
charwhit 6:c38929c0fd95 177 }
charwhit 9:bd2f012e2f57 178 passed = true;
charwhit 6:c38929c0fd95 179 }
charwhit 9:bd2f012e2f57 180 else if (x[0] > 300 && x[2]>300 && x[4]>300)
charwhit 9:bd2f012e2f57 181 passed = true;
charwhit 9:bd2f012e2f57 182 else
charwhit 9:bd2f012e2f57 183 passed = false;
charwhit 9:bd2f012e2f57 184
charwhit 6:c38929c0fd95 185
nbtavis 2:80a1ed62c307 186
charwhit 6:c38929c0fd95 187 // Get the position of the line.
charwhit 6:c38929c0fd95 188 /* for (int i =0; i < 4; ++i)
charwhit 6:c38929c0fd95 189 current_pos[i] = current_pos[i+1];
charwhit 6:c38929c0fd95 190 current_pos[4] = robot.line_position();
charwhit 6:c38929c0fd95 191 proportional = current_pos[4];
charwhit 6:c38929c0fd95 192
charwhit 6:c38929c0fd95 193 // compute the derivative
charwhit 6:c38929c0fd95 194 derivative = 0;
charwhit 6:c38929c0fd95 195 for (int i =1; i<5;++i) {
charwhit 6:c38929c0fd95 196 if (i ==1)
charwhit 6:c38929c0fd95 197 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 6:c38929c0fd95 198 else if (i == 2)
charwhit 6:c38929c0fd95 199 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 6:c38929c0fd95 200 else if (i==3)
charwhit 6:c38929c0fd95 201 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 6:c38929c0fd95 202 else
charwhit 6:c38929c0fd95 203 derivative += (current_pos[i] - current_pos[i-1]);
charwhit 6:c38929c0fd95 204 }
charwhit 6:c38929c0fd95 205
charwhit 6:c38929c0fd95 206 derivative = derivative; */
charwhit 6:c38929c0fd95 207
charwhit 6:c38929c0fd95 208
charwhit 6:c38929c0fd95 209 current_pos = robot.line_position();
charwhit 14:9bb4ac4968d2 210 if (fgets(sweepValue, 100, sweep) != NULL){
charwhit 14:9bb4ac4968d2 211
charwhit 14:9bb4ac4968d2 212 //f = 19.8/20 * t + .2;
charwhit 14:9bb4ac4968d2 213 //newSetPoint = A*sin(2*PI*f*t);
charwhit 14:9bb4ac4968d2 214 proportional = atof(sweepValue) + current_pos;
charwhit 13:0422156f83f6 215
charwhit 14:9bb4ac4968d2 216 }
charwhit 13:0422156f83f6 217 else{
charwhit 13:0422156f83f6 218 robot.printf("Try Again");
charwhit 13:0422156f83f6 219 break;
charwhit 14:9bb4ac4968d2 220 }
charwhit 13:0422156f83f6 221
charwhit 6:c38929c0fd95 222
charwhit 6:c38929c0fd95 223 derivative = current_pos - previous_pos;
charwhit 6:c38929c0fd95 224
charwhit 6:c38929c0fd95 225
charwhit 6:c38929c0fd95 226 //compute the integral
charwhit 6:c38929c0fd95 227 integral =+ proportional;
charwhit 6:c38929c0fd95 228
charwhit 6:c38929c0fd95 229 //remember the last position.
charwhit 6:c38929c0fd95 230 previous_pos = current_pos;
charwhit 6:c38929c0fd95 231
charwhit 6:c38929c0fd95 232 // compute the power
charwhit 6:c38929c0fd95 233 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 6:c38929c0fd95 234 //computer new speeds
charwhit 6:c38929c0fd95 235 right = speed+power;
charwhit 6:c38929c0fd95 236 left = speed-power;
charwhit 6:c38929c0fd95 237
charwhit 6:c38929c0fd95 238 //limit checks
charwhit 6:c38929c0fd95 239 if(right<MIN)
charwhit 6:c38929c0fd95 240 right = MIN;
charwhit 6:c38929c0fd95 241 else if (right > MAX)
charwhit 6:c38929c0fd95 242 right = MAX;
charwhit 6:c38929c0fd95 243
charwhit 6:c38929c0fd95 244 if(left<MIN)
charwhit 6:c38929c0fd95 245 left = MIN;
charwhit 6:c38929c0fd95 246 else if (left>MIN)
charwhit 6:c38929c0fd95 247 left = MAX;
charwhit 6:c38929c0fd95 248
charwhit 6:c38929c0fd95 249 //set speed
charwhit 6:c38929c0fd95 250
charwhit 6:c38929c0fd95 251 robot.left_motor(left);
charwhit 6:c38929c0fd95 252 robot.right_motor(right);
charwhit 9:bd2f012e2f57 253
charwhit 9:bd2f012e2f57 254
charwhit 10:030b7e4ff7be 255 //if (myFile.is_open())
charwhit 10:030b7e4ff7be 256 //{
charwhit 10:030b7e4ff7be 257 //btbee.printf("%f %f %f\n", left, right, robot.line_position());
charwhit 13:0422156f83f6 258 Uvalue.push_back(power);
charwhit 10:030b7e4ff7be 259 lineposval.push_back(robot.line_position());
charwhit 10:030b7e4ff7be 260 //}
charwhit 10:030b7e4ff7be 261
charwhit 13:0422156f83f6 262 //++import;
charwhit 13:0422156f83f6 263 t = t + .005;
charwhit 6:c38929c0fd95 264 wait((5-time_wait.read_ms())/1000);
bbabbs 0:17669460c6b1 265 }
charwhit 6:c38929c0fd95 266
charwhit 6:c38929c0fd95 267
charwhit 6:c38929c0fd95 268
charwhit 6:c38929c0fd95 269 robot.stop();
charwhit 6:c38929c0fd95 270
charwhit 6:c38929c0fd95 271 char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 6:c38929c0fd95 272 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 6:c38929c0fd95 273 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 6:c38929c0fd95 274 ,'G','8','E','8','D','8','C','4'
charwhit 6:c38929c0fd95 275 };
nbtavis 2:80a1ed62c307 276 int numb = 59;
charwhit 6:c38929c0fd95 277
nbtavis 2:80a1ed62c307 278 robot.playtune(hail,numb);
charwhit 6:c38929c0fd95 279
charwhit 6:c38929c0fd95 280
charwhit 6:c38929c0fd95 281
charwhit 6:c38929c0fd95 282
charwhit 6:c38929c0fd95 283 }