Nick Tavis / Robot

basic code

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@11:83dfbc5e93ed, 2015-05-29 (annotated)

Committer:
charwhit
Date:
Fri May 29 11:55:50 2015 +0000
Revision:
11:83dfbc5e93ed
Parent:
10:030b7e4ff7be
Child:
12:bb90691e5166
Finally got USB to work

Who changed what in which revision?

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