Nick Tavis / Robot

basic code

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@9:bd2f012e2f57, 2015-05-28 (annotated)

Committer:
charwhit
Date:
Thu May 28 13:40:16 2015 +0000
Revision:
9:bd2f012e2f57
Parent:
8:7d491b51665e
Child:
10:030b7e4ff7be
Newest code before Closed Loop ID

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