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@8:b862106f1162, 2020-02-22 (annotated)

Committer:
jamesheavey
Date:
Sat Feb 22 04:46:51 2020 +0000
Revision:
8:b862106f1162
Parent:
7:7fefd782532d
Child:
9:952586accbf9
goal detector works

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 0:df5216b20861 23 // Prototypes
jamesheavey 0:df5216b20861 24 void init();
jamesheavey 0:df5216b20861 25 void calibrate();
jamesheavey 0:df5216b20861 26 void follow_line( float speed );
jamesheavey 7:7fefd782532d 27 char junction_detect();
jamesheavey 6:c10b367747a0 28 char turn_logic();
jamesheavey 1:79219d0a33c8 29 void turn_selector( char turn );
jamesheavey 0:df5216b20861 30 void left();
jamesheavey 1:79219d0a33c8 31 void right();
jamesheavey 0:df5216b20861 32 void back();
jamesheavey 5:ae417756235a 33 //void check_goal();
jamesheavey 2:940e46e21353 34 void goal();
jamesheavey 2:940e46e21353 35 void simplify();
jamesheavey 5:ae417756235a 36 void return_to_start();
jamesheavey 0:df5216b20861 37
jamesheavey 0:df5216b20861 38 // Constants
jamesheavey 0:df5216b20861 39
jamesheavey 0:df5216b20861 40 const float A = 0.5; // 20
jamesheavey 0:df5216b20861 41 const float B = 1/10000; // 10000
jamesheavey 4:38c29dbc5953 42 const float C = 1.5; // 2/3
jamesheavey 0:df5216b20861 43
jamesheavey 5:ae417756235a 44 const int sens_thresh = 500; // replace the hard coded bits
jamesheavey 4:38c29dbc5953 45 const int turn_speed = 0.2;
jamesheavey 2:940e46e21353 46
jamesheavey 2:940e46e21353 47 // Global Variables
jamesheavey 2:940e46e21353 48
jamesheavey 2:940e46e21353 49 char path[100];
jamesheavey 2:940e46e21353 50 int path_length = 0;
jamesheavey 2:940e46e21353 51 unsigned int *sensor;
jamesheavey 3:a5e06482462e 52 float speed;
jamesheavey 5:ae417756235a 53 int goal_check;
jamesheavey 2:940e46e21353 54
jamesheavey 2:940e46e21353 55 // Main
jamesheavey 2:940e46e21353 56
jamesheavey 0:df5216b20861 57 int main()
jamesheavey 0:df5216b20861 58 {
jamesheavey 0:df5216b20861 59 init();
jamesheavey 0:df5216b20861 60 calibrate();
jamesheavey 5:ae417756235a 61 speed = (pot_S*0.3)+0.2; // have it so max is 0.5 and min is 0.2
jamesheavey 0:df5216b20861 62
jamesheavey 0:df5216b20861 63 float proportional = 0.0;
jamesheavey 0:df5216b20861 64 float prev_proportional = 0.0;
jamesheavey 0:df5216b20861 65 float integral = 0.0;
jamesheavey 0:df5216b20861 66 float derivative = 0.0;
jamesheavey 0:df5216b20861 67
jamesheavey 0:df5216b20861 68 float dt = 1/50; // updating 50 times a second
jamesheavey 0:df5216b20861 69
jamesheavey 0:df5216b20861 70 while (1) {
jamesheavey 0:df5216b20861 71 robot.scan();
jamesheavey 5:ae417756235a 72
jamesheavey 0:df5216b20861 73 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 1:79219d0a33c8 74 leds = 0b0110;
jamesheavey 0:df5216b20861 75
jamesheavey 0:df5216b20861 76 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 0:df5216b20861 77 derivative = proportional - prev_proportional;
jamesheavey 0:df5216b20861 78 integral += proportional;
jamesheavey 0:df5216b20861 79 prev_proportional = proportional;
jamesheavey 0:df5216b20861 80
jamesheavey 0:df5216b20861 81 // calculate motor correction
jamesheavey 0:df5216b20861 82 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 0:df5216b20861 83
jamesheavey 0:df5216b20861 84 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 0:df5216b20861 85 if( motor_correction > speed ) {
jamesheavey 0:df5216b20861 86 motor_correction = speed;
jamesheavey 0:df5216b20861 87 }
jamesheavey 0:df5216b20861 88 if( motor_correction < -speed ) {
jamesheavey 0:df5216b20861 89 motor_correction = -speed;
jamesheavey 0:df5216b20861 90 }
jamesheavey 0:df5216b20861 91
jamesheavey 0:df5216b20861 92 if( proportional < 0 ) {
jamesheavey 0:df5216b20861 93 robot.motors(speed+motor_correction,speed);
jamesheavey 0:df5216b20861 94 } else {
jamesheavey 0:df5216b20861 95 robot.motors(speed,speed-motor_correction);
jamesheavey 0:df5216b20861 96 }
jamesheavey 0:df5216b20861 97
jamesheavey 0:df5216b20861 98 //follow_line(speed);
jamesheavey 7:7fefd782532d 99 if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
jamesheavey 7:7fefd782532d 100 wait(0.05);
jamesheavey 7:7fefd782532d 101 if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
jamesheavey 7:7fefd782532d 102 goal();
jamesheavey 7:7fefd782532d 103 }
jamesheavey 7:7fefd782532d 104 }
jamesheavey 5:ae417756235a 105 // add if juntion detected, move forward until both 0 and 4 or the middle three are low, then check the type of junction and turn accordingly (lines below)
jamesheavey 6:c10b367747a0 106 //if ( junction_detect() == true ){
jamesheavey 7:7fefd782532d 107 char turn = junction_detect(); // rename this function something else
jamesheavey 5:ae417756235a 108 turn_selector(turn);
jamesheavey 6:c10b367747a0 109 if ( turn != 'S' && turn != 'R') { // doesnt need 'S', also may not need 'R' as that is not technically a junction
jamesheavey 6:c10b367747a0 110 path[path_length] = turn;
jamesheavey 6:c10b367747a0 111 path_length ++;
jamesheavey 6:c10b367747a0 112 }
jamesheavey 1:79219d0a33c8 113
jamesheavey 6:c10b367747a0 114 //}
jamesheavey 6:c10b367747a0 115
jamesheavey 1:79219d0a33c8 116
jamesheavey 5:ae417756235a 117
jamesheavey 0:df5216b20861 118
jamesheavey 4:38c29dbc5953 119 simplify();
jamesheavey 4:38c29dbc5953 120
jamesheavey 4:38c29dbc5953 121 robot.lcd_clear();
jamesheavey 4:38c29dbc5953 122 robot.lcd_print(path,100);
jamesheavey 4:38c29dbc5953 123
jamesheavey 4:38c29dbc5953 124 //robot.display_data();
jamesheavey 0:df5216b20861 125
jamesheavey 0:df5216b20861 126 wait(dt);
jamesheavey 0:df5216b20861 127 }
jamesheavey 0:df5216b20861 128 }
jamesheavey 0:df5216b20861 129
jamesheavey 0:df5216b20861 130 void init()
jamesheavey 0:df5216b20861 131 {
jamesheavey 0:df5216b20861 132 robot.init();
jamesheavey 0:df5216b20861 133
jamesheavey 0:df5216b20861 134 button_A.mode(PullUp);
jamesheavey 0:df5216b20861 135 button_B.mode(PullUp);
jamesheavey 0:df5216b20861 136 button_X.mode(PullUp);
jamesheavey 0:df5216b20861 137 button_Y.mode(PullUp);
jamesheavey 0:df5216b20861 138 button_enter.mode(PullUp);
jamesheavey 0:df5216b20861 139 button_back.mode(PullUp);
jamesheavey 0:df5216b20861 140
jamesheavey 5:ae417756235a 141 //leds = 0b0000;
jamesheavey 0:df5216b20861 142 }
jamesheavey 0:df5216b20861 143
jamesheavey 0:df5216b20861 144 void calibrate()
jamesheavey 0:df5216b20861 145 {
jamesheavey 0:df5216b20861 146 leds = 0b1111;
jamesheavey 0:df5216b20861 147 robot.reset_calibration();
jamesheavey 0:df5216b20861 148
jamesheavey 0:df5216b20861 149 while (button_enter.read() == 1) {} // keep looping waiting for Enter to be pressed
jamesheavey 0:df5216b20861 150
jamesheavey 0:df5216b20861 151 wait(2.0); // small delay to allow hands to move away etc.
jamesheavey 0:df5216b20861 152
jamesheavey 0:df5216b20861 153 robot.auto_calibrate(); // run calibration routine
jamesheavey 0:df5216b20861 154 leds = 0b0000;
jamesheavey 0:df5216b20861 155 }
jamesheavey 0:df5216b20861 156
jamesheavey 0:df5216b20861 157 void follow_line( float speed )
jamesheavey 0:df5216b20861 158 {
jamesheavey 0:df5216b20861 159
jamesheavey 1:79219d0a33c8 160 }
jamesheavey 0:df5216b20861 161
jamesheavey 7:7fefd782532d 162 char junction_detect()
jamesheavey 6:c10b367747a0 163 {
jamesheavey 7:7fefd782532d 164 bool left = false;
jamesheavey 6:c10b367747a0 165 bool right = false;
jamesheavey 7:7fefd782532d 166 bool straight = false;
jamesheavey 7:7fefd782532d 167 bool goal_test1 = false;
jamesheavey 7:7fefd782532d 168 bool goal_test2 = false;
jamesheavey 7:7fefd782532d 169
jamesheavey 7:7fefd782532d 170 if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
jamesheavey 7:7fefd782532d 171 return 'B';
jamesheavey 7:7fefd782532d 172 }
jamesheavey 7:7fefd782532d 173
jamesheavey 7:7fefd782532d 174 if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
jamesheavey 7:7fefd782532d 175 if (sensor[0] > sens_thresh) {
jamesheavey 7:7fefd782532d 176 left = true;
jamesheavey 7:7fefd782532d 177 while ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh) ) {
jamesheavey 7:7fefd782532d 178 robot.forward(speed);
jamesheavey 7:7fefd782532d 179 robot.scan();
jamesheavey 7:7fefd782532d 180 if (sensor[4] > sens_thresh) {
jamesheavey 7:7fefd782532d 181 right = true;
jamesheavey 7:7fefd782532d 182 }
jamesheavey 7:7fefd782532d 183
jamesheavey 7:7fefd782532d 184 }
jamesheavey 7:7fefd782532d 185
jamesheavey 7:7fefd782532d 186 if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
jamesheavey 7:7fefd782532d 187 robot.forward(speed);
jamesheavey 7:7fefd782532d 188 if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
jamesheavey 7:7fefd782532d 189 goal_test1 = true;
jamesheavey 7:7fefd782532d 190 }
jamesheavey 7:7fefd782532d 191
jamesheavey 7:7fefd782532d 192
jamesheavey 7:7fefd782532d 193 robot.scan();
jamesheavey 7:7fefd782532d 194 }
jamesheavey 7:7fefd782532d 195
jamesheavey 6:c10b367747a0 196
jamesheavey 7:7fefd782532d 197 if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
jamesheavey 7:7fefd782532d 198 straight = true;
jamesheavey 7:7fefd782532d 199 }
jamesheavey 7:7fefd782532d 200 }
jamesheavey 7:7fefd782532d 201
jamesheavey 7:7fefd782532d 202 else if (sensor[4] > sens_thresh) {
jamesheavey 7:7fefd782532d 203 right = true;
jamesheavey 7:7fefd782532d 204 while ( sensor[4] > sens_thresh && (sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh) ) {
jamesheavey 7:7fefd782532d 205 robot.forward(speed);
jamesheavey 7:7fefd782532d 206 robot.scan();
jamesheavey 7:7fefd782532d 207 if (sensor[0] > sens_thresh) {
jamesheavey 7:7fefd782532d 208 left = true;
jamesheavey 7:7fefd782532d 209 }
jamesheavey 7:7fefd782532d 210
jamesheavey 7:7fefd782532d 211 }
jamesheavey 7:7fefd782532d 212
jamesheavey 7:7fefd782532d 213 if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
jamesheavey 7:7fefd782532d 214 straight = true;
jamesheavey 7:7fefd782532d 215 }
jamesheavey 7:7fefd782532d 216 }
jamesheavey 7:7fefd782532d 217
jamesheavey 7:7fefd782532d 218
jamesheavey 7:7fefd782532d 219 if (goal_test1) {
jamesheavey 7:7fefd782532d 220 return 'G';
jamesheavey 7:7fefd782532d 221 } else if (left) {
jamesheavey 7:7fefd782532d 222 return 'L';
jamesheavey 7:7fefd782532d 223 } else if (straight) {
jamesheavey 7:7fefd782532d 224 return 'S';
jamesheavey 7:7fefd782532d 225 } else if (right) {
jamesheavey 7:7fefd782532d 226 return 'R';
jamesheavey 6:c10b367747a0 227 }
jamesheavey 6:c10b367747a0 228
jamesheavey 6:c10b367747a0 229 }
jamesheavey 7:7fefd782532d 230
jamesheavey 7:7fefd782532d 231 return 'S';
jamesheavey 6:c10b367747a0 232
jamesheavey 7:7fefd782532d 233 /*
jamesheavey 7:7fefd782532d 234 if ( sensor[0] > sens_thresh || sensor [4] > sens_thresh || (sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500) ) {
jamesheavey 6:c10b367747a0 235 return true;
jamesheavey 6:c10b367747a0 236 } else {
jamesheavey 6:c10b367747a0 237 return false;
jamesheavey 7:7fefd782532d 238 }*/
jamesheavey 6:c10b367747a0 239 }
jamesheavey 6:c10b367747a0 240
jamesheavey 6:c10b367747a0 241 char turn_logic()
jamesheavey 1:79219d0a33c8 242 {
jamesheavey 5:ae417756235a 243
jamesheavey 5:ae417756235a 244 // either increase the wait in main loop 50-> 20
jamesheavey 5:ae417756235a 245
jamesheavey 5:ae417756235a 246 //or have it so it inches forward at a junction (if junction detected, then inch forward and decide
jamesheavey 5:ae417756235a 247
jamesheavey 7:7fefd782532d 248 if ( sensor[0] > 500 ) {
jamesheavey 1:79219d0a33c8 249 return 'L';
jamesheavey 1:79219d0a33c8 250 } else if ( sensor[1] > 500 || sensor[2] > 500 || sensor[3] > 500 ) {
jamesheavey 1:79219d0a33c8 251 return 'S';
jamesheavey 1:79219d0a33c8 252 } else if ( sensor[4] > 500 ) {
jamesheavey 1:79219d0a33c8 253 return 'R';
jamesheavey 5:ae417756235a 254 } else if ( sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500 ) {
jamesheavey 5:ae417756235a 255 return 'B';
jamesheavey 0:df5216b20861 256 } else {
jamesheavey 1:79219d0a33c8 257 return 'S';
jamesheavey 0:df5216b20861 258 }
jamesheavey 0:df5216b20861 259 }
jamesheavey 0:df5216b20861 260
jamesheavey 1:79219d0a33c8 261 void turn_selector( char turn )
jamesheavey 1:79219d0a33c8 262 {
jamesheavey 1:79219d0a33c8 263 switch(turn) {
jamesheavey 5:ae417756235a 264 case 'G':
jamesheavey 5:ae417756235a 265 goal();
jamesheavey 1:79219d0a33c8 266 case 'L':
jamesheavey 1:79219d0a33c8 267 left();
jamesheavey 5:ae417756235a 268 break;
jamesheavey 1:79219d0a33c8 269 case 'S':
jamesheavey 1:79219d0a33c8 270 break;
jamesheavey 1:79219d0a33c8 271 case 'R':
jamesheavey 1:79219d0a33c8 272 right();
jamesheavey 5:ae417756235a 273 break;
jamesheavey 1:79219d0a33c8 274 case 'B':
jamesheavey 1:79219d0a33c8 275 back();
jamesheavey 5:ae417756235a 276 break;
jamesheavey 1:79219d0a33c8 277 }
jamesheavey 1:79219d0a33c8 278 }
jamesheavey 1:79219d0a33c8 279
jamesheavey 0:df5216b20861 280 void left()
jamesheavey 0:df5216b20861 281 {
jamesheavey 1:79219d0a33c8 282 leds = 0b1100;
jamesheavey 3:a5e06482462e 283
jamesheavey 3:a5e06482462e 284 while (sensor[0] > 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 285
jamesheavey 0:df5216b20861 286 robot.spin_left(0.2);
jamesheavey 3:a5e06482462e 287 wait(0.1);
jamesheavey 3:a5e06482462e 288
jamesheavey 3:a5e06482462e 289 while (sensor[1] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 290
jamesheavey 3:a5e06482462e 291 while (sensor[1] > 500) { robot.scan(); }
jamesheavey 1:79219d0a33c8 292 }
jamesheavey 1:79219d0a33c8 293
jamesheavey 1:79219d0a33c8 294 void right()
jamesheavey 1:79219d0a33c8 295 {
jamesheavey 1:79219d0a33c8 296 leds = 0b0011;
jamesheavey 5:ae417756235a 297
jamesheavey 3:a5e06482462e 298 while (sensor[4] > 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 299
jamesheavey 1:79219d0a33c8 300 robot.spin_right(0.2);
jamesheavey 3:a5e06482462e 301 wait(0.1);
jamesheavey 3:a5e06482462e 302
jamesheavey 3:a5e06482462e 303 while (sensor[3] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 304
jamesheavey 3:a5e06482462e 305 while (sensor[3] > 500) { robot.scan(); }
jamesheavey 0:df5216b20861 306 }
jamesheavey 0:df5216b20861 307
jamesheavey 0:df5216b20861 308 void back()
jamesheavey 0:df5216b20861 309 {
jamesheavey 1:79219d0a33c8 310 leds = 0b1111;
jamesheavey 5:ae417756235a 311 robot.reverse(speed);
jamesheavey 5:ae417756235a 312 wait(0.1);
jamesheavey 0:df5216b20861 313 robot.spin_right(0.2);
jamesheavey 3:a5e06482462e 314
jamesheavey 3:a5e06482462e 315 while (sensor[3] < 500) { robot.scan(); }
jamesheavey 3:a5e06482462e 316
jamesheavey 3:a5e06482462e 317 while (sensor[3] > 500) { robot.scan(); }
jamesheavey 0:df5216b20861 318 }
jamesheavey 1:79219d0a33c8 319
jamesheavey 2:940e46e21353 320 void simplify()
jamesheavey 1:79219d0a33c8 321 {
jamesheavey 2:940e46e21353 322 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 323 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 324 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 325
jamesheavey 4:38c29dbc5953 326 if( path[path_length-2] == 'B' && path_length > 3) {
jamesheavey 2:940e46e21353 327 int angle_change;
jamesheavey 2:940e46e21353 328
jamesheavey 4:38c29dbc5953 329 for (int i = 0; i < 3; i++) {
jamesheavey 4:38c29dbc5953 330 if (path[i] == 'L') { angle_change += 270; }
jamesheavey 4:38c29dbc5953 331 else if (path[i] == 'R') { angle_change += 90; }
jamesheavey 4:38c29dbc5953 332 else if (path[i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 333 }
jamesheavey 4:38c29dbc5953 334
jamesheavey 4:38c29dbc5953 335 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 336
jamesheavey 4:38c29dbc5953 337 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 338 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 339 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 340 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 341
jamesheavey 4:38c29dbc5953 342 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 343
jamesheavey 4:38c29dbc5953 344 path_length -= 2;
jamesheavey 2:940e46e21353 345 }
jamesheavey 5:ae417756235a 346 }
jamesheavey 5:ae417756235a 347
jamesheavey 5:ae417756235a 348 /*
jamesheavey 5:ae417756235a 349 void check_goal()
jamesheavey 5:ae417756235a 350 {
jamesheavey 5:ae417756235a 351 if (sensor[0] > sens_thresh && sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh && sensor[4] > sens_thresh) {
jamesheavey 5:ae417756235a 352 goal_check ++;
jamesheavey 5:ae417756235a 353 }
jamesheavey 5:ae417756235a 354 }*/
jamesheavey 5:ae417756235a 355
jamesheavey 5:ae417756235a 356 void goal()
jamesheavey 5:ae417756235a 357 {
jamesheavey 5:ae417756235a 358 robot.stop();
jamesheavey 5:ae417756235a 359 while(1) {
jamesheavey 5:ae417756235a 360 leds = 0b1000;
jamesheavey 5:ae417756235a 361 wait(0.2);
jamesheavey 5:ae417756235a 362 leds = 0b0100;
jamesheavey 5:ae417756235a 363 wait(0.2);
jamesheavey 5:ae417756235a 364 leds = 0b0010;
jamesheavey 5:ae417756235a 365 wait(0.2);
jamesheavey 5:ae417756235a 366 leds = 0b0001;
jamesheavey 5:ae417756235a 367 wait(0.2);
jamesheavey 5:ae417756235a 368 }
jamesheavey 5:ae417756235a 369 }
jamesheavey 5:ae417756235a 370
jamesheavey 5:ae417756235a 371 void return_to_start()
jamesheavey 5:ae417756235a 372 {
jamesheavey 5:ae417756235a 373
jamesheavey 5:ae417756235a 374 }