James Heavey / Mbed 2 deprecated 3875_DISSERTATION

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:   mbed 3875_Individualproject

main/main.cpp@17:77b8515a9568, 2020-03-11 (annotated)

Committer:
jamesheavey
Date:
Wed Mar 11 11:03:59 2020 +0000
Revision:
17:77b8515a9568
Parent:
16:96c7dc8a1119
Child:
18:991658b628fc
sensors broke;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamesheavey 0:df5216b20861 1 #include "main.h"
jamesheavey 0:df5216b20861 2
jamesheavey 0:df5216b20861 3 // API
jamesheavey 0:df5216b20861 4 m3pi robot;
jamesheavey 0:df5216b20861 5
jamesheavey 0:df5216b20861 6 // LEDs
jamesheavey 0:df5216b20861 7 BusOut leds(LED4,LED3,LED2,LED1);
jamesheavey 0:df5216b20861 8
jamesheavey 0:df5216b20861 9 // Buttons
jamesheavey 0:df5216b20861 10 DigitalIn button_A(p18);
jamesheavey 0:df5216b20861 11 DigitalIn button_B(p17);
jamesheavey 0:df5216b20861 12 DigitalIn button_X(p21);
jamesheavey 0:df5216b20861 13 DigitalIn button_Y(p22);
jamesheavey 0:df5216b20861 14 DigitalIn button_enter(p24);
jamesheavey 0:df5216b20861 15 DigitalIn button_back(p23);
jamesheavey 0:df5216b20861 16
jamesheavey 0:df5216b20861 17 // Potentiometers
jamesheavey 0:df5216b20861 18 AnalogIn pot_P(p15);
jamesheavey 0:df5216b20861 19 AnalogIn pot_I(p16);
jamesheavey 0:df5216b20861 20 AnalogIn pot_D(p19);
jamesheavey 0:df5216b20861 21 AnalogIn pot_S(p20);
jamesheavey 0:df5216b20861 22
jamesheavey 17:77b8515a9568 23 // Sensors
jamesheavey 17:77b8515a9568 24 QTRSensorsRC encoders;
jamesheavey 17:77b8515a9568 25
jamesheavey 0:df5216b20861 26 // Prototypes
jamesheavey 0:df5216b20861 27 void init();
jamesheavey 0:df5216b20861 28 void calibrate();
jamesheavey 10:691c02b352cb 29 void follow_line();
jamesheavey 14:87052bb35211 30 bool junction_detect();
jamesheavey 14:87052bb35211 31 char junction_logic();
jamesheavey 14:87052bb35211 32 void turn_select( char turn );
jamesheavey 0:df5216b20861 33 void left();
jamesheavey 1:79219d0a33c8 34 void right();
jamesheavey 0:df5216b20861 35 void back();
jamesheavey 2:940e46e21353 36 void goal();
jamesheavey 2:940e46e21353 37 void simplify();
jamesheavey 10:691c02b352cb 38 void invert_path();
jamesheavey 0:df5216b20861 39
jamesheavey 0:df5216b20861 40 // Constants
jamesheavey 0:df5216b20861 41
jamesheavey 0:df5216b20861 42 const float A = 0.5; // 20
jamesheavey 0:df5216b20861 43 const float B = 1/10000; // 10000
jamesheavey 4:38c29dbc5953 44 const float C = 1.5; // 2/3
jamesheavey 0:df5216b20861 45
jamesheavey 5:ae417756235a 46 const int sens_thresh = 500; // replace the hard coded bits
jamesheavey 4:38c29dbc5953 47 const int turn_speed = 0.2;
jamesheavey 2:940e46e21353 48
jamesheavey 2:940e46e21353 49 // Global Variables
jamesheavey 2:940e46e21353 50
jamesheavey 2:940e46e21353 51 char path[100];
jamesheavey 10:691c02b352cb 52 char inv_path[100];
jamesheavey 2:940e46e21353 53 int path_length = 0;
jamesheavey 2:940e46e21353 54 unsigned int *sensor;
jamesheavey 17:77b8515a9568 55 unsigned int enc_vals[2];
jamesheavey 3:a5e06482462e 56 float speed;
jamesheavey 10:691c02b352cb 57 float proportional = 0.0;
jamesheavey 10:691c02b352cb 58 float prev_proportional = 0.0;
jamesheavey 10:691c02b352cb 59 float integral = 0.0;
jamesheavey 10:691c02b352cb 60 float derivative = 0.0;
jamesheavey 2:940e46e21353 61
jamesheavey 2:940e46e21353 62 // Main
jamesheavey 2:940e46e21353 63
jamesheavey 0:df5216b20861 64 int main()
jamesheavey 0:df5216b20861 65 {
jamesheavey 0:df5216b20861 66 init();
jamesheavey 0:df5216b20861 67 calibrate();
jamesheavey 15:6c461501d12d 68
jamesheavey 12:d80399686f32 69 speed = (pot_S*0.3)+0.2; // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
jamesheavey 0:df5216b20861 70
jamesheavey 0:df5216b20861 71 float dt = 1/50; // updating 50 times a second
jamesheavey 0:df5216b20861 72
jamesheavey 0:df5216b20861 73 while (1) {
jamesheavey 17:77b8515a9568 74
jamesheavey 17:77b8515a9568 75 // encoders.read(enc_vals);
jamesheavey 17:77b8515a9568 76 // robot.lcd_clear();
jamesheavey 17:77b8515a9568 77 //
jamesheavey 17:77b8515a9568 78 // char buffer[10];
jamesheavey 17:77b8515a9568 79 // sprintf(buffer,"%d",enc_vals[1]);
jamesheavey 17:77b8515a9568 80 // robot.lcd_goto_xy(0,0);
jamesheavey 17:77b8515a9568 81 // robot.lcd_print("buffer",10);
jamesheavey 17:77b8515a9568 82 //
jamesheavey 17:77b8515a9568 83 // if (enc_vals[1] > 500) {
jamesheavey 17:77b8515a9568 84 // leds = 0b0011;
jamesheavey 17:77b8515a9568 85 // }else if (enc_vals[1] < 500){
jamesheavey 17:77b8515a9568 86 // leds = 0b1100;
jamesheavey 17:77b8515a9568 87 // }else{
jamesheavey 17:77b8515a9568 88 // leds = 0b0000;
jamesheavey 17:77b8515a9568 89 // }
jamesheavey 17:77b8515a9568 90
jamesheavey 10:691c02b352cb 91 follow_line();
jamesheavey 13:bd271266e161 92
jamesheavey 14:87052bb35211 93 if ( junction_detect() ) {
jamesheavey 14:87052bb35211 94 char turn = junction_logic();
jamesheavey 14:87052bb35211 95 turn_select(turn);
jamesheavey 14:87052bb35211 96
jamesheavey 10:691c02b352cb 97 path[path_length] = turn;
jamesheavey 10:691c02b352cb 98 path_length ++;
jamesheavey 0:df5216b20861 99 }
jamesheavey 14:87052bb35211 100
jamesheavey 0:df5216b20861 101
jamesheavey 4:38c29dbc5953 102 simplify();
jamesheavey 4:38c29dbc5953 103
jamesheavey 4:38c29dbc5953 104 robot.lcd_clear();
jamesheavey 4:38c29dbc5953 105 robot.lcd_print(path,100);
jamesheavey 4:38c29dbc5953 106
jamesheavey 4:38c29dbc5953 107 //robot.display_data();
jamesheavey 0:df5216b20861 108
jamesheavey 0:df5216b20861 109 wait(dt);
jamesheavey 0:df5216b20861 110 }
jamesheavey 0:df5216b20861 111 }
jamesheavey 0:df5216b20861 112
jamesheavey 0:df5216b20861 113 void init()
jamesheavey 0:df5216b20861 114 {
jamesheavey 0:df5216b20861 115 robot.init();
jamesheavey 17:77b8515a9568 116
jamesheavey 17:77b8515a9568 117 PinName pins[2] = {p25,p26}; // choose digital pins that are free (maybe steal P and I pot pins 15&16) try 25 and 26 first
jamesheavey 17:77b8515a9568 118 encoders.init(pins,2);
jamesheavey 0:df5216b20861 119
jamesheavey 0:df5216b20861 120 button_A.mode(PullUp);
jamesheavey 0:df5216b20861 121 button_B.mode(PullUp);
jamesheavey 0:df5216b20861 122 button_X.mode(PullUp);
jamesheavey 0:df5216b20861 123 button_Y.mode(PullUp);
jamesheavey 0:df5216b20861 124 button_enter.mode(PullUp);
jamesheavey 0:df5216b20861 125 button_back.mode(PullUp);
jamesheavey 0:df5216b20861 126
jamesheavey 10:691c02b352cb 127 leds = 0b0000;
jamesheavey 0:df5216b20861 128 }
jamesheavey 0:df5216b20861 129
jamesheavey 0:df5216b20861 130 void calibrate()
jamesheavey 0:df5216b20861 131 {
jamesheavey 0:df5216b20861 132 leds = 0b1111;
jamesheavey 0:df5216b20861 133 robot.reset_calibration();
jamesheavey 0:df5216b20861 134
jamesheavey 15:6c461501d12d 135 while (button_enter.read() == 1) {} // wait for enter to be pressed
jamesheavey 15:6c461501d12d 136
jamesheavey 15:6c461501d12d 137 wait(2.0);
jamesheavey 0:df5216b20861 138
jamesheavey 15:6c461501d12d 139 robot.auto_calibrate();
jamesheavey 17:77b8515a9568 140
jamesheavey 0:df5216b20861 141
jamesheavey 15:6c461501d12d 142 robot.stop();
jamesheavey 15:6c461501d12d 143 wait(0.05);
jamesheavey 15:6c461501d12d 144 robot.scan();
jamesheavey 17:77b8515a9568 145
jamesheavey 17:77b8515a9568 146 encoders.calibrate();
jamesheavey 17:77b8515a9568 147
jamesheavey 17:77b8515a9568 148 leds = 0b0000;
jamesheavey 0:df5216b20861 149 }
jamesheavey 0:df5216b20861 150
jamesheavey 10:691c02b352cb 151 void follow_line()
jamesheavey 0:df5216b20861 152 {
jamesheavey 10:691c02b352cb 153 robot.scan();
jamesheavey 10:691c02b352cb 154 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 155
jamesheavey 15:6c461501d12d 156 leds = 0b0110;
jamesheavey 15:6c461501d12d 157
jamesheavey 10:691c02b352cb 158 proportional = robot.read_line(); // returns a value between -1,1 (-1 = PC0 or further , -1 to -0.5 = PC1 (-0.5 is directly below PC1) , -0.5 to 0 = PC2 , 0 to 0.5 = PC3 , 0.5 to 1 and further = PC4)
jamesheavey 10:691c02b352cb 159 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 160 integral += proportional;
jamesheavey 10:691c02b352cb 161 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 162
jamesheavey 10:691c02b352cb 163 // calculate motor correction
jamesheavey 10:691c02b352cb 164 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 165
jamesheavey 10:691c02b352cb 166 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 167 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 168 motor_correction = speed;
jamesheavey 10:691c02b352cb 169 }
jamesheavey 10:691c02b352cb 170 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 171 motor_correction = -speed;
jamesheavey 10:691c02b352cb 172 }
jamesheavey 0:df5216b20861 173
jamesheavey 10:691c02b352cb 174 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 175 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 176 } else {
jamesheavey 10:691c02b352cb 177 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 178 }
jamesheavey 1:79219d0a33c8 179 }
jamesheavey 0:df5216b20861 180
jamesheavey 14:87052bb35211 181 bool junction_detect()
jamesheavey 6:c10b367747a0 182 {
jamesheavey 14:87052bb35211 183 if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
jamesheavey 14:87052bb35211 184 return true;
jamesheavey 14:87052bb35211 185 } else if ( sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
jamesheavey 14:87052bb35211 186 return true;
jamesheavey 14:87052bb35211 187 } else {
jamesheavey 14:87052bb35211 188 return false;
jamesheavey 14:87052bb35211 189 }
jamesheavey 14:87052bb35211 190 }
jamesheavey 14:87052bb35211 191
jamesheavey 14:87052bb35211 192 char junction_logic()
jamesheavey 14:87052bb35211 193 {
jamesheavey 14:87052bb35211 194 bool straight = false;
jamesheavey 7:7fefd782532d 195 bool left = false;
jamesheavey 6:c10b367747a0 196 bool right = false;
jamesheavey 9:952586accbf9 197 bool goal = false;
jamesheavey 7:7fefd782532d 198
jamesheavey 7:7fefd782532d 199 if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
jamesheavey 13:bd271266e161 200 while ( (sensor[0] > sens_thresh || sensor[4] > sens_thresh) && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
jamesheavey 13:bd271266e161 201 robot.forward(speed);
jamesheavey 9:952586accbf9 202 robot.scan();
jamesheavey 13:bd271266e161 203 if ( sensor[0] > sens_thresh ) { left = true; }
jamesheavey 13:bd271266e161 204 if ( sensor[4] > sens_thresh ) { right = true; }
jamesheavey 13:bd271266e161 205 }
jamesheavey 7:7fefd782532d 206
jamesheavey 13:bd271266e161 207 if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
jamesheavey 15:6c461501d12d 208 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 209 robot.scan();
jamesheavey 12:d80399686f32 210 if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
jamesheavey 13:bd271266e161 211 goal = true;
jamesheavey 12:d80399686f32 212 }
jamesheavey 13:bd271266e161 213 }
jamesheavey 14:87052bb35211 214
jamesheavey 14:87052bb35211 215 robot.scan();
jamesheavey 14:87052bb35211 216
jamesheavey 14:87052bb35211 217 if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
jamesheavey 14:87052bb35211 218 straight = true;
jamesheavey 14:87052bb35211 219 }
jamesheavey 14:87052bb35211 220
jamesheavey 9:952586accbf9 221 } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
jamesheavey 9:952586accbf9 222 return 'B';
jamesheavey 6:c10b367747a0 223 }
jamesheavey 7:7fefd782532d 224
jamesheavey 9:952586accbf9 225 if (goal) {
jamesheavey 9:952586accbf9 226 return 'G';
jamesheavey 9:952586accbf9 227 } else if (left) {
jamesheavey 9:952586accbf9 228 return 'L';
jamesheavey 14:87052bb35211 229 } else if (straight) {
jamesheavey 14:87052bb35211 230 return 'S';
jamesheavey 9:952586accbf9 231 } else if (right) {
jamesheavey 9:952586accbf9 232 return 'R';
jamesheavey 6:c10b367747a0 233 } else {
jamesheavey 9:952586accbf9 234 return 'S';
jamesheavey 9:952586accbf9 235 }
jamesheavey 6:c10b367747a0 236 }
jamesheavey 6:c10b367747a0 237
jamesheavey 0:df5216b20861 238
jamesheavey 14:87052bb35211 239 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 240 {
jamesheavey 1:79219d0a33c8 241 switch(turn) {
jamesheavey 5:ae417756235a 242 case 'G':
jamesheavey 5:ae417756235a 243 goal();
jamesheavey 1:79219d0a33c8 244 case 'L':
jamesheavey 1:79219d0a33c8 245 left();
jamesheavey 5:ae417756235a 246 break;
jamesheavey 1:79219d0a33c8 247 case 'S':
jamesheavey 1:79219d0a33c8 248 break;
jamesheavey 1:79219d0a33c8 249 case 'R':
jamesheavey 1:79219d0a33c8 250 right();
jamesheavey 5:ae417756235a 251 break;
jamesheavey 1:79219d0a33c8 252 case 'B':
jamesheavey 1:79219d0a33c8 253 back();
jamesheavey 5:ae417756235a 254 break;
jamesheavey 1:79219d0a33c8 255 }
jamesheavey 1:79219d0a33c8 256 }
jamesheavey 1:79219d0a33c8 257
jamesheavey 0:df5216b20861 258 void left()
jamesheavey 0:df5216b20861 259 {
jamesheavey 1:79219d0a33c8 260 leds = 0b1100;
jamesheavey 3:a5e06482462e 261
jamesheavey 3:a5e06482462e 262 while (sensor[0] > 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 263
jamesheavey 14:87052bb35211 264 robot.spin_left(0.2);
jamesheavey 3:a5e06482462e 265 wait(0.1);
jamesheavey 3:a5e06482462e 266
jamesheavey 3:a5e06482462e 267 while (sensor[1] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 268
jamesheavey 3:a5e06482462e 269 while (sensor[1] > 500) { robot.scan(); }
jamesheavey 1:79219d0a33c8 270 }
jamesheavey 1:79219d0a33c8 271
jamesheavey 1:79219d0a33c8 272 void right()
jamesheavey 1:79219d0a33c8 273 {
jamesheavey 1:79219d0a33c8 274 leds = 0b0011;
jamesheavey 5:ae417756235a 275
jamesheavey 3:a5e06482462e 276 while (sensor[4] > 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 277
jamesheavey 14:87052bb35211 278 robot.spin_right(0.2);
jamesheavey 3:a5e06482462e 279 wait(0.1);
jamesheavey 3:a5e06482462e 280
jamesheavey 3:a5e06482462e 281 while (sensor[3] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 282
jamesheavey 3:a5e06482462e 283 while (sensor[3] > 500) { robot.scan(); }
jamesheavey 0:df5216b20861 284 }
jamesheavey 0:df5216b20861 285
jamesheavey 0:df5216b20861 286 void back()
jamesheavey 0:df5216b20861 287 {
jamesheavey 1:79219d0a33c8 288 leds = 0b1111;
jamesheavey 5:ae417756235a 289 robot.reverse(speed);
jamesheavey 5:ae417756235a 290 wait(0.1);
jamesheavey 14:87052bb35211 291 robot.spin_right(0.2);
jamesheavey 3:a5e06482462e 292
jamesheavey 3:a5e06482462e 293 while (sensor[3] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 294
jamesheavey 3:a5e06482462e 295 while (sensor[3] > 500) { robot.scan(); }
jamesheavey 0:df5216b20861 296 }
jamesheavey 1:79219d0a33c8 297
jamesheavey 2:940e46e21353 298 void simplify()
jamesheavey 1:79219d0a33c8 299 {
jamesheavey 2:940e46e21353 300 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 301 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 302 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 303
jamesheavey 10:691c02b352cb 304 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 305 int angle_change = 0;
jamesheavey 2:940e46e21353 306
jamesheavey 10:691c02b352cb 307 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 308 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 309 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 310 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 311 }
jamesheavey 4:38c29dbc5953 312
jamesheavey 4:38c29dbc5953 313 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 314
jamesheavey 4:38c29dbc5953 315 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 316 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 317 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 318 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 319
jamesheavey 4:38c29dbc5953 320 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 321
jamesheavey 4:38c29dbc5953 322 path_length -= 2;
jamesheavey 2:940e46e21353 323 }
jamesheavey 5:ae417756235a 324 }
jamesheavey 5:ae417756235a 325
jamesheavey 10:691c02b352cb 326 void goal()
jamesheavey 10:691c02b352cb 327 {
jamesheavey 10:691c02b352cb 328 invert_path();
jamesheavey 11:c3299aca7d8f 329
jamesheavey 11:c3299aca7d8f 330 leds = 0b0000;
jamesheavey 10:691c02b352cb 331
jamesheavey 10:691c02b352cb 332 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 333 //robot.lcd_print(path,100);
jamesheavey 16:96c7dc8a1119 334 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 335
jamesheavey 11:c3299aca7d8f 336 while(1) {
jamesheavey 15:6c461501d12d 337 int pointer = 0;
jamesheavey 15:6c461501d12d 338
jamesheavey 15:6c461501d12d 339 robot.stop();
jamesheavey 15:6c461501d12d 340
jamesheavey 15:6c461501d12d 341 leds = 0b1001;
jamesheavey 15:6c461501d12d 342 wait(0.2);
jamesheavey 15:6c461501d12d 343 leds = 0b0110;
jamesheavey 15:6c461501d12d 344 wait(0.2);
jamesheavey 15:6c461501d12d 345
jamesheavey 15:6c461501d12d 346 robot.reverse(speed);
jamesheavey 15:6c461501d12d 347 while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 348 wait(0.05);
jamesheavey 15:6c461501d12d 349
jamesheavey 15:6c461501d12d 350 robot.spin_right(0.2);
jamesheavey 15:6c461501d12d 351 while(sensor[2] > sens_thresh) { robot.scan(); }
jamesheavey 15:6c461501d12d 352 while(sensor[3] < sens_thresh) { robot.scan(); }
jamesheavey 15:6c461501d12d 353 while(sensor[3] > sens_thresh) { robot.scan(); }
jamesheavey 15:6c461501d12d 354
jamesheavey 15:6c461501d12d 355 robot.stop();
jamesheavey 15:6c461501d12d 356
jamesheavey 15:6c461501d12d 357 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 358 follow_line();
jamesheavey 15:6c461501d12d 359
jamesheavey 15:6c461501d12d 360 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 361 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 362 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 363 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 364 robot.forward(speed);
jamesheavey 15:6c461501d12d 365 leds = 0b1010;
jamesheavey 15:6c461501d12d 366 while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
jamesheavey 15:6c461501d12d 367 }
jamesheavey 15:6c461501d12d 368 pointer++;
jamesheavey 15:6c461501d12d 369 }
jamesheavey 15:6c461501d12d 370 }
jamesheavey 15:6c461501d12d 371
jamesheavey 15:6c461501d12d 372 back();
jamesheavey 12:d80399686f32 373
jamesheavey 11:c3299aca7d8f 374 robot.stop();
jamesheavey 14:87052bb35211 375 while ( button_enter.read() == 1 ) { speed = (pot_S*0.3)+0.2; } // keep looping waiting for Enter to be pressed (can change speed)
jamesheavey 11:c3299aca7d8f 376
jamesheavey 15:6c461501d12d 377 pointer = 0;
jamesheavey 15:6c461501d12d 378
jamesheavey 11:c3299aca7d8f 379 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 380 wait(0.2);
jamesheavey 11:c3299aca7d8f 381 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 382 wait(0.2);
jamesheavey 11:c3299aca7d8f 383 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 384 wait(0.2);
jamesheavey 11:c3299aca7d8f 385 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 386 wait(0.2);
jamesheavey 11:c3299aca7d8f 387
jamesheavey 11:c3299aca7d8f 388 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 389 follow_line();
jamesheavey 11:c3299aca7d8f 390
jamesheavey 14:87052bb35211 391 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 392 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 393 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 394 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 395 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 396 leds = 0b1010;
jamesheavey 11:c3299aca7d8f 397 while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 398 }
jamesheavey 11:c3299aca7d8f 399 pointer++;
jamesheavey 10:691c02b352cb 400 }
jamesheavey 10:691c02b352cb 401 }
jamesheavey 5:ae417756235a 402 }
jamesheavey 5:ae417756235a 403 }
jamesheavey 5:ae417756235a 404
jamesheavey 10:691c02b352cb 405 void invert_path()
jamesheavey 5:ae417756235a 406 {
jamesheavey 10:691c02b352cb 407 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 408 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 409 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 410 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 411 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 412 }
jamesheavey 5:ae417756235a 413 }