MEGACARAZ / Mbed 2 deprecated Turning

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   btbee m3pi_ng mbed

main.cpp@8:86d31f2a7479, 2015-06-04 (annotated)

Committer:
rzeer
Date:
Thu Jun 04 08:37:54 2015 +0000
Revision:
8:86d31f2a7479
Parent:
7:0b4f136366c6
Child:
9:723a95f7dbeb
modified bluetooth reading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
crmackey 0:f4302186f4cb 1 #include "mbed.h"
crmackey 0:f4302186f4cb 2 #include "m3pi_ng.h"
crmackey 0:f4302186f4cb 3 #include "time.h"
crmackey 0:f4302186f4cb 4 #include "btbee.h"
crmackey 5:0c2da538ada2 5 #include <vector>
crmackey 5:0c2da538ada2 6 #include <string>
crmackey 0:f4302186f4cb 7
crmackey 0:f4302186f4cb 8 m3pi m3pi;
crmackey 0:f4302186f4cb 9 btbee btbee;
crmackey 0:f4302186f4cb 10 DigitalIn m3pi_pb(p21);
crmackey 0:f4302186f4cb 11 DigitalIn m3pi_IN[] = {(p12)};
crmackey 0:f4302186f4cb 12 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
crmackey 0:f4302186f4cb 13 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
crmackey 0:f4302186f4cb 14
crmackey 0:f4302186f4cb 15 // Minimum and maximum motor speeds
crmackey 0:f4302186f4cb 16 #define MAX 0.5
crmackey 0:f4302186f4cb 17 #define MIN 0
crmackey 0:f4302186f4cb 18
crmackey 0:f4302186f4cb 19 // PID terms
crmackey 0:f4302186f4cb 20 #define P_TERM 1.5
crmackey 0:f4302186f4cb 21 #define I_TERM 0
crmackey 0:f4302186f4cb 22 #define D_TERM 25
rzeer 7:0b4f136366c6 23
rzeer 7:0b4f136366c6 24 //this will be inputed into the "read_all" function and will store the char* sent from python
rzeer 7:0b4f136366c6 25 char arr_input[30];
crmackey 0:f4302186f4cb 26
rzeer 7:0b4f136366c6 27 //this is the number of characters read in from python
rzeer 7:0b4f136366c6 28 int chars_read;
crmackey 0:f4302186f4cb 29
crmackey 0:f4302186f4cb 30 float right; // initializes variables
crmackey 0:f4302186f4cb 31 float left;
crmackey 0:f4302186f4cb 32 float current_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 33 float previous_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 34 float derivative,proportional,integral = 0;
crmackey 0:f4302186f4cb 35 float power;
crmackey 0:f4302186f4cb 36 float speed = MAX;
crmackey 0:f4302186f4cb 37 int LapTest[5];
crmackey 4:a0baf6752548 38 float s1, s2, s3, s4, s5;
crmackey 0:f4302186f4cb 39 Timer TurnTime;
crmackey 3:711ac018d3ec 40 Timer LapTime;
crmackey 4:a0baf6752548 41 int counter = 0;
crmackey 5:0c2da538ada2 42 string path;
crmackey 5:0c2da538ada2 43 char dir;
crmackey 5:0c2da538ada2 44
crmackey 0:f4302186f4cb 45
crmackey 0:f4302186f4cb 46 void RightTurn() {
crmackey 0:f4302186f4cb 47
crmackey 0:f4302186f4cb 48 m3pi.stop();
crmackey 0:f4302186f4cb 49 TurnTime.reset();
crmackey 0:f4302186f4cb 50 TurnTime.start();
crmackey 0:f4302186f4cb 51
crmackey 0:f4302186f4cb 52 while (1) {
crmackey 0:f4302186f4cb 53
crmackey 0:f4302186f4cb 54 m3pi.right_motor(0.45);
crmackey 0:f4302186f4cb 55 m3pi.left_motor(-0.1);
crmackey 0:f4302186f4cb 56
crmackey 0:f4302186f4cb 57 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 58
crmackey 5:0c2da538ada2 59 LapTime.reset();
crmackey 5:0c2da538ada2 60 LapTime.start();
crmackey 5:0c2da538ada2 61
crmackey 0:f4302186f4cb 62 return;
crmackey 0:f4302186f4cb 63
crmackey 0:f4302186f4cb 64 }
crmackey 0:f4302186f4cb 65
crmackey 0:f4302186f4cb 66 }
crmackey 0:f4302186f4cb 67
crmackey 0:f4302186f4cb 68 }
crmackey 0:f4302186f4cb 69
crmackey 0:f4302186f4cb 70
crmackey 0:f4302186f4cb 71 void LeftTurn() {
crmackey 0:f4302186f4cb 72
crmackey 0:f4302186f4cb 73 m3pi.stop();
crmackey 0:f4302186f4cb 74 TurnTime.reset();
crmackey 0:f4302186f4cb 75 TurnTime.start();
crmackey 0:f4302186f4cb 76
crmackey 0:f4302186f4cb 77 while (1) {
crmackey 0:f4302186f4cb 78
crmackey 0:f4302186f4cb 79 m3pi.left_motor(0.45);
crmackey 0:f4302186f4cb 80 m3pi.right_motor(-0.1);
crmackey 0:f4302186f4cb 81
crmackey 0:f4302186f4cb 82 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 83
crmackey 5:0c2da538ada2 84 LapTime.reset();
crmackey 5:0c2da538ada2 85 LapTime.start();
crmackey 5:0c2da538ada2 86
crmackey 0:f4302186f4cb 87 return;
crmackey 0:f4302186f4cb 88
crmackey 0:f4302186f4cb 89 }
crmackey 0:f4302186f4cb 90
crmackey 0:f4302186f4cb 91 }
crmackey 0:f4302186f4cb 92
crmackey 0:f4302186f4cb 93 }
crmackey 5:0c2da538ada2 94
crmackey 5:0c2da538ada2 95 void TurnAround() {
crmackey 5:0c2da538ada2 96
crmackey 5:0c2da538ada2 97 m3pi.stop();
crmackey 5:0c2da538ada2 98 TurnTime.reset();
crmackey 5:0c2da538ada2 99 TurnTime.start();
crmackey 5:0c2da538ada2 100
crmackey 5:0c2da538ada2 101 while (1) {
crmackey 5:0c2da538ada2 102
crmackey 5:0c2da538ada2 103 m3pi.left_motor(0.5);
crmackey 5:0c2da538ada2 104 m3pi.right_motor(-0.5);
crmackey 5:0c2da538ada2 105
crmackey 5:0c2da538ada2 106 if( TurnTime.read() > 0.225) {
crmackey 5:0c2da538ada2 107
crmackey 5:0c2da538ada2 108 m3pi.stop();
crmackey 5:0c2da538ada2 109
crmackey 5:0c2da538ada2 110 return;
crmackey 5:0c2da538ada2 111
crmackey 5:0c2da538ada2 112 }
crmackey 5:0c2da538ada2 113 }
crmackey 5:0c2da538ada2 114 }
crmackey 5:0c2da538ada2 115
crmackey 5:0c2da538ada2 116 void GoStraight() {
crmackey 5:0c2da538ada2 117
crmackey 5:0c2da538ada2 118 TurnTime.reset();
crmackey 5:0c2da538ada2 119 TurnTime.start();
crmackey 5:0c2da538ada2 120
crmackey 5:0c2da538ada2 121 while(1) {
crmackey 5:0c2da538ada2 122
crmackey 5:0c2da538ada2 123 m3pi.right_motor(0.5);
crmackey 5:0c2da538ada2 124 m3pi.left_motor(0.5);
crmackey 5:0c2da538ada2 125
crmackey 5:0c2da538ada2 126 if(TurnTime.read() > 0.02) {
crmackey 5:0c2da538ada2 127
crmackey 5:0c2da538ada2 128 LapTime.reset();
crmackey 5:0c2da538ada2 129 LapTime.start();
crmackey 5:0c2da538ada2 130
crmackey 5:0c2da538ada2 131 return;
crmackey 5:0c2da538ada2 132
crmackey 5:0c2da538ada2 133 }
crmackey 5:0c2da538ada2 134 }
crmackey 5:0c2da538ada2 135 }
crmackey 5:0c2da538ada2 136
crmackey 5:0c2da538ada2 137
crmackey 5:0c2da538ada2 138 bool CrossDetect() {
crmackey 5:0c2da538ada2 139
crmackey 5:0c2da538ada2 140 if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500) {
crmackey 5:0c2da538ada2 141
crmackey 5:0c2da538ada2 142 return(1);
crmackey 5:0c2da538ada2 143
crmackey 5:0c2da538ada2 144 }
crmackey 5:0c2da538ada2 145
crmackey 5:0c2da538ada2 146 else if(s1 > 700 and s2 > 700) {
crmackey 5:0c2da538ada2 147
crmackey 5:0c2da538ada2 148 return(1);
crmackey 5:0c2da538ada2 149
crmackey 5:0c2da538ada2 150 }
crmackey 5:0c2da538ada2 151
crmackey 5:0c2da538ada2 152 else if(s5 > 700 and s4 > 700) {
crmackey 5:0c2da538ada2 153
crmackey 5:0c2da538ada2 154 return(1);
crmackey 5:0c2da538ada2 155
crmackey 5:0c2da538ada2 156 }
crmackey 5:0c2da538ada2 157
crmackey 5:0c2da538ada2 158 else {
crmackey 5:0c2da538ada2 159
crmackey 5:0c2da538ada2 160 return(0);
crmackey 5:0c2da538ada2 161
crmackey 5:0c2da538ada2 162 }
crmackey 5:0c2da538ada2 163
crmackey 5:0c2da538ada2 164 }
crmackey 5:0c2da538ada2 165
crmackey 0:f4302186f4cb 166
crmackey 0:f4302186f4cb 167 int main() {
crmackey 5:0c2da538ada2 168
crmackey 5:0c2da538ada2 169 vector < string > place(6, "");
crmackey 5:0c2da538ada2 170 vector < vector < string > > rga(6,place);
crmackey 0:f4302186f4cb 171
crmackey 5:0c2da538ada2 172 rga.at(0).at(0) = "X";
crmackey 5:0c2da538ada2 173 rga.at(0).at(1) = "RLLRX";
crmackey 5:0c2da538ada2 174 rga.at(0).at(2) = "RLSSX";
crmackey 5:0c2da538ada2 175 rga.at(0).at(3) = "RLSRLX";
crmackey 5:0c2da538ada2 176 rga.at(0).at(4) = "RRLSLRRRLX";
crmackey 5:0c2da538ada2 177 rga.at(0).at(5) = "RRLRX";
crmackey 5:0c2da538ada2 178
crmackey 5:0c2da538ada2 179 rga.at(1).at(0) = "LRRLX";
crmackey 5:0c2da538ada2 180 rga.at(1).at(1) = "X";
crmackey 5:0c2da538ada2 181 rga.at(1).at(2) = "LLSX";
crmackey 5:0c2da538ada2 182 rga.at(1).at(3) = "LLRLX";
crmackey 5:0c2da538ada2 183 rga.at(1).at(4) = "LSRLRRLX";
crmackey 5:0c2da538ada2 184 rga.at(1).at(5) = "LRSLRX";
crmackey 5:0c2da538ada2 185
crmackey 5:0c2da538ada2 186 rga.at(2).at(0) = "SSRLX";
crmackey 5:0c2da538ada2 187 rga.at(2).at(1) = "SRRX";
crmackey 5:0c2da538ada2 188 rga.at(2).at(2) = "X";
crmackey 5:0c2da538ada2 189 rga.at(2).at(3) = "LLX";
crmackey 5:0c2da538ada2 190 rga.at(2).at(4) = "LRSRLX";
crmackey 5:0c2da538ada2 191 rga.at(2).at(5) = "SSSLRX";
crmackey 5:0c2da538ada2 192
crmackey 6:8bd154642fb4 193 rga.at(3).at(0) = "SRRLLRLRX";
crmackey 5:0c2da538ada2 194 rga.at(3).at(1) = "RLRRX";
crmackey 5:0c2da538ada2 195 rga.at(3).at(2) = "RRX";
crmackey 5:0c2da538ada2 196 rga.at(3).at(3) = "X";
crmackey 5:0c2da538ada2 197 rga.at(3).at(4) = "SSRLX";
crmackey 5:0c2da538ada2 198 rga.at(3).at(5) = "SRLRLX";
crmackey 5:0c2da538ada2 199
crmackey 5:0c2da538ada2 200 rga.at(4).at(0) = "RLLLRSRLLX";
crmackey 5:0c2da538ada2 201 rga.at(4).at(1) = "RLLRLSRX";
crmackey 5:0c2da538ada2 202 rga.at(4).at(2) = "RLLRLRSX";
crmackey 5:0c2da538ada2 203 rga.at(4).at(3) = "RLSSX";
crmackey 5:0c2da538ada2 204 rga.at(4).at(4) = "X";
crmackey 5:0c2da538ada2 205 rga.at(4).at(5) = "RLLLRLX";
crmackey 5:0c2da538ada2 206
crmackey 5:0c2da538ada2 207 rga.at(5).at(0) = "LRLLX";
crmackey 5:0c2da538ada2 208 rga.at(5).at(1) = "LSLX";
crmackey 5:0c2da538ada2 209 rga.at(5).at(2) = "LRSSSX";
crmackey 5:0c2da538ada2 210 rga.at(5).at(3) = "RLRLSX";
crmackey 5:0c2da538ada2 211 rga.at(5).at(4) = "RLRRRLX";
crmackey 5:0c2da538ada2 212 rga.at(5).at(5) = "X";
crmackey 5:0c2da538ada2 213
crmackey 5:0c2da538ada2 214 btbee.reset();
crmackey 0:f4302186f4cb 215 m3pi_pb.mode(PullUp);
crmackey 0:f4302186f4cb 216
crmackey 0:f4302186f4cb 217 m3pi.printf("Wait 4");
crmackey 0:f4302186f4cb 218 m3pi.locate(0,1);
crmackey 0:f4302186f4cb 219 m3pi.printf("PC");
crmackey 0:f4302186f4cb 220
crmackey 0:f4302186f4cb 221 while(m3pi_pb) { // Loop that runs until user presses button after bluetooth is connected.
crmackey 0:f4302186f4cb 222 m3pi_led[0]=!m3pi_led[0];
crmackey 0:f4302186f4cb 223 wait(3);
crmackey 0:f4302186f4cb 224 btbee.printf("\n");
crmackey 0:f4302186f4cb 225 btbee.printf("PC connected. Press the button \n");
crmackey 0:f4302186f4cb 226 }
crmackey 0:f4302186f4cb 227
crmackey 0:f4302186f4cb 228 m3pi.cls();
crmackey 0:f4302186f4cb 229
crmackey 3:711ac018d3ec 230 // reads battery voltage to screen on start up
crmackey 0:f4302186f4cb 231 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:f4302186f4cb 232 m3pi.print(Bat,7);
crmackey 0:f4302186f4cb 233 wait(0.75);
crmackey 0:f4302186f4cb 234 m3pi.cls();
crmackey 0:f4302186f4cb 235 float batteryvoltage = m3pi.battery();
crmackey 0:f4302186f4cb 236 char* str = new char[30];
crmackey 0:f4302186f4cb 237 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:f4302186f4cb 238 m3pi.print(str,6);
crmackey 0:f4302186f4cb 239 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:f4302186f4cb 240 wait(0.75);
crmackey 0:f4302186f4cb 241 m3pi.cls();
crmackey 0:f4302186f4cb 242
crmackey 0:f4302186f4cb 243 if (batteryvoltage < 4.2) // exits program if voltage is less than 4.2 V
crmackey 0:f4302186f4cb 244 {
crmackey 0:f4302186f4cb 245 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:f4302186f4cb 246 m3pi.print(low,7);
crmackey 0:f4302186f4cb 247 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:f4302186f4cb 248 m3pi.playtune(ExitSound,11);
crmackey 0:f4302186f4cb 249 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:f4302186f4cb 250 exit(1);
crmackey 0:f4302186f4cb 251 }
crmackey 0:f4302186f4cb 252
crmackey 0:f4302186f4cb 253 btbee.printf("Now calibrating \n"); // calibrating robot on the line.
crmackey 0:f4302186f4cb 254
crmackey 0:f4302186f4cb 255 m3pi.sensor_auto_calibrate();
crmackey 0:f4302186f4cb 256
crmackey 0:f4302186f4cb 257 btbee.printf("Finished calibrating \n");
crmackey 0:f4302186f4cb 258
crmackey 0:f4302186f4cb 259
crmackey 0:f4302186f4cb 260 btbee.printf("\n");
crmackey 0:f4302186f4cb 261 btbee.printf("Now starting \n");
crmackey 0:f4302186f4cb 262
crmackey 3:711ac018d3ec 263 LapTime.start();
crmackey 3:711ac018d3ec 264
rzeer 7:0b4f136366c6 265 // path = rga.at(0).at(3);
crmackey 5:0c2da538ada2 266
crmackey 0:f4302186f4cb 267
crmackey 0:f4302186f4cb 268 while (1) {
crmackey 0:f4302186f4cb 269
rzeer 7:0b4f136366c6 270
rzeer 7:0b4f136366c6 271
rzeer 7:0b4f136366c6 272 if(btbee.readable()){
rzeer 7:0b4f136366c6 273 //read input from python
rzeer 7:0b4f136366c6 274 btbee.read_all(arr_input, 30, chars_read);
rzeer 8:86d31f2a7479 275 //we need to go through each point and conduct a path to the next point.
rzeer 8:86d31f2a7479 276
rzeer 7:0b4f136366c6 277 char startPoint = arr_input[0]; //this will get us the first character of the string
rzeer 7:0b4f136366c6 278 char nextPoint = arr_input[1];
rzeer 7:0b4f136366c6 279 //now we will remove the first char from the input string so that we do not read it twice
rzeer 7:0b4f136366c6 280 //we do this by incrementing the string pointer
rzeer 7:0b4f136366c6 281 arr_input++;
rzeer 7:0b4f136366c6 282
rzeer 7:0b4f136366c6 283
rzeer 7:0b4f136366c6 284 //this stores the chars into variables so its easier to deal with
rzeer 7:0b4f136366c6 285 int x = atoi(startPoint);
rzeer 7:0b4f136366c6 286 int y = atoi(nextPoint);
rzeer 7:0b4f136366c6 287 //put this into the array
rzeer 7:0b4f136366c6 288 path = rga.at(x).at(y);
rzeer 7:0b4f136366c6 289
rzeer 7:0b4f136366c6 290 chars_read--;
rzeer 8:86d31f2a7479 291
rzeer 7:0b4f136366c6 292 }
rzeer 8:86d31f2a7479 293
rzeer 8:86d31f2a7479 294
rzeer 7:0b4f136366c6 295
rzeer 7:0b4f136366c6 296
rzeer 7:0b4f136366c6 297
crmackey 5:0c2da538ada2 298 LapTime.start();
crmackey 0:f4302186f4cb 299
crmackey 0:f4302186f4cb 300 // Get the position of the line.
crmackey 0:f4302186f4cb 301 current_pos_of_line = m3pi.line_position();
crmackey 0:f4302186f4cb 302 proportional = current_pos_of_line;
crmackey 0:f4302186f4cb 303
crmackey 0:f4302186f4cb 304 // Compute the derivative
crmackey 0:f4302186f4cb 305 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:f4302186f4cb 306
crmackey 0:f4302186f4cb 307 // Compute the integral
crmackey 0:f4302186f4cb 308 integral += proportional;
crmackey 0:f4302186f4cb 309
crmackey 0:f4302186f4cb 310 // Remember the last position.
crmackey 0:f4302186f4cb 311 previous_pos_of_line = current_pos_of_line;
crmackey 0:f4302186f4cb 312
crmackey 0:f4302186f4cb 313 // Compute the power
crmackey 0:f4302186f4cb 314 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:f4302186f4cb 315
crmackey 0:f4302186f4cb 316 // Compute new speeds
crmackey 0:f4302186f4cb 317 right = speed+power;
crmackey 0:f4302186f4cb 318 left = speed-power;
crmackey 0:f4302186f4cb 319
crmackey 0:f4302186f4cb 320 // limit checks
crmackey 0:f4302186f4cb 321 if (right < MIN)
crmackey 0:f4302186f4cb 322 right = MIN;
crmackey 0:f4302186f4cb 323 else if (right > MAX)
crmackey 0:f4302186f4cb 324 right = MAX;
crmackey 0:f4302186f4cb 325
crmackey 0:f4302186f4cb 326 if (left < MIN)
crmackey 0:f4302186f4cb 327 left = MIN;
crmackey 0:f4302186f4cb 328 else if (left > MAX)
crmackey 0:f4302186f4cb 329 left = MAX;
crmackey 0:f4302186f4cb 330
crmackey 0:f4302186f4cb 331 // set speed
crmackey 0:f4302186f4cb 332 m3pi.left_motor(left);
crmackey 0:f4302186f4cb 333 m3pi.right_motor(right);
crmackey 0:f4302186f4cb 334
crmackey 0:f4302186f4cb 335
crmackey 0:f4302186f4cb 336 m3pi.calibrated_sensor(LapTest);
crmackey 0:f4302186f4cb 337
crmackey 0:f4302186f4cb 338 s1 = LapTest[0];
crmackey 0:f4302186f4cb 339 s2 = LapTest[1];
crmackey 0:f4302186f4cb 340 s3 = LapTest[2];
crmackey 0:f4302186f4cb 341 s4 = LapTest[3];
crmackey 0:f4302186f4cb 342 s5 = LapTest[4];
crmackey 0:f4302186f4cb 343
crmackey 4:a0baf6752548 344 //if(LapTime.read() > 1.0) {
crmackey 4:a0baf6752548 345 //
crmackey 4:a0baf6752548 346 // btbee.printf("Sensor 1: %f\n", s1);
crmackey 4:a0baf6752548 347 // btbee.printf("Sensor 2: %f\n", s2);
crmackey 4:a0baf6752548 348 // btbee.printf("Sensor 3: %f\n", s3);
crmackey 4:a0baf6752548 349 // btbee.printf("Sensor 4: %f\n", s4);
crmackey 4:a0baf6752548 350 // btbee.printf("Sensor 5: %f\n", s5);
crmackey 4:a0baf6752548 351 // btbee.printf("\n");
crmackey 4:a0baf6752548 352 // LapTime.reset();
crmackey 4:a0baf6752548 353 // LapTime.start();
crmackey 4:a0baf6752548 354 //
crmackey 4:a0baf6752548 355 // }
crmackey 4:a0baf6752548 356
crmackey 5:0c2da538ada2 357
crmackey 5:0c2da538ada2 358 if(CrossDetect() and LapTime > 0.1) {
crmackey 5:0c2da538ada2 359
crmackey 5:0c2da538ada2 360 btbee.printf("\n");
crmackey 5:0c2da538ada2 361 dir = path.at(counter);
crmackey 5:0c2da538ada2 362 btbee.printf("%c\n", dir);
crmackey 5:0c2da538ada2 363
crmackey 5:0c2da538ada2 364 switch(dir) {
crmackey 4:a0baf6752548 365
crmackey 5:0c2da538ada2 366 case 'R': {
crmackey 5:0c2da538ada2 367
crmackey 5:0c2da538ada2 368 RightTurn();
crmackey 5:0c2da538ada2 369 btbee.printf("Turning right\n");
crmackey 5:0c2da538ada2 370 break;
crmackey 5:0c2da538ada2 371
crmackey 5:0c2da538ada2 372 }
crmackey 5:0c2da538ada2 373
crmackey 5:0c2da538ada2 374 case 'L': {
crmackey 5:0c2da538ada2 375
crmackey 5:0c2da538ada2 376 LeftTurn();
crmackey 5:0c2da538ada2 377 btbee.printf("Turning left\n");
crmackey 5:0c2da538ada2 378 break;
crmackey 5:0c2da538ada2 379
crmackey 5:0c2da538ada2 380 }
crmackey 5:0c2da538ada2 381
crmackey 5:0c2da538ada2 382 case 'S': {
crmackey 5:0c2da538ada2 383
crmackey 5:0c2da538ada2 384 GoStraight();
crmackey 5:0c2da538ada2 385 btbee.printf("Going straight\n");
crmackey 5:0c2da538ada2 386 break;
crmackey 5:0c2da538ada2 387
crmackey 5:0c2da538ada2 388 }
crmackey 5:0c2da538ada2 389
crmackey 5:0c2da538ada2 390 case 'X': {
crmackey 5:0c2da538ada2 391
crmackey 5:0c2da538ada2 392 btbee.printf("Reached goal. Turning around\n");
crmackey 5:0c2da538ada2 393 TurnAround();
rzeer 8:86d31f2a7479 394 //we need to go through each point and conduct a path to the next point.
rzeer 8:86d31f2a7479 395 if(chars_read){
rzeer 8:86d31f2a7479 396 char startPoint = arr_input[0]; //this will get us the first character of the string
rzeer 8:86d31f2a7479 397 char nextPoint = arr_input[1];
rzeer 8:86d31f2a7479 398 //now we will remove the first char from the input string so that we do not read it twice
rzeer 8:86d31f2a7479 399 //we do this by incrementing the string pointer
rzeer 8:86d31f2a7479 400 arr_input++;
rzeer 8:86d31f2a7479 401
rzeer 8:86d31f2a7479 402
rzeer 8:86d31f2a7479 403 //this stores the chars into variables so its easier to deal with
rzeer 8:86d31f2a7479 404 int x = atoi(startPoint);
rzeer 8:86d31f2a7479 405 int y = atoi(nextPoint);
rzeer 8:86d31f2a7479 406 //put this into the array
rzeer 8:86d31f2a7479 407 path = rga.at(x).at(y);
rzeer 8:86d31f2a7479 408
rzeer 8:86d31f2a7479 409 chars_read--;
rzeer 8:86d31f2a7479 410 }
rzeer 8:86d31f2a7479 411 // exit(1);
crmackey 5:0c2da538ada2 412
rzeer 8:86d31f2a7479 413 }
crmackey 5:0c2da538ada2 414
crmackey 5:0c2da538ada2 415 }
crmackey 5:0c2da538ada2 416
crmackey 5:0c2da538ada2 417 counter++;
crmackey 4:a0baf6752548 418
crmackey 4:a0baf6752548 419 }
crmackey 5:0c2da538ada2 420
crmackey 0:f4302186f4cb 421
crmackey 0:f4302186f4cb 422
crmackey 0:f4302186f4cb 423 }
crmackey 0:f4302186f4cb 424
crmackey 5:0c2da538ada2 425 }