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@47:745c8846a92d, 2020-04-16 (annotated)

Committer:
jamesheavey
Date:
Thu Apr 16 16:25:02 2020 +0000
Revision:
47:745c8846a92d
Parent:
46:c770365cf5a6 
HOW DOES REMOVING retur_to_start() break it????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamesheavey 32:598bedb22c7c 1 #include "main.h"
jamesheavey 24:adb946be4ce5 2 #include <math.h>
jamesheavey 0:df5216b20861 3
jamesheavey 0:df5216b20861 4 // API
jamesheavey 0:df5216b20861 5 m3pi robot;
jamesheavey 0:df5216b20861 6
jamesheavey 0:df5216b20861 7 // LEDs
jamesheavey 0:df5216b20861 8 BusOut leds(LED4,LED3,LED2,LED1);
jamesheavey 0:df5216b20861 9
jamesheavey 0:df5216b20861 10 // Buttons
jamesheavey 0:df5216b20861 11 DigitalIn button_A(p18);
jamesheavey 0:df5216b20861 12 DigitalIn button_B(p17);
jamesheavey 0:df5216b20861 13 DigitalIn button_X(p21);
jamesheavey 0:df5216b20861 14 DigitalIn button_Y(p22);
jamesheavey 0:df5216b20861 15 DigitalIn button_enter(p24);
jamesheavey 0:df5216b20861 16 DigitalIn button_back(p23);
jamesheavey 0:df5216b20861 17
jamesheavey 0:df5216b20861 18 // Potentiometers
jamesheavey 0:df5216b20861 19 AnalogIn pot_P(p15);
jamesheavey 0:df5216b20861 20 AnalogIn pot_I(p16);
jamesheavey 0:df5216b20861 21 AnalogIn pot_D(p19);
jamesheavey 0:df5216b20861 22 AnalogIn pot_S(p20);
jamesheavey 0:df5216b20861 23
jamesheavey 17:77b8515a9568 24 // Sensors
jamesheavey 18:991658b628fc 25 DigitalInOut enc_L(p26); //connected to digital P26
jamesheavey 18:991658b628fc 26 DigitalInOut enc_R(p25); //connected to digital P25
jamesheavey 18:991658b628fc 27
jamesheavey 2:940e46e21353 28 // Main
jamesheavey 2:940e46e21353 29
jamesheavey 0:df5216b20861 30 int main()
jamesheavey 0:df5216b20861 31 {
jamesheavey 0:df5216b20861 32 init();
jamesheavey 20:5cf6a378801d 33
jamesheavey 20:5cf6a378801d 34 robot.lcd_goto_xy(0,0);
jamesheavey 22:02dda79d50b4 35 robot.lcd_print("A=SIMPLE", 10);
jamesheavey 20:5cf6a378801d 36 robot.lcd_goto_xy(0,1);
jamesheavey 22:02dda79d50b4 37 robot.lcd_print("B=LOOPED", 10);
jamesheavey 20:5cf6a378801d 38
jamesheavey 20:5cf6a378801d 39 while(button_A.read() == 1 && button_B.read() == 1) {}
jamesheavey 20:5cf6a378801d 40
jamesheavey 20:5cf6a378801d 41 if (button_B.read()) { loop_check = true; } // non-looped
jamesheavey 20:5cf6a378801d 42 if (button_A.read()) { loop_check = false; } // looped
jamesheavey 20:5cf6a378801d 43
jamesheavey 20:5cf6a378801d 44 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 45 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 46 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 47 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 48 robot.lcd_print("= start ", 10);
jamesheavey 20:5cf6a378801d 49
jamesheavey 0:df5216b20861 50 calibrate();
jamesheavey 15:6c461501d12d 51
jamesheavey 20:5cf6a378801d 52 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 53
jamesheavey 43:ec047ba15db1 54 speed = 0.3;//(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 55
jamesheavey 0:df5216b20861 56 float dt = 1/50; // updating 50 times a second
jamesheavey 20:5cf6a378801d 57
jamesheavey 0:df5216b20861 58 while (1) {
jamesheavey 21:54ea75f7984f 59
jamesheavey 20:5cf6a378801d 60 if (loop_check == true) {
jamesheavey 20:5cf6a378801d 61 non_looped();
jamesheavey 20:5cf6a378801d 62 } else {
jamesheavey 20:5cf6a378801d 63 looped();
jamesheavey 18:991658b628fc 64 }
jamesheavey 0:df5216b20861 65
jamesheavey 0:df5216b20861 66 wait(dt);
jamesheavey 0:df5216b20861 67 }
jamesheavey 0:df5216b20861 68 }
jamesheavey 0:df5216b20861 69
jamesheavey 18:991658b628fc 70 void read_encoders()
jamesheavey 18:991658b628fc 71 {
jamesheavey 18:991658b628fc 72 enc_R.output(); // Set the I/O line to an output
jamesheavey 18:991658b628fc 73 enc_L.output();
jamesheavey 18:991658b628fc 74 enc_R.mode(PullUp);
jamesheavey 18:991658b628fc 75 enc_L.mode(PullUp);
jamesheavey 18:991658b628fc 76
jamesheavey 19:4c08275cb3c9 77 wait_us(10); // Must be atleast 10us for the 10 nF capacitor to charge
jamesheavey 18:991658b628fc 78 enc_R.mode(PullNone);
jamesheavey 18:991658b628fc 79 enc_L.mode(PullNone);
jamesheavey 18:991658b628fc 80 enc_R = 1; // Drive the line high
jamesheavey 18:991658b628fc 81 enc_L = 1;
jamesheavey 18:991658b628fc 82
jamesheavey 18:991658b628fc 83 t_R.start();
jamesheavey 18:991658b628fc 84 enc_R.input(); // Make the I/O line an input (high impedance)
jamesheavey 19:4c08275cb3c9 85 while (enc_R == 1 || t_R.read_us() < 1000); // replace 1000 with a hard variable (1000 = 1ms = 1kHz sampling) (might be able to drop this further
jamesheavey 19:4c08275cb3c9 86 // sampling time is required to be this high for times when there is no reflectance but we only care about high reflectance
jamesheavey 19:4c08275cb3c9 87
jamesheavey 19:4c08275cb3c9 88 // maybe i could wait a few microseconds, see if the encoder is still high, if high then no reflectance, if low, the high reflectance
jamesheavey 19:4c08275cb3c9 89 // this would increase sampling time
jamesheavey 19:4c08275cb3c9 90
jamesheavey 19:4c08275cb3c9 91 // also, the fact that the waits are in the same loop means that the loop will run at different speeds depending on whether a sensor is triggered or not
jamesheavey 19:4c08275cb3c9 92 // if both are triggered it will run fast, otherwise it will have to wait 1000+ us for each sensor
jamesheavey 19:4c08275cb3c9 93
jamesheavey 19:4c08275cb3c9 94 // this therefore needs to be done in parallel and also must not affect the time of other operations in the main loop
jamesheavey 18:991658b628fc 95 encoder[0] = t_R.read_us(); // Measure the time for the capacitor to discharge by waiting for the I/O line to go low
jamesheavey 18:991658b628fc 96 t_R.stop();
jamesheavey 18:991658b628fc 97 t_R.reset();
jamesheavey 18:991658b628fc 98
jamesheavey 18:991658b628fc 99 t_L.start();
jamesheavey 18:991658b628fc 100 enc_L.input();
jamesheavey 19:4c08275cb3c9 101 while (enc_L == 1 || t_L.read_us() < 1000);
jamesheavey 18:991658b628fc 102 encoder[1] = t_L.read_us();
jamesheavey 18:991658b628fc 103 t_L.stop();
jamesheavey 18:991658b628fc 104 t_L.reset();
jamesheavey 18:991658b628fc 105 }
jamesheavey 18:991658b628fc 106
jamesheavey 0:df5216b20861 107 void init()
jamesheavey 0:df5216b20861 108 {
jamesheavey 0:df5216b20861 109 robot.init();
jamesheavey 0:df5216b20861 110
jamesheavey 0:df5216b20861 111 button_A.mode(PullUp);
jamesheavey 0:df5216b20861 112 button_B.mode(PullUp);
jamesheavey 0:df5216b20861 113 button_X.mode(PullUp);
jamesheavey 0:df5216b20861 114 button_Y.mode(PullUp);
jamesheavey 0:df5216b20861 115 button_enter.mode(PullUp);
jamesheavey 0:df5216b20861 116 button_back.mode(PullUp);
jamesheavey 0:df5216b20861 117
jamesheavey 10:691c02b352cb 118 leds = 0b0000;
jamesheavey 0:df5216b20861 119 }
jamesheavey 0:df5216b20861 120
jamesheavey 0:df5216b20861 121 void calibrate()
jamesheavey 0:df5216b20861 122 {
jamesheavey 0:df5216b20861 123 leds = 0b1111;
jamesheavey 0:df5216b20861 124 robot.reset_calibration();
jamesheavey 0:df5216b20861 125
jamesheavey 15:6c461501d12d 126 while (button_enter.read() == 1) {} // wait for enter to be pressed
jamesheavey 15:6c461501d12d 127
jamesheavey 15:6c461501d12d 128 wait(2.0);
jamesheavey 0:df5216b20861 129
jamesheavey 15:6c461501d12d 130 robot.auto_calibrate();
jamesheavey 17:77b8515a9568 131
jamesheavey 15:6c461501d12d 132 robot.stop();
jamesheavey 15:6c461501d12d 133 wait(0.05);
jamesheavey 15:6c461501d12d 134 robot.scan();
jamesheavey 17:77b8515a9568 135
jamesheavey 17:77b8515a9568 136 leds = 0b0000;
jamesheavey 0:df5216b20861 137 }
jamesheavey 20:5cf6a378801d 138
jamesheavey 20:5cf6a378801d 139 void non_looped()
jamesheavey 20:5cf6a378801d 140 {
jamesheavey 20:5cf6a378801d 141 follow_line();
jamesheavey 20:5cf6a378801d 142
jamesheavey 20:5cf6a378801d 143 if ( junction_detect() ) {
jamesheavey 20:5cf6a378801d 144 char turn = junction_logic();
jamesheavey 20:5cf6a378801d 145 turn_select(turn);
jamesheavey 20:5cf6a378801d 146
jamesheavey 20:5cf6a378801d 147 path[path_length] = turn;
jamesheavey 20:5cf6a378801d 148 path_length ++;
jamesheavey 20:5cf6a378801d 149 }
jamesheavey 20:5cf6a378801d 150
jamesheavey 20:5cf6a378801d 151 simplify();
jamesheavey 20:5cf6a378801d 152
jamesheavey 20:5cf6a378801d 153 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 154 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 155
jamesheavey 20:5cf6a378801d 156 //robot.display_data();
jamesheavey 20:5cf6a378801d 157 }
jamesheavey 20:5cf6a378801d 158
jamesheavey 23:71e84953b3f3 159 void looped()
jamesheavey 20:5cf6a378801d 160 {
jamesheavey 24:adb946be4ce5 161 if( first ) { // init the start node on first loop run only
jamesheavey 23:71e84953b3f3 162 first = false;
jamesheavey 23:71e84953b3f3 163 curr_coords[0] = 0;
jamesheavey 23:71e84953b3f3 164 curr_coords[1] = 0;
jamesheavey 23:71e84953b3f3 165 dir = 'N';
jamesheavey 23:71e84953b3f3 166 total_points = 0;
jamesheavey 34:63f7c61ee4da 167 point[total_points] = total_points; // first point is 0
jamesheavey 23:71e84953b3f3 168 coords_x[total_points] = curr_coords[0];
jamesheavey 23:71e84953b3f3 169 coords_y[total_points] = curr_coords[1];
jamesheavey 34:63f7c61ee4da 170 type[total_points] = 0b0100; // start is always 1 exit type in the north direction
jamesheavey 34:63f7c61ee4da 171 explored[total_points] = 0b0100;
jamesheavey 26:582560881379 172 looped_path[total_points] = 0; // start node is '0'
jamesheavey 23:71e84953b3f3 173 }
jamesheavey 23:71e84953b3f3 174
jamesheavey 20:5cf6a378801d 175 // follow line until reaching a junction, determine its type and coordinates
jamesheavey 38:b5b06625d06e 176 if ( t_restart ){ // only start the timer if it isnt already started
jamesheavey 38:b5b06625d06e 177 t_coord.start();
jamesheavey 38:b5b06625d06e 178 t_restart = false;
jamesheavey 38:b5b06625d06e 179
jamesheavey 38:b5b06625d06e 180 }
jamesheavey 21:54ea75f7984f 181
jamesheavey 21:54ea75f7984f 182 follow_line();
jamesheavey 21:54ea75f7984f 183
jamesheavey 21:54ea75f7984f 184 if ( junction_detect() ) {
jamesheavey 26:582560881379 185 path_length++; // increment the path position index
jamesheavey 22:02dda79d50b4 186
jamesheavey 38:b5b06625d06e 187 float time = t_coord.read();
jamesheavey 38:b5b06625d06e 188 int dist_est = ceil(time*2); // scaled so that a longer straight will have a different time to a shorter straight
jamesheavey 38:b5b06625d06e 189 t_coord.stop();
jamesheavey 38:b5b06625d06e 190 t_coord.reset();
jamesheavey 38:b5b06625d06e 191 t_restart = true; //restart the timer next loop
jamesheavey 21:54ea75f7984f 192
jamesheavey 38:b5b06625d06e 193 if (dir == 'N'){ curr_coords[1] += dist_est; } // y coord
jamesheavey 38:b5b06625d06e 194 if (dir == 'E'){ curr_coords[0] += dist_est; } // x coord
jamesheavey 38:b5b06625d06e 195 if (dir == 'S'){ curr_coords[1] -= dist_est; }
jamesheavey 38:b5b06625d06e 196 if (dir == 'W'){ curr_coords[0] -= dist_est; }
jamesheavey 21:54ea75f7984f 197
jamesheavey 21:54ea75f7984f 198 // check that the coordinates are not already in the list, if not add the point, if it is already return the point number and increment the explored
jamesheavey 27:0a3f028f9365 199 if (coord_check()) { // coord_check returns true if curr_coords coordinates are not present in coords_x and y
jamesheavey 32:598bedb22c7c 200
jamesheavey 22:02dda79d50b4 201 total_points++;
jamesheavey 27:0a3f028f9365 202 node_logic(); // determines what junction type we are at updates the explored (path entered on) and type arrays accordingly
jamesheavey 30:d62f122e8d60 203
jamesheavey 43:ec047ba15db1 204 if(goal_node) { point[total_points] = 100; goal_node = false; } // 100 will be the indicator for the goal node that we can visit once mapping is complete
jamesheavey 43:ec047ba15db1 205 else { point[total_points] = total_points; } // numbered 0 -> total_points
jamesheavey 30:d62f122e8d60 206
jamesheavey 22:02dda79d50b4 207 coords_x[total_points] = curr_coords[0];
jamesheavey 22:02dda79d50b4 208 coords_y[total_points] = curr_coords[1];
jamesheavey 22:02dda79d50b4 209 }
jamesheavey 21:54ea75f7984f 210
jamesheavey 32:598bedb22c7c 211 update_index();
jamesheavey 44:73a83a0fa467 212
jamesheavey 22:02dda79d50b4 213 // use current coords to find which point to place in path
jamesheavey 31:1e6d0ef05996 214
jamesheavey 25:7523239a2fc1 215 looped_path[path_length] = point[curr_index]; //returns an int of which point we are at what its called
jamesheavey 23:71e84953b3f3 216 choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
jamesheavey 21:54ea75f7984f 217
jamesheavey 24:adb946be4ce5 218 // check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
jamesheavey 23:71e84953b3f3 219 // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
jamesheavey 25:7523239a2fc1 220 // i.e. the function will take the node ID integer as an argument and go backwards through the path until reaching that node (appending each node along the way to the end of the path)
jamesheavey 21:54ea75f7984f 221
jamesheavey 26:582560881379 222 // needs a function that checks if current node has any paths left to explore, if not, then it must return via the path to a node that isnt fully explored and continue from there
jamesheavey 26:582560881379 223
jamesheavey 21:54ea75f7984f 224 }
jamesheavey 32:598bedb22c7c 225
jamesheavey 31:1e6d0ef05996 226
jamesheavey 31:1e6d0ef05996 227 // robot.lcd_clear();
jamesheavey 31:1e6d0ef05996 228 // char *b = &dir;
jamesheavey 31:1e6d0ef05996 229 // robot.lcd_print(b,1);
jamesheavey 31:1e6d0ef05996 230
jamesheavey 31:1e6d0ef05996 231
jamesheavey 20:5cf6a378801d 232 //robot.display_data();
jamesheavey 20:5cf6a378801d 233 }
jamesheavey 21:54ea75f7984f 234
jamesheavey 23:71e84953b3f3 235 bool coord_check()
jamesheavey 23:71e84953b3f3 236 {
jamesheavey 23:71e84953b3f3 237 bool result = true;
jamesheavey 23:71e84953b3f3 238 //returns true if the current coords dont match a previous point
jamesheavey 23:71e84953b3f3 239 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 240 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 23:71e84953b3f3 241 result = false;
jamesheavey 23:71e84953b3f3 242 }
jamesheavey 23:71e84953b3f3 243 }
jamesheavey 23:71e84953b3f3 244
jamesheavey 23:71e84953b3f3 245 return result;
jamesheavey 23:71e84953b3f3 246 }
jamesheavey 23:71e84953b3f3 247
jamesheavey 25:7523239a2fc1 248 void update_index() // update index (pointer to current point/type/coords/explored)
jamesheavey 23:71e84953b3f3 249 {
jamesheavey 23:71e84953b3f3 250 // checks the curr_coords againts the coords array, returns the index to relate to the point array
jamesheavey 23:71e84953b3f3 251 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 252 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 25:7523239a2fc1 253 curr_index = i;
jamesheavey 23:71e84953b3f3 254 }
jamesheavey 23:71e84953b3f3 255 }
jamesheavey 23:71e84953b3f3 256 }
jamesheavey 23:71e84953b3f3 257
jamesheavey 29:ecf497c3fdc0 258 int path_to_point_index( int path_point )
jamesheavey 29:ecf497c3fdc0 259 {
jamesheavey 29:ecf497c3fdc0 260 for(int i = 0; i <= total_points; i++) {
jamesheavey 29:ecf497c3fdc0 261 if(path_point == point[i]) {
jamesheavey 29:ecf497c3fdc0 262 return i;
jamesheavey 29:ecf497c3fdc0 263 }
jamesheavey 29:ecf497c3fdc0 264 }
jamesheavey 29:ecf497c3fdc0 265
jamesheavey 29:ecf497c3fdc0 266 return curr_index; // default
jamesheavey 29:ecf497c3fdc0 267 }
jamesheavey 29:ecf497c3fdc0 268
jamesheavey 21:54ea75f7984f 269 void node_logic()
jamesheavey 21:54ea75f7984f 270 {
jamesheavey 24:adb946be4ce5 271 // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
jamesheavey 25:7523239a2fc1 272
jamesheavey 25:7523239a2fc1 273 // first determine what turns are available relative to the robots current direction (left, straight etc.)
jamesheavey 25:7523239a2fc1 274 // convert these relative available turns into available absolute diections (N,E etc.)
jamesheavey 25:7523239a2fc1 275 // set _type to the appropriate value based on available directions (including entry direction = opposite of current)
jamesheavey 25:7523239a2fc1 276 // set _explored entry path as 1
jamesheavey 25:7523239a2fc1 277 // set type[total_points] = _type; & explored[total_points] = _explored;
jamesheavey 25:7523239a2fc1 278
jamesheavey 21:54ea75f7984f 279 bool north = false;
jamesheavey 21:54ea75f7984f 280 bool south = false;
jamesheavey 21:54ea75f7984f 281 bool east = false;
jamesheavey 21:54ea75f7984f 282 bool west = false;
jamesheavey 21:54ea75f7984f 283
jamesheavey 21:54ea75f7984f 284 bool left = false;
jamesheavey 21:54ea75f7984f 285 bool straight = false;
jamesheavey 21:54ea75f7984f 286 bool right = false;
jamesheavey 43:ec047ba15db1 287 bool goal = false;
jamesheavey 21:54ea75f7984f 288
jamesheavey 25:7523239a2fc1 289 int _type = 0b0000;
jamesheavey 25:7523239a2fc1 290 int _explored = 0b0000;
jamesheavey 21:54ea75f7984f 291
jamesheavey 21:54ea75f7984f 292 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 21:54ea75f7984f 293 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 21:54ea75f7984f 294 robot.forward(speed);
jamesheavey 21:54ea75f7984f 295 robot.scan();
jamesheavey 21:54ea75f7984f 296 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 21:54ea75f7984f 297 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 21:54ea75f7984f 298 }
jamesheavey 21:54ea75f7984f 299
jamesheavey 21:54ea75f7984f 300 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 301 wait(0.05); // maybe change or replace w something better
jamesheavey 21:54ea75f7984f 302 robot.scan();
jamesheavey 27:0a3f028f9365 303 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 43:ec047ba15db1 304 goal = true;
jamesheavey 27:0a3f028f9365 305 }
jamesheavey 21:54ea75f7984f 306 }
jamesheavey 21:54ea75f7984f 307 robot.scan();
jamesheavey 21:54ea75f7984f 308
jamesheavey 21:54ea75f7984f 309 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 310 straight = true;
jamesheavey 21:54ea75f7984f 311 }
jamesheavey 21:54ea75f7984f 312 }
jamesheavey 21:54ea75f7984f 313
jamesheavey 43:ec047ba15db1 314 if(goal) {
jamesheavey 43:ec047ba15db1 315 if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 43:ec047ba15db1 316 else if ( dir == 'E' ) { west = true; _explored |= 0b1000; }
jamesheavey 43:ec047ba15db1 317 else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 43:ec047ba15db1 318 else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 43:ec047ba15db1 319
jamesheavey 43:ec047ba15db1 320 if ( west ) { _type |= 0b1000; }
jamesheavey 43:ec047ba15db1 321 if ( north ) { _type |= 0b0100; }
jamesheavey 43:ec047ba15db1 322 if ( east ) { _type |= 0b0010; }
jamesheavey 43:ec047ba15db1 323 if ( south ) { _type |= 0b0001; }
jamesheavey 43:ec047ba15db1 324
jamesheavey 43:ec047ba15db1 325 goal_node = true;
jamesheavey 43:ec047ba15db1 326 }
jamesheavey 21:54ea75f7984f 327
jamesheavey 43:ec047ba15db1 328 else {
jamesheavey 27:0a3f028f9365 329 int angle = 0;
jamesheavey 28:63ff8290964a 330 int reset_ang = 0;
jamesheavey 27:0a3f028f9365 331
jamesheavey 34:63f7c61ee4da 332 if (dir == 'E') { angle = 90; }
jamesheavey 34:63f7c61ee4da 333 else if (dir == 'S') { angle = 180; }
jamesheavey 34:63f7c61ee4da 334 else if (dir == 'W') { angle = 270; }
jamesheavey 34:63f7c61ee4da 335
jamesheavey 34:63f7c61ee4da 336 reset_ang = angle;
jamesheavey 27:0a3f028f9365 337
jamesheavey 27:0a3f028f9365 338 if (left) {
jamesheavey 27:0a3f028f9365 339 angle += 270;
jamesheavey 27:0a3f028f9365 340 angle = angle % 360;
jamesheavey 27:0a3f028f9365 341 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 342 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 343 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 344 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 345 angle = reset_ang;
jamesheavey 27:0a3f028f9365 346 }
jamesheavey 27:0a3f028f9365 347
jamesheavey 27:0a3f028f9365 348 if (right) {
jamesheavey 27:0a3f028f9365 349 angle += 90;
jamesheavey 27:0a3f028f9365 350 angle = angle % 360;
jamesheavey 27:0a3f028f9365 351 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 352 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 353 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 354 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 355 angle = reset_ang;
jamesheavey 27:0a3f028f9365 356 }
jamesheavey 27:0a3f028f9365 357
jamesheavey 27:0a3f028f9365 358 if (straight) {
jamesheavey 27:0a3f028f9365 359 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 360 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 361 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 362 if (angle == 270) { west = true; }
jamesheavey 27:0a3f028f9365 363 }
jamesheavey 27:0a3f028f9365 364
jamesheavey 27:0a3f028f9365 365 if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 366 else if ( dir == 'E' ) { west = true; _explored |= 0b1000; } // this is acc done in choose turn so might not be needed here
jamesheavey 27:0a3f028f9365 367 else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 27:0a3f028f9365 368 else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 27:0a3f028f9365 369
jamesheavey 27:0a3f028f9365 370 if ( west ) { _type |= 0b1000; }
jamesheavey 27:0a3f028f9365 371 if ( north ) { _type |= 0b0100; }
jamesheavey 27:0a3f028f9365 372 if ( east ) { _type |= 0b0010; }
jamesheavey 27:0a3f028f9365 373 if ( south ) { _type |= 0b0001; }
jamesheavey 43:ec047ba15db1 374 }
jamesheavey 27:0a3f028f9365 375
jamesheavey 26:582560881379 376 type[total_points] = _type; // maybe update_index and use curr_index instead of total_points
jamesheavey 25:7523239a2fc1 377 explored[total_points] = _explored;
jamesheavey 21:54ea75f7984f 378
jamesheavey 21:54ea75f7984f 379 }
jamesheavey 21:54ea75f7984f 380
jamesheavey 21:54ea75f7984f 381 void choose_turn()
jamesheavey 21:54ea75f7984f 382 {
jamesheavey 25:7523239a2fc1 383 // look at cuurent coords, find what node we are at
jamesheavey 24:adb946be4ce5 384 // looks at the type vs the explored and does the turn that is equivalent to the first 1 in type that is a 0 in explored (WNES priority)
jamesheavey 25:7523239a2fc1 385 // sets the explored of the current node to 1 in whatever path is chosen
jamesheavey 25:7523239a2fc1 386 // also update dir
jamesheavey 37:2967f3f9c936 387
jamesheavey 34:63f7c61ee4da 388 if( dir == 'N' ) { explored[curr_index] |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 389 else if ( dir == 'E' ) { explored[curr_index] |= 0b1000; }
jamesheavey 34:63f7c61ee4da 390 else if ( dir == 'S' ) { explored[curr_index] |= 0b0100; }
jamesheavey 34:63f7c61ee4da 391 else if ( dir == 'W' ) { explored[curr_index] |= 0b0010; }
jamesheavey 39:005ad4610152 392 // print_data("enter junc");
jamesheavey 25:7523239a2fc1 393 int unexp_paths = type[curr_index] & ~( explored[curr_index] ); // produces a binary of 1's in the available unexplored paths
jamesheavey 25:7523239a2fc1 394
jamesheavey 44:73a83a0fa467 395 if ( unexp_paths == 0b0000 ) { back_track(); }
jamesheavey 44:73a83a0fa467 396 unexp_paths = type[curr_index] & ~( explored[curr_index] );
jamesheavey 32:598bedb22c7c 397
jamesheavey 32:598bedb22c7c 398 int curr_angle = 0;
jamesheavey 32:598bedb22c7c 399 if ( dir == 'E' ) { curr_angle = 90;}
jamesheavey 32:598bedb22c7c 400 else if ( dir == 'S' ) { curr_angle = 180;}
jamesheavey 32:598bedb22c7c 401 else if ( dir == 'W' ) { curr_angle = 270;}
jamesheavey 25:7523239a2fc1 402
jamesheavey 32:598bedb22c7c 403 int desired_angle = 0;
jamesheavey 32:598bedb22c7c 404 if ( (unexp_paths & 0b1000) == 0b1000) {
jamesheavey 32:598bedb22c7c 405 desired_angle = 270;
jamesheavey 32:598bedb22c7c 406 dir = 'W';
jamesheavey 32:598bedb22c7c 407 explored[curr_index] |= 0b1000;
jamesheavey 32:598bedb22c7c 408 }
jamesheavey 32:598bedb22c7c 409 else if ( (unexp_paths & 0b0100) == 0b0100) {
jamesheavey 32:598bedb22c7c 410 desired_angle = 0;
jamesheavey 32:598bedb22c7c 411 dir = 'N';
jamesheavey 32:598bedb22c7c 412 explored[curr_index] |= 0b0100;
jamesheavey 25:7523239a2fc1 413 }
jamesheavey 32:598bedb22c7c 414 else if ( (unexp_paths & 0b0010) == 0b0010) {
jamesheavey 32:598bedb22c7c 415 desired_angle = 90;
jamesheavey 32:598bedb22c7c 416 dir = 'E';
jamesheavey 32:598bedb22c7c 417 explored[curr_index] |= 0b0010;
jamesheavey 32:598bedb22c7c 418 }
jamesheavey 32:598bedb22c7c 419 else if ( (unexp_paths & 0b0001) == 0b0001) {
jamesheavey 32:598bedb22c7c 420 desired_angle = 180;
jamesheavey 32:598bedb22c7c 421 dir = 'S';
jamesheavey 32:598bedb22c7c 422 explored[curr_index] |= 0b0001;
jamesheavey 32:598bedb22c7c 423 }
jamesheavey 32:598bedb22c7c 424
jamesheavey 32:598bedb22c7c 425 int turn_angle = (desired_angle - curr_angle + 360) % 360;
jamesheavey 32:598bedb22c7c 426
jamesheavey 39:005ad4610152 427 // robot.lcd_clear();
jamesheavey 39:005ad4610152 428 // robot.lcd_print("turn" , 4);
jamesheavey 39:005ad4610152 429 // wait(2);
jamesheavey 37:2967f3f9c936 430
jamesheavey 39:005ad4610152 431 // robot.lcd_clear();
jamesheavey 39:005ad4610152 432 // if( turn_angle == 0) { robot.lcd_print("S",1); }
jamesheavey 39:005ad4610152 433 // else if( turn_angle == 90) { robot.lcd_print("R",1); }
jamesheavey 39:005ad4610152 434 // else if( turn_angle == 180) { robot.lcd_print("B",1); }
jamesheavey 39:005ad4610152 435 // else if( turn_angle == 270) { robot.lcd_print("L",1); }
jamesheavey 39:005ad4610152 436 // wait(2);
jamesheavey 39:005ad4610152 437 //
jamesheavey 39:005ad4610152 438 // print_data("After Turn");
jamesheavey 37:2967f3f9c936 439
jamesheavey 46:c770365cf5a6 440 if( turn_angle == 0) { turn_select('S'); }
jamesheavey 32:598bedb22c7c 441 else if( turn_angle == 90) { turn_select('R'); }
jamesheavey 46:c770365cf5a6 442 else if( turn_angle == 180) { turn_select('B'); }
jamesheavey 32:598bedb22c7c 443 else if( turn_angle == 270) { turn_select('L'); }
jamesheavey 21:54ea75f7984f 444 }
jamesheavey 21:54ea75f7984f 445
jamesheavey 28:63ff8290964a 446 void back_track()
jamesheavey 28:63ff8290964a 447 {
jamesheavey 33:9fa9e09f2e8f 448 // find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
jamesheavey 29:ecf497c3fdc0 449 // also if no nodes have unexplored paths set complete to true
jamesheavey 29:ecf497c3fdc0 450
jamesheavey 44:73a83a0fa467 451 complete = false;
jamesheavey 46:c770365cf5a6 452 int d_node; // an index to the most recent (desired) node with unexplored paths
jamesheavey 29:ecf497c3fdc0 453
jamesheavey 34:63f7c61ee4da 454 for(int i = total_points; i >= 0; i--) { // start from the most recently discovered node
jamesheavey 34:63f7c61ee4da 455 if( explored[i] != type[i] ) {
jamesheavey 35:2fd4ee9ac889 456 complete = true;
jamesheavey 46:c770365cf5a6 457 d_node = i;
jamesheavey 34:63f7c61ee4da 458 break;
jamesheavey 34:63f7c61ee4da 459 }
jamesheavey 29:ecf497c3fdc0 460 }
jamesheavey 29:ecf497c3fdc0 461
jamesheavey 35:2fd4ee9ac889 462 if( complete == false ) { looped_goal(); }
jamesheavey 34:63f7c61ee4da 463
jamesheavey 34:63f7c61ee4da 464 else {
jamesheavey 34:63f7c61ee4da 465 // compare node coordinates to previous node coordinates
jamesheavey 34:63f7c61ee4da 466 // determine which direction the previous node is compared to current
jamesheavey 34:63f7c61ee4da 467 // set the current nodes direction path to 0 and the opposite direction path to 0
jamesheavey 34:63f7c61ee4da 468 // decrement the count (setting the previous as current node and the next previous as previous)
jamesheavey 34:63f7c61ee4da 469 // when previous node == point[pointer1] break
jamesheavey 34:63f7c61ee4da 470 // choose turn should then do all those turns and arrive at correct node
jamesheavey 45:4fa8af658523 471
jamesheavey 45:4fa8af658523 472 // check if the current node exists before the discovery of the desired node
jamesheavey 45:4fa8af658523 473 // if it does, check the number of nodes between before and after
jamesheavey 45:4fa8af658523 474 // which ever is shorter, set the 0 of those nodes
jamesheavey 45:4fa8af658523 475
jamesheavey 45:4fa8af658523 476 int curr_node = curr_index;
jamesheavey 45:4fa8af658523 477 bool before = false;
jamesheavey 45:4fa8af658523 478 bool after = false;
jamesheavey 45:4fa8af658523 479 int before_index;
jamesheavey 45:4fa8af658523 480 int after_index;
jamesheavey 45:4fa8af658523 481
jamesheavey 45:4fa8af658523 482 bool desired_discovered = false;
jamesheavey 45:4fa8af658523 483 for(int k = 0; k <= path_length; k++) {
jamesheavey 46:c770365cf5a6 484 if(looped_path[k] == point[d_node]) { desired_discovered = true; } // maybe set a variable
jamesheavey 45:4fa8af658523 485 if(desired_discovered == false && looped_path[k] == point[curr_node]) { before = true; before_index = k; }
jamesheavey 45:4fa8af658523 486 if(desired_discovered == true && looped_path[k] == point[curr_node]) { after_index = k; break; }
jamesheavey 45:4fa8af658523 487 }
jamesheavey 45:4fa8af658523 488
jamesheavey 46:c770365cf5a6 489
jamesheavey 46:c770365cf5a6 490 if( before_index < after_index ){
jamesheavey 46:c770365cf5a6 491 before = true;
jamesheavey 46:c770365cf5a6 492 after = false;
jamesheavey 45:4fa8af658523 493 }
jamesheavey 46:c770365cf5a6 494 else{
jamesheavey 46:c770365cf5a6 495 before = false;
jamesheavey 46:c770365cf5a6 496 after = true;
jamesheavey 46:c770365cf5a6 497 }
jamesheavey 46:c770365cf5a6 498
jamesheavey 45:4fa8af658523 499
jamesheavey 34:63f7c61ee4da 500 char dir_diff;
jamesheavey 45:4fa8af658523 501
jamesheavey 45:4fa8af658523 502 if(after == true) {
jamesheavey 45:4fa8af658523 503
jamesheavey 46:c770365cf5a6 504 for(int j = after_index; j >= 0; j--) {
jamesheavey 45:4fa8af658523 505 curr_node = path_to_point_index(looped_path[j]);
jamesheavey 45:4fa8af658523 506
jamesheavey 45:4fa8af658523 507 int prev_node = path_to_point_index(looped_path[j-1]);
jamesheavey 45:4fa8af658523 508 if(coords_x[prev_node] != coords_x[curr_node]) {
jamesheavey 45:4fa8af658523 509 if(coords_x[prev_node] - coords_x[curr_node] > 0){
jamesheavey 45:4fa8af658523 510 dir_diff = 'E';
jamesheavey 45:4fa8af658523 511 } else {
jamesheavey 45:4fa8af658523 512 dir_diff = 'W';
jamesheavey 45:4fa8af658523 513 }
jamesheavey 45:4fa8af658523 514 } else if( coords_y[prev_node] != coords_y[curr_node] ) {
jamesheavey 45:4fa8af658523 515 if(coords_y[prev_node] - coords_y[curr_node] > 0){
jamesheavey 45:4fa8af658523 516 dir_diff = 'N';
jamesheavey 45:4fa8af658523 517 } else {
jamesheavey 45:4fa8af658523 518 dir_diff = 'S';
jamesheavey 45:4fa8af658523 519 }
jamesheavey 34:63f7c61ee4da 520 }
jamesheavey 45:4fa8af658523 521
jamesheavey 45:4fa8af658523 522 if( dir_diff == 'N' ) {
jamesheavey 45:4fa8af658523 523 explored[curr_node] &= 0b1011;
jamesheavey 45:4fa8af658523 524 }
jamesheavey 45:4fa8af658523 525 else if( dir_diff == 'E' ) {
jamesheavey 45:4fa8af658523 526 explored[curr_node] &= 0b1101;
jamesheavey 34:63f7c61ee4da 527 }
jamesheavey 45:4fa8af658523 528 else if( dir_diff == 'S' ) {
jamesheavey 45:4fa8af658523 529 explored[curr_node] &= 0b1110;
jamesheavey 45:4fa8af658523 530 }
jamesheavey 45:4fa8af658523 531 else if( dir_diff == 'W' ) {
jamesheavey 45:4fa8af658523 532 explored[curr_node] &= 0b0111;
jamesheavey 45:4fa8af658523 533 }
jamesheavey 46:c770365cf5a6 534 if(point[prev_node] == point[d_node]) { break; }
jamesheavey 34:63f7c61ee4da 535 }
jamesheavey 45:4fa8af658523 536 }
jamesheavey 45:4fa8af658523 537
jamesheavey 45:4fa8af658523 538 else if( before == true ) {
jamesheavey 45:4fa8af658523 539 for(int j = before_index; j <= path_length; j++) {
jamesheavey 45:4fa8af658523 540 curr_node = path_to_point_index(looped_path[j]);
jamesheavey 45:4fa8af658523 541
jamesheavey 45:4fa8af658523 542 int next_node = path_to_point_index(looped_path[j+1]);
jamesheavey 45:4fa8af658523 543 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 45:4fa8af658523 544 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 45:4fa8af658523 545 dir_diff = 'E';
jamesheavey 45:4fa8af658523 546 } else {
jamesheavey 45:4fa8af658523 547 dir_diff = 'W';
jamesheavey 45:4fa8af658523 548 }
jamesheavey 45:4fa8af658523 549 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 45:4fa8af658523 550 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 45:4fa8af658523 551 dir_diff = 'N';
jamesheavey 45:4fa8af658523 552 } else {
jamesheavey 45:4fa8af658523 553 dir_diff = 'S';
jamesheavey 45:4fa8af658523 554 }
jamesheavey 45:4fa8af658523 555 }
jamesheavey 45:4fa8af658523 556
jamesheavey 45:4fa8af658523 557 if( dir_diff == 'N' ) {
jamesheavey 45:4fa8af658523 558 explored[curr_node] &= 0b1011;
jamesheavey 45:4fa8af658523 559 }
jamesheavey 45:4fa8af658523 560 else if( dir_diff == 'E' ) {
jamesheavey 45:4fa8af658523 561 explored[curr_node] &= 0b1101;
jamesheavey 45:4fa8af658523 562 }
jamesheavey 45:4fa8af658523 563 else if( dir_diff == 'S' ) {
jamesheavey 45:4fa8af658523 564 explored[curr_node] &= 0b1110;
jamesheavey 45:4fa8af658523 565 }
jamesheavey 45:4fa8af658523 566 else if( dir_diff == 'W' ) {
jamesheavey 45:4fa8af658523 567 explored[curr_node] &= 0b0111;
jamesheavey 45:4fa8af658523 568 }
jamesheavey 46:c770365cf5a6 569 if(point[next_node] == point[d_node]) { break; }
jamesheavey 34:63f7c61ee4da 570 }
jamesheavey 34:63f7c61ee4da 571 }
jamesheavey 34:63f7c61ee4da 572 }
jamesheavey 28:63ff8290964a 573 }
jamesheavey 28:63ff8290964a 574
jamesheavey 44:73a83a0fa467 575 void return_to_start()
jamesheavey 44:73a83a0fa467 576 {
jamesheavey 44:73a83a0fa467 577
jamesheavey 44:73a83a0fa467 578 // for bt, check the path between the node before the desired node and after
jamesheavey 44:73a83a0fa467 579 // which ever is shorter, do the 0 on that path
jamesheavey 44:73a83a0fa467 580
jamesheavey 44:73a83a0fa467 581 // for return to start just do it from the start
jamesheavey 44:73a83a0fa467 582 int pointer;
jamesheavey 44:73a83a0fa467 583 char dir_diff;
jamesheavey 44:73a83a0fa467 584
jamesheavey 44:73a83a0fa467 585 for( int i = 0; i <= path_length; i++ ) {
jamesheavey 44:73a83a0fa467 586 if( looped_path[i] == point[curr_index] ) {
jamesheavey 44:73a83a0fa467 587 pointer = i;
jamesheavey 44:73a83a0fa467 588 break;
jamesheavey 44:73a83a0fa467 589 }
jamesheavey 44:73a83a0fa467 590 }
jamesheavey 44:73a83a0fa467 591
jamesheavey 44:73a83a0fa467 592 for(int j = pointer; j >= 0; j--) {
jamesheavey 44:73a83a0fa467 593 int curr_node = path_to_point_index(looped_path[j]);
jamesheavey 44:73a83a0fa467 594 int prev_node = path_to_point_index(looped_path[j-1]);
jamesheavey 44:73a83a0fa467 595 if(coords_x[prev_node] != coords_x[curr_node]) {
jamesheavey 44:73a83a0fa467 596 if(coords_x[prev_node] - coords_x[curr_node] > 0){
jamesheavey 44:73a83a0fa467 597 dir_diff = 'E';
jamesheavey 44:73a83a0fa467 598 } else {
jamesheavey 44:73a83a0fa467 599 dir_diff = 'W';
jamesheavey 44:73a83a0fa467 600 }
jamesheavey 44:73a83a0fa467 601 } else if( coords_y[prev_node] != coords_y[curr_node] ) {
jamesheavey 44:73a83a0fa467 602 if(coords_y[prev_node] - coords_y[curr_node] > 0){
jamesheavey 44:73a83a0fa467 603 dir_diff = 'N';
jamesheavey 44:73a83a0fa467 604 } else {
jamesheavey 44:73a83a0fa467 605 dir_diff = 'S';
jamesheavey 44:73a83a0fa467 606 }
jamesheavey 44:73a83a0fa467 607 }
jamesheavey 44:73a83a0fa467 608
jamesheavey 44:73a83a0fa467 609 if( dir_diff == 'N' ) {
jamesheavey 44:73a83a0fa467 610 explored[curr_node] &= 0b1011;
jamesheavey 44:73a83a0fa467 611 }
jamesheavey 44:73a83a0fa467 612 else if( dir_diff == 'E' ) {
jamesheavey 44:73a83a0fa467 613 explored[curr_node] &= 0b1101;
jamesheavey 44:73a83a0fa467 614 }
jamesheavey 44:73a83a0fa467 615 else if( dir_diff == 'S' ) {
jamesheavey 44:73a83a0fa467 616 explored[curr_node] &= 0b1110;
jamesheavey 44:73a83a0fa467 617 }
jamesheavey 44:73a83a0fa467 618 else if( dir_diff == 'W' ) {
jamesheavey 44:73a83a0fa467 619 explored[curr_node] &= 0b0111;
jamesheavey 44:73a83a0fa467 620 }
jamesheavey 44:73a83a0fa467 621 }
jamesheavey 44:73a83a0fa467 622
jamesheavey 44:73a83a0fa467 623 while( point[curr_index] != 0 ) {
jamesheavey 44:73a83a0fa467 624
jamesheavey 44:73a83a0fa467 625 if ( t_restart ){ // only start the timer if it isnt already started
jamesheavey 44:73a83a0fa467 626 t_coord.start();
jamesheavey 44:73a83a0fa467 627 t_restart = false;
jamesheavey 44:73a83a0fa467 628 }
jamesheavey 44:73a83a0fa467 629
jamesheavey 44:73a83a0fa467 630 follow_line();
jamesheavey 44:73a83a0fa467 631
jamesheavey 44:73a83a0fa467 632 if ( junction_detect() ) {
jamesheavey 44:73a83a0fa467 633 path_length++; // increment the path position index
jamesheavey 44:73a83a0fa467 634
jamesheavey 44:73a83a0fa467 635 float time = t_coord.read();
jamesheavey 44:73a83a0fa467 636 int dist_est = ceil(time*2);
jamesheavey 44:73a83a0fa467 637 t_coord.stop();
jamesheavey 44:73a83a0fa467 638 t_coord.reset();
jamesheavey 44:73a83a0fa467 639 t_restart = true;
jamesheavey 44:73a83a0fa467 640
jamesheavey 44:73a83a0fa467 641 if (dir == 'N'){ curr_coords[1] += dist_est; } // y coord
jamesheavey 44:73a83a0fa467 642 if (dir == 'E'){ curr_coords[0] += dist_est; } // x coord
jamesheavey 44:73a83a0fa467 643 if (dir == 'S'){ curr_coords[1] -= dist_est; }
jamesheavey 44:73a83a0fa467 644 if (dir == 'W'){ curr_coords[0] -= dist_est; }
jamesheavey 44:73a83a0fa467 645
jamesheavey 44:73a83a0fa467 646 update_index();
jamesheavey 44:73a83a0fa467 647 looped_path[path_length] = point[curr_index];
jamesheavey 44:73a83a0fa467 648 choose_turn();
jamesheavey 44:73a83a0fa467 649 }
jamesheavey 44:73a83a0fa467 650
jamesheavey 44:73a83a0fa467 651 wait(1/50);
jamesheavey 44:73a83a0fa467 652 }
jamesheavey 47:745c8846a92d 653
jamesheavey 47:745c8846a92d 654 //// basic requirement is to add a valid path from the curr node to the start node
jamesheavey 47:745c8846a92d 655 // // if possible traverse this path back to the start then end
jamesheavey 47:745c8846a92d 656 //
jamesheavey 47:745c8846a92d 657 // // create a sub array of nodes between start and curr node
jamesheavey 47:745c8846a92d 658 // // remove all dead ends and repeated nodes
jamesheavey 47:745c8846a92d 659 // // add that array to the end of the path
jamesheavey 47:745c8846a92d 660 //
jamesheavey 47:745c8846a92d 661 // char sub_path[100];
jamesheavey 47:745c8846a92d 662 // char prev_nodes[100];
jamesheavey 47:745c8846a92d 663 // int prev_length = 0;
jamesheavey 47:745c8846a92d 664 // int sub_length = 0;
jamesheavey 47:745c8846a92d 665 // int index1;
jamesheavey 47:745c8846a92d 666 // int index2;
jamesheavey 47:745c8846a92d 667 //
jamesheavey 47:745c8846a92d 668 // for(int i = 0; i <= path_length; i++) {
jamesheavey 47:745c8846a92d 669 // sub_path[i] = looped_path[i];
jamesheavey 47:745c8846a92d 670 // if( looped_path[i] == point[curr_index] ) { sub_length = i; break; }
jamesheavey 47:745c8846a92d 671 // }
jamesheavey 47:745c8846a92d 672 //
jamesheavey 47:745c8846a92d 673 // bool error = true;
jamesheavey 47:745c8846a92d 674 //
jamesheavey 47:745c8846a92d 675 // while (error) {
jamesheavey 47:745c8846a92d 676 // for(int i = 0; i <= sub_length; i++) {
jamesheavey 47:745c8846a92d 677 // bool in_prev = false;
jamesheavey 47:745c8846a92d 678 // for( int l = 0; l<=prev_length; l++ ) {
jamesheavey 47:745c8846a92d 679 // if( sub_path[i] == prev_nodes[l] ) { in_prev = true; }
jamesheavey 47:745c8846a92d 680 // }
jamesheavey 47:745c8846a92d 681 // if( in_prev ) {
jamesheavey 47:745c8846a92d 682 // index1 = i;
jamesheavey 47:745c8846a92d 683 // for(int j = 0; j <= sub_length; j++){
jamesheavey 47:745c8846a92d 684 // if(sub_path[j] == sub_path[index1]) {
jamesheavey 47:745c8846a92d 685 // index2 = j;
jamesheavey 47:745c8846a92d 686 // break;
jamesheavey 47:745c8846a92d 687 // }
jamesheavey 47:745c8846a92d 688 // }
jamesheavey 47:745c8846a92d 689 // break;
jamesheavey 47:745c8846a92d 690 // } else if( sub_path[i] != NULL ) {
jamesheavey 47:745c8846a92d 691 // prev_length++;
jamesheavey 47:745c8846a92d 692 // prev_nodes[prev_length] = sub_path[i];
jamesheavey 47:745c8846a92d 693 // }
jamesheavey 47:745c8846a92d 694 // }
jamesheavey 47:745c8846a92d 695 //
jamesheavey 47:745c8846a92d 696 // // set all nodes between the indexes to null
jamesheavey 47:745c8846a92d 697 // for( int k = index1+1; k <= index2; k++ ) {
jamesheavey 47:745c8846a92d 698 // sub_path[k] = NULL;
jamesheavey 47:745c8846a92d 699 // }
jamesheavey 47:745c8846a92d 700 // int real_len = 0;
jamesheavey 47:745c8846a92d 701 // for( int x = 0; x <= sub_length; x++ ) {
jamesheavey 47:745c8846a92d 702 // if( sub_path[x] != NULL ) { real_len++; }
jamesheavey 47:745c8846a92d 703 // }
jamesheavey 47:745c8846a92d 704 //
jamesheavey 47:745c8846a92d 705 // if( prev_length == real_len ) { error = false; }
jamesheavey 47:745c8846a92d 706 //
jamesheavey 47:745c8846a92d 707 // }
jamesheavey 47:745c8846a92d 708 //
jamesheavey 47:745c8846a92d 709 // leds = 0b1111;
jamesheavey 47:745c8846a92d 710 // for(int m = 0; m <= sub_length; m++) {
jamesheavey 47:745c8846a92d 711 // leds = ~leds;
jamesheavey 47:745c8846a92d 712 // char buffer[1];
jamesheavey 47:745c8846a92d 713 // sprintf(buffer,"%d", sub_path[m]);
jamesheavey 47:745c8846a92d 714 // robot.lcd_print(buffer,1);
jamesheavey 47:745c8846a92d 715 // wait(3);
jamesheavey 47:745c8846a92d 716 // robot.lcd_clear();
jamesheavey 47:745c8846a92d 717 // }
jamesheavey 47:745c8846a92d 718 // // print these to check
jamesheavey 47:745c8846a92d 719 // // add inverted sub path to the real path and increment the path length by real_len -1
jamesheavey 44:73a83a0fa467 720 }
jamesheavey 44:73a83a0fa467 721
jamesheavey 10:691c02b352cb 722 void follow_line()
jamesheavey 0:df5216b20861 723 {
jamesheavey 10:691c02b352cb 724 robot.scan();
jamesheavey 10:691c02b352cb 725 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 726
jamesheavey 15:6c461501d12d 727 leds = 0b0110;
jamesheavey 15:6c461501d12d 728
jamesheavey 10:691c02b352cb 729 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 730 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 731 integral += proportional;
jamesheavey 10:691c02b352cb 732 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 733
jamesheavey 10:691c02b352cb 734 // calculate motor correction
jamesheavey 10:691c02b352cb 735 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 736
jamesheavey 10:691c02b352cb 737 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 738 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 739 motor_correction = speed;
jamesheavey 10:691c02b352cb 740 }
jamesheavey 10:691c02b352cb 741 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 742 motor_correction = -speed;
jamesheavey 10:691c02b352cb 743 }
jamesheavey 0:df5216b20861 744
jamesheavey 10:691c02b352cb 745 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 746 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 747 } else {
jamesheavey 10:691c02b352cb 748 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 749 }
jamesheavey 18:991658b628fc 750
jamesheavey 19:4c08275cb3c9 751 // read_encoders();
jamesheavey 19:4c08275cb3c9 752 // if (encoder[0] > 3100) { dist_est_1 += 1; } // going to have to reset these dist estimates every junction (in the if (junc_detect()) statement)
jamesheavey 19:4c08275cb3c9 753 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 754 }
jamesheavey 0:df5216b20861 755
jamesheavey 14:87052bb35211 756 bool junction_detect()
jamesheavey 6:c10b367747a0 757 {
jamesheavey 20:5cf6a378801d 758 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 759 return true;
jamesheavey 20:5cf6a378801d 760 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 761 return true;
jamesheavey 14:87052bb35211 762 } else {
jamesheavey 14:87052bb35211 763 return false;
jamesheavey 14:87052bb35211 764 }
jamesheavey 14:87052bb35211 765 }
jamesheavey 14:87052bb35211 766
jamesheavey 14:87052bb35211 767 char junction_logic()
jamesheavey 14:87052bb35211 768 {
jamesheavey 14:87052bb35211 769 bool straight = false;
jamesheavey 7:7fefd782532d 770 bool left = false;
jamesheavey 6:c10b367747a0 771 bool right = false;
jamesheavey 9:952586accbf9 772 bool goal = false;
jamesheavey 7:7fefd782532d 773
jamesheavey 20:5cf6a378801d 774 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 775 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 776 robot.forward(speed);
jamesheavey 9:952586accbf9 777 robot.scan();
jamesheavey 20:5cf6a378801d 778 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 779 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 780 }
jamesheavey 7:7fefd782532d 781
jamesheavey 20:5cf6a378801d 782 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 783 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 784 robot.scan();
jamesheavey 20:5cf6a378801d 785 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 786 goal = true;
jamesheavey 12:d80399686f32 787 }
jamesheavey 13:bd271266e161 788 }
jamesheavey 14:87052bb35211 789
jamesheavey 14:87052bb35211 790 robot.scan();
jamesheavey 14:87052bb35211 791
jamesheavey 20:5cf6a378801d 792 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 793 straight = true;
jamesheavey 14:87052bb35211 794 }
jamesheavey 14:87052bb35211 795
jamesheavey 20:5cf6a378801d 796 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 797 return 'B';
jamesheavey 6:c10b367747a0 798 }
jamesheavey 7:7fefd782532d 799
jamesheavey 9:952586accbf9 800 if (goal) {
jamesheavey 9:952586accbf9 801 return 'G';
jamesheavey 9:952586accbf9 802 } else if (left) {
jamesheavey 9:952586accbf9 803 return 'L';
jamesheavey 14:87052bb35211 804 } else if (straight) {
jamesheavey 14:87052bb35211 805 return 'S';
jamesheavey 9:952586accbf9 806 } else if (right) {
jamesheavey 9:952586accbf9 807 return 'R';
jamesheavey 6:c10b367747a0 808 } else {
jamesheavey 9:952586accbf9 809 return 'S';
jamesheavey 9:952586accbf9 810 }
jamesheavey 6:c10b367747a0 811 }
jamesheavey 6:c10b367747a0 812
jamesheavey 0:df5216b20861 813
jamesheavey 14:87052bb35211 814 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 815 {
jamesheavey 1:79219d0a33c8 816 switch(turn) {
jamesheavey 5:ae417756235a 817 case 'G':
jamesheavey 5:ae417756235a 818 goal();
jamesheavey 1:79219d0a33c8 819 case 'L':
jamesheavey 1:79219d0a33c8 820 left();
jamesheavey 5:ae417756235a 821 break;
jamesheavey 1:79219d0a33c8 822 case 'S':
jamesheavey 1:79219d0a33c8 823 break;
jamesheavey 1:79219d0a33c8 824 case 'R':
jamesheavey 1:79219d0a33c8 825 right();
jamesheavey 5:ae417756235a 826 break;
jamesheavey 1:79219d0a33c8 827 case 'B':
jamesheavey 1:79219d0a33c8 828 back();
jamesheavey 5:ae417756235a 829 break;
jamesheavey 1:79219d0a33c8 830 }
jamesheavey 1:79219d0a33c8 831 }
jamesheavey 1:79219d0a33c8 832
jamesheavey 0:df5216b20861 833 void left()
jamesheavey 0:df5216b20861 834 {
jamesheavey 1:79219d0a33c8 835 leds = 0b1100;
jamesheavey 3:a5e06482462e 836
jamesheavey 20:5cf6a378801d 837 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 838
jamesheavey 39:005ad4610152 839 robot.spin_left(TURN_SPEED);
jamesheavey 39:005ad4610152 840 wait(0.2);
jamesheavey 3:a5e06482462e 841
jamesheavey 20:5cf6a378801d 842 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 843
jamesheavey 20:5cf6a378801d 844 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 845 }
jamesheavey 1:79219d0a33c8 846
jamesheavey 1:79219d0a33c8 847 void right()
jamesheavey 1:79219d0a33c8 848 {
jamesheavey 1:79219d0a33c8 849 leds = 0b0011;
jamesheavey 5:ae417756235a 850
jamesheavey 20:5cf6a378801d 851 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 852
jamesheavey 20:5cf6a378801d 853 robot.spin_right(TURN_SPEED);
jamesheavey 39:005ad4610152 854 wait(0.2);
jamesheavey 3:a5e06482462e 855
jamesheavey 20:5cf6a378801d 856 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 857
jamesheavey 20:5cf6a378801d 858 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 859 }
jamesheavey 0:df5216b20861 860
jamesheavey 0:df5216b20861 861 void back()
jamesheavey 0:df5216b20861 862 {
jamesheavey 1:79219d0a33c8 863 leds = 0b1111;
jamesheavey 32:598bedb22c7c 864 // robot.reverse(speed);
jamesheavey 32:598bedb22c7c 865 // wait(0.1);
jamesheavey 20:5cf6a378801d 866 robot.spin_right(TURN_SPEED);
jamesheavey 41:2b6b73dd897c 867 if(loop_check == false) { // works better for looped
jamesheavey 42:69bffd3679bf 868 wait(0.7);
jamesheavey 41:2b6b73dd897c 869 }
jamesheavey 41:2b6b73dd897c 870 else {
jamesheavey 41:2b6b73dd897c 871 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 872
jamesheavey 41:2b6b73dd897c 873 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 874 }
jamesheavey 0:df5216b20861 875 }
jamesheavey 1:79219d0a33c8 876
jamesheavey 2:940e46e21353 877 void simplify()
jamesheavey 1:79219d0a33c8 878 {
jamesheavey 2:940e46e21353 879 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 880 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 881 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 882
jamesheavey 10:691c02b352cb 883 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 884 int angle_change = 0;
jamesheavey 2:940e46e21353 885
jamesheavey 10:691c02b352cb 886 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 887 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 888 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 889 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 890 }
jamesheavey 4:38c29dbc5953 891
jamesheavey 4:38c29dbc5953 892 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 893
jamesheavey 4:38c29dbc5953 894 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 895 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 896 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 897 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 898
jamesheavey 4:38c29dbc5953 899 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 900
jamesheavey 4:38c29dbc5953 901 path_length -= 2;
jamesheavey 2:940e46e21353 902 }
jamesheavey 5:ae417756235a 903 }
jamesheavey 5:ae417756235a 904
jamesheavey 10:691c02b352cb 905 void goal()
jamesheavey 10:691c02b352cb 906 {
jamesheavey 10:691c02b352cb 907 invert_path();
jamesheavey 11:c3299aca7d8f 908
jamesheavey 11:c3299aca7d8f 909 leds = 0b0000;
jamesheavey 10:691c02b352cb 910
jamesheavey 10:691c02b352cb 911 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 912 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 913
jamesheavey 11:c3299aca7d8f 914 while(1) {
jamesheavey 15:6c461501d12d 915 int pointer = 0;
jamesheavey 15:6c461501d12d 916
jamesheavey 15:6c461501d12d 917 robot.stop();
jamesheavey 15:6c461501d12d 918
jamesheavey 15:6c461501d12d 919 leds = 0b1001;
jamesheavey 15:6c461501d12d 920 wait(0.2);
jamesheavey 15:6c461501d12d 921 leds = 0b0110;
jamesheavey 15:6c461501d12d 922 wait(0.2);
jamesheavey 15:6c461501d12d 923
jamesheavey 15:6c461501d12d 924 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 925 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 926 wait(0.05);
jamesheavey 15:6c461501d12d 927
jamesheavey 20:5cf6a378801d 928 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 929 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 930 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 931 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 932
jamesheavey 15:6c461501d12d 933 robot.stop();
jamesheavey 15:6c461501d12d 934
jamesheavey 15:6c461501d12d 935 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 936 follow_line();
jamesheavey 15:6c461501d12d 937
jamesheavey 15:6c461501d12d 938 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 939 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 940 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 941 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 942 robot.forward(speed);
jamesheavey 15:6c461501d12d 943 leds = 0b1010;
jamesheavey 20:5cf6a378801d 944 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 945 }
jamesheavey 15:6c461501d12d 946 pointer++;
jamesheavey 15:6c461501d12d 947 }
jamesheavey 15:6c461501d12d 948 }
jamesheavey 15:6c461501d12d 949
jamesheavey 15:6c461501d12d 950 back();
jamesheavey 12:d80399686f32 951
jamesheavey 11:c3299aca7d8f 952 robot.stop();
jamesheavey 20:5cf6a378801d 953 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 954 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 955 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 956 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 957
jamesheavey 14:87052bb35211 958 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 959
jamesheavey 20:5cf6a378801d 960 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 961 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 962
jamesheavey 15:6c461501d12d 963 pointer = 0;
jamesheavey 15:6c461501d12d 964
jamesheavey 11:c3299aca7d8f 965 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 966 wait(0.2);
jamesheavey 11:c3299aca7d8f 967 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 968 wait(0.2);
jamesheavey 11:c3299aca7d8f 969 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 970 wait(0.2);
jamesheavey 11:c3299aca7d8f 971 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 972 wait(0.2);
jamesheavey 11:c3299aca7d8f 973
jamesheavey 11:c3299aca7d8f 974 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 975 follow_line();
jamesheavey 11:c3299aca7d8f 976
jamesheavey 14:87052bb35211 977 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 978 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 979 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 980 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 981 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 982 leds = 0b1010;
jamesheavey 20:5cf6a378801d 983 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 984 }
jamesheavey 11:c3299aca7d8f 985 pointer++;
jamesheavey 10:691c02b352cb 986 }
jamesheavey 10:691c02b352cb 987 }
jamesheavey 5:ae417756235a 988 }
jamesheavey 5:ae417756235a 989 }
jamesheavey 5:ae417756235a 990
jamesheavey 29:ecf497c3fdc0 991 void looped_goal()
jamesheavey 29:ecf497c3fdc0 992 {
jamesheavey 44:73a83a0fa467 993 // return to start (adding nodes to path)
jamesheavey 44:73a83a0fa467 994 // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
jamesheavey 44:73a83a0fa467 995 // loop between start and goal
jamesheavey 34:63f7c61ee4da 996 robot.stop();
jamesheavey 45:4fa8af658523 997 //return_to_start();
jamesheavey 45:4fa8af658523 998 // simplify_looped();
jamesheavey 34:63f7c61ee4da 999
jamesheavey 44:73a83a0fa467 1000 while( button_enter.read() == 1 ) {
jamesheavey 29:ecf497c3fdc0 1001 leds = 0b1001;
jamesheavey 29:ecf497c3fdc0 1002 wait(0.2);
jamesheavey 29:ecf497c3fdc0 1003 leds = 0b0110;
jamesheavey 29:ecf497c3fdc0 1004 wait(0.2);
jamesheavey 29:ecf497c3fdc0 1005 }
jamesheavey 29:ecf497c3fdc0 1006 }
jamesheavey 29:ecf497c3fdc0 1007
jamesheavey 10:691c02b352cb 1008 void invert_path()
jamesheavey 5:ae417756235a 1009 {
jamesheavey 10:691c02b352cb 1010 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 1011 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 1012 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 1013 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 1014 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 1015 }
jamesheavey 28:63ff8290964a 1016 }
jamesheavey 37:2967f3f9c936 1017
jamesheavey 37:2967f3f9c936 1018 void print_data(char *word)
jamesheavey 37:2967f3f9c936 1019 {
jamesheavey 37:2967f3f9c936 1020 robot.lcd_clear();
jamesheavey 37:2967f3f9c936 1021 robot.lcd_print(word,10);
jamesheavey 37:2967f3f9c936 1022 robot.stop();
jamesheavey 37:2967f3f9c936 1023 wait(2);
jamesheavey 37:2967f3f9c936 1024
jamesheavey 37:2967f3f9c936 1025 char buffer1[2];
jamesheavey 37:2967f3f9c936 1026 char buffer2[2];
jamesheavey 37:2967f3f9c936 1027 char buffer3[2];
jamesheavey 37:2967f3f9c936 1028 robot.lcd_clear();
jamesheavey 38:b5b06625d06e 1029 // sprintf(buffer1,"%x",type[curr_index]);
jamesheavey 38:b5b06625d06e 1030 // sprintf(buffer2,"%x",explored[curr_index]);
jamesheavey 37:2967f3f9c936 1031
jamesheavey 38:b5b06625d06e 1032 sprintf(buffer1,"%d",curr_coords[0]);
jamesheavey 38:b5b06625d06e 1033 sprintf(buffer2,"%d",curr_coords[1]);
jamesheavey 37:2967f3f9c936 1034 sprintf(buffer3,"%d",curr_index);
jamesheavey 37:2967f3f9c936 1035 robot.lcd_print(buffer1,2);
jamesheavey 37:2967f3f9c936 1036 robot.lcd_goto_xy(0,1);
jamesheavey 37:2967f3f9c936 1037 robot.lcd_print(buffer2,2);
jamesheavey 37:2967f3f9c936 1038 robot.lcd_goto_xy(5,0);
jamesheavey 37:2967f3f9c936 1039 char *b = &dir;
jamesheavey 37:2967f3f9c936 1040 robot.lcd_print(b,2);
jamesheavey 37:2967f3f9c936 1041 robot.lcd_goto_xy(5,1);
jamesheavey 37:2967f3f9c936 1042 robot.lcd_print(buffer3,2);
jamesheavey 37:2967f3f9c936 1043
jamesheavey 37:2967f3f9c936 1044 robot.stop();
jamesheavey 37:2967f3f9c936 1045 wait(2);
jamesheavey 37:2967f3f9c936 1046 }