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@65:2103a02d72bc, 2020-05-08 (annotated)

Committer:
jamesheavey
Date:
Fri May 08 15:14:17 2020 +0000
Revision:
65:2103a02d72bc
Parent:
64:a58d8538132a
Child:
66:3130be689176
goal loop works, shorten added to try simplify the goal path function, doesnt work yet when implemented

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 56:1cd434b28322 171 explored[total_points] = 0b0000;
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 32:598bedb22c7c 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 23:71e84953b3f3 222
jamesheavey 26:582560881379 223
jamesheavey 26:582560881379 224 // 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 225
jamesheavey 21:54ea75f7984f 226 }
jamesheavey 32:598bedb22c7c 227
jamesheavey 31:1e6d0ef05996 228
jamesheavey 31:1e6d0ef05996 229 // robot.lcd_clear();
jamesheavey 31:1e6d0ef05996 230 // char *b = &dir;
jamesheavey 31:1e6d0ef05996 231 // robot.lcd_print(b,1);
jamesheavey 31:1e6d0ef05996 232
jamesheavey 31:1e6d0ef05996 233
jamesheavey 20:5cf6a378801d 234 //robot.display_data();
jamesheavey 20:5cf6a378801d 235 }
jamesheavey 21:54ea75f7984f 236
jamesheavey 23:71e84953b3f3 237 bool coord_check()
jamesheavey 23:71e84953b3f3 238 {
jamesheavey 23:71e84953b3f3 239 bool result = true;
jamesheavey 23:71e84953b3f3 240 //returns true if the current coords dont match a previous point
jamesheavey 23:71e84953b3f3 241 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 242 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 23:71e84953b3f3 243 result = false;
jamesheavey 23:71e84953b3f3 244 }
jamesheavey 23:71e84953b3f3 245 }
jamesheavey 23:71e84953b3f3 246
jamesheavey 23:71e84953b3f3 247 return result;
jamesheavey 23:71e84953b3f3 248 }
jamesheavey 23:71e84953b3f3 249
jamesheavey 25:7523239a2fc1 250 void update_index() // update index (pointer to current point/type/coords/explored)
jamesheavey 23:71e84953b3f3 251 {
jamesheavey 23:71e84953b3f3 252 // checks the curr_coords againts the coords array, returns the index to relate to the point array
jamesheavey 23:71e84953b3f3 253 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 254 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 25:7523239a2fc1 255 curr_index = i;
jamesheavey 23:71e84953b3f3 256 }
jamesheavey 23:71e84953b3f3 257 }
jamesheavey 23:71e84953b3f3 258 }
jamesheavey 23:71e84953b3f3 259
jamesheavey 29:ecf497c3fdc0 260 int path_to_point_index( int path_point )
jamesheavey 29:ecf497c3fdc0 261 {
jamesheavey 29:ecf497c3fdc0 262 for(int i = 0; i <= total_points; i++) {
jamesheavey 29:ecf497c3fdc0 263 if(path_point == point[i]) {
jamesheavey 29:ecf497c3fdc0 264 return i;
jamesheavey 29:ecf497c3fdc0 265 }
jamesheavey 29:ecf497c3fdc0 266 }
jamesheavey 29:ecf497c3fdc0 267
jamesheavey 29:ecf497c3fdc0 268 return curr_index; // default
jamesheavey 29:ecf497c3fdc0 269 }
jamesheavey 29:ecf497c3fdc0 270
jamesheavey 21:54ea75f7984f 271 void node_logic()
jamesheavey 21:54ea75f7984f 272 {
jamesheavey 24:adb946be4ce5 273 // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
jamesheavey 25:7523239a2fc1 274
jamesheavey 25:7523239a2fc1 275 // first determine what turns are available relative to the robots current direction (left, straight etc.)
jamesheavey 25:7523239a2fc1 276 // convert these relative available turns into available absolute diections (N,E etc.)
jamesheavey 25:7523239a2fc1 277 // set _type to the appropriate value based on available directions (including entry direction = opposite of current)
jamesheavey 25:7523239a2fc1 278 // set _explored entry path as 1
jamesheavey 25:7523239a2fc1 279 // set type[total_points] = _type; & explored[total_points] = _explored;
jamesheavey 25:7523239a2fc1 280
jamesheavey 21:54ea75f7984f 281 bool north = false;
jamesheavey 21:54ea75f7984f 282 bool south = false;
jamesheavey 21:54ea75f7984f 283 bool east = false;
jamesheavey 21:54ea75f7984f 284 bool west = false;
jamesheavey 21:54ea75f7984f 285
jamesheavey 21:54ea75f7984f 286 bool left = false;
jamesheavey 21:54ea75f7984f 287 bool straight = false;
jamesheavey 21:54ea75f7984f 288 bool right = false;
jamesheavey 43:ec047ba15db1 289 bool goal = false;
jamesheavey 21:54ea75f7984f 290
jamesheavey 25:7523239a2fc1 291 int _type = 0b0000;
jamesheavey 25:7523239a2fc1 292 int _explored = 0b0000;
jamesheavey 21:54ea75f7984f 293
jamesheavey 21:54ea75f7984f 294 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 21:54ea75f7984f 295 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 21:54ea75f7984f 296 robot.forward(speed);
jamesheavey 21:54ea75f7984f 297 robot.scan();
jamesheavey 21:54ea75f7984f 298 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 21:54ea75f7984f 299 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 21:54ea75f7984f 300 }
jamesheavey 21:54ea75f7984f 301
jamesheavey 21:54ea75f7984f 302 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 303 wait(0.05); // maybe change or replace w something better
jamesheavey 21:54ea75f7984f 304 robot.scan();
jamesheavey 27:0a3f028f9365 305 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 43:ec047ba15db1 306 goal = true;
jamesheavey 27:0a3f028f9365 307 }
jamesheavey 21:54ea75f7984f 308 }
jamesheavey 21:54ea75f7984f 309 robot.scan();
jamesheavey 21:54ea75f7984f 310
jamesheavey 21:54ea75f7984f 311 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 312 straight = true;
jamesheavey 21:54ea75f7984f 313 }
jamesheavey 21:54ea75f7984f 314 }
jamesheavey 21:54ea75f7984f 315
jamesheavey 43:ec047ba15db1 316 if(goal) {
jamesheavey 43:ec047ba15db1 317 if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 43:ec047ba15db1 318 else if ( dir == 'E' ) { west = true; _explored |= 0b1000; }
jamesheavey 43:ec047ba15db1 319 else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 43:ec047ba15db1 320 else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 43:ec047ba15db1 321
jamesheavey 43:ec047ba15db1 322 if ( west ) { _type |= 0b1000; }
jamesheavey 43:ec047ba15db1 323 if ( north ) { _type |= 0b0100; }
jamesheavey 43:ec047ba15db1 324 if ( east ) { _type |= 0b0010; }
jamesheavey 43:ec047ba15db1 325 if ( south ) { _type |= 0b0001; }
jamesheavey 43:ec047ba15db1 326
jamesheavey 43:ec047ba15db1 327 goal_node = true;
jamesheavey 43:ec047ba15db1 328 }
jamesheavey 21:54ea75f7984f 329
jamesheavey 43:ec047ba15db1 330 else {
jamesheavey 27:0a3f028f9365 331 int angle = 0;
jamesheavey 28:63ff8290964a 332 int reset_ang = 0;
jamesheavey 27:0a3f028f9365 333
jamesheavey 34:63f7c61ee4da 334 if (dir == 'E') { angle = 90; }
jamesheavey 34:63f7c61ee4da 335 else if (dir == 'S') { angle = 180; }
jamesheavey 34:63f7c61ee4da 336 else if (dir == 'W') { angle = 270; }
jamesheavey 34:63f7c61ee4da 337
jamesheavey 34:63f7c61ee4da 338 reset_ang = angle;
jamesheavey 27:0a3f028f9365 339
jamesheavey 27:0a3f028f9365 340 if (left) {
jamesheavey 27:0a3f028f9365 341 angle += 270;
jamesheavey 27:0a3f028f9365 342 angle = angle % 360;
jamesheavey 27:0a3f028f9365 343 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 344 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 345 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 346 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 347 angle = reset_ang;
jamesheavey 27:0a3f028f9365 348 }
jamesheavey 27:0a3f028f9365 349
jamesheavey 27:0a3f028f9365 350 if (right) {
jamesheavey 27:0a3f028f9365 351 angle += 90;
jamesheavey 27:0a3f028f9365 352 angle = angle % 360;
jamesheavey 27:0a3f028f9365 353 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 354 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 355 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 356 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 357 angle = reset_ang;
jamesheavey 27:0a3f028f9365 358 }
jamesheavey 27:0a3f028f9365 359
jamesheavey 27:0a3f028f9365 360 if (straight) {
jamesheavey 27:0a3f028f9365 361 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 362 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 363 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 364 if (angle == 270) { west = true; }
jamesheavey 27:0a3f028f9365 365 }
jamesheavey 27:0a3f028f9365 366
jamesheavey 27:0a3f028f9365 367 if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 368 else if ( dir == 'E' ) { west = true; _explored |= 0b1000; } // this is acc done in choose turn so might not be needed here
jamesheavey 27:0a3f028f9365 369 else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 27:0a3f028f9365 370 else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 27:0a3f028f9365 371
jamesheavey 27:0a3f028f9365 372 if ( west ) { _type |= 0b1000; }
jamesheavey 27:0a3f028f9365 373 if ( north ) { _type |= 0b0100; }
jamesheavey 27:0a3f028f9365 374 if ( east ) { _type |= 0b0010; }
jamesheavey 27:0a3f028f9365 375 if ( south ) { _type |= 0b0001; }
jamesheavey 43:ec047ba15db1 376 }
jamesheavey 27:0a3f028f9365 377
jamesheavey 26:582560881379 378 type[total_points] = _type; // maybe update_index and use curr_index instead of total_points
jamesheavey 25:7523239a2fc1 379 explored[total_points] = _explored;
jamesheavey 21:54ea75f7984f 380
jamesheavey 21:54ea75f7984f 381 }
jamesheavey 21:54ea75f7984f 382
jamesheavey 21:54ea75f7984f 383 void choose_turn()
jamesheavey 21:54ea75f7984f 384 {
jamesheavey 25:7523239a2fc1 385 // look at cuurent coords, find what node we are at
jamesheavey 24:adb946be4ce5 386 // 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 387 // sets the explored of the current node to 1 in whatever path is chosen
jamesheavey 25:7523239a2fc1 388 // also update dir
jamesheavey 37:2967f3f9c936 389
jamesheavey 34:63f7c61ee4da 390 if( dir == 'N' ) { explored[curr_index] |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 391 else if ( dir == 'E' ) { explored[curr_index] |= 0b1000; }
jamesheavey 34:63f7c61ee4da 392 else if ( dir == 'S' ) { explored[curr_index] |= 0b0100; }
jamesheavey 34:63f7c61ee4da 393 else if ( dir == 'W' ) { explored[curr_index] |= 0b0010; }
jamesheavey 39:005ad4610152 394 // print_data("enter junc");
jamesheavey 25:7523239a2fc1 395 int unexp_paths = type[curr_index] & ~( explored[curr_index] ); // produces a binary of 1's in the available unexplored paths
jamesheavey 25:7523239a2fc1 396
jamesheavey 32:598bedb22c7c 397 if (unexp_paths == 0b0000) {
jamesheavey 34:63f7c61ee4da 398 back_track();
jamesheavey 36:1af069be0fdb 399 unexp_paths = type[curr_index] & ~( explored[curr_index] );
jamesheavey 30:d62f122e8d60 400 }
jamesheavey 32:598bedb22c7c 401
jamesheavey 32:598bedb22c7c 402 int curr_angle = 0;
jamesheavey 32:598bedb22c7c 403 if ( dir == 'E' ) { curr_angle = 90;}
jamesheavey 32:598bedb22c7c 404 else if ( dir == 'S' ) { curr_angle = 180;}
jamesheavey 32:598bedb22c7c 405 else if ( dir == 'W' ) { curr_angle = 270;}
jamesheavey 25:7523239a2fc1 406
jamesheavey 32:598bedb22c7c 407 int desired_angle = 0;
jamesheavey 32:598bedb22c7c 408 if ( (unexp_paths & 0b1000) == 0b1000) {
jamesheavey 32:598bedb22c7c 409 desired_angle = 270;
jamesheavey 32:598bedb22c7c 410 dir = 'W';
jamesheavey 32:598bedb22c7c 411 explored[curr_index] |= 0b1000;
jamesheavey 32:598bedb22c7c 412 }
jamesheavey 32:598bedb22c7c 413 else if ( (unexp_paths & 0b0100) == 0b0100) {
jamesheavey 32:598bedb22c7c 414 desired_angle = 0;
jamesheavey 32:598bedb22c7c 415 dir = 'N';
jamesheavey 32:598bedb22c7c 416 explored[curr_index] |= 0b0100;
jamesheavey 25:7523239a2fc1 417 }
jamesheavey 32:598bedb22c7c 418 else if ( (unexp_paths & 0b0010) == 0b0010) {
jamesheavey 32:598bedb22c7c 419 desired_angle = 90;
jamesheavey 32:598bedb22c7c 420 dir = 'E';
jamesheavey 32:598bedb22c7c 421 explored[curr_index] |= 0b0010;
jamesheavey 32:598bedb22c7c 422 }
jamesheavey 32:598bedb22c7c 423 else if ( (unexp_paths & 0b0001) == 0b0001) {
jamesheavey 32:598bedb22c7c 424 desired_angle = 180;
jamesheavey 32:598bedb22c7c 425 dir = 'S';
jamesheavey 32:598bedb22c7c 426 explored[curr_index] |= 0b0001;
jamesheavey 32:598bedb22c7c 427 }
jamesheavey 32:598bedb22c7c 428
jamesheavey 32:598bedb22c7c 429 int turn_angle = (desired_angle - curr_angle + 360) % 360;
jamesheavey 32:598bedb22c7c 430
jamesheavey 39:005ad4610152 431 // robot.lcd_clear();
jamesheavey 39:005ad4610152 432 // robot.lcd_print("turn" , 4);
jamesheavey 39:005ad4610152 433 // wait(2);
jamesheavey 54:5ff74beb043e 434 // robot.stop();
jamesheavey 39:005ad4610152 435 // robot.lcd_clear();
jamesheavey 39:005ad4610152 436 // if( turn_angle == 0) { robot.lcd_print("S",1); }
jamesheavey 39:005ad4610152 437 // else if( turn_angle == 90) { robot.lcd_print("R",1); }
jamesheavey 39:005ad4610152 438 // else if( turn_angle == 180) { robot.lcd_print("B",1); }
jamesheavey 39:005ad4610152 439 // else if( turn_angle == 270) { robot.lcd_print("L",1); }
jamesheavey 39:005ad4610152 440 // wait(2);
jamesheavey 39:005ad4610152 441 //
jamesheavey 39:005ad4610152 442 // print_data("After Turn");
jamesheavey 37:2967f3f9c936 443
jamesheavey 57:9f9067e3f99e 444 if( turn_angle == 0) { robot.forward(speed); wait(0.1); turn_select('S'); }
jamesheavey 58:cb32aa0f4116 445 else if( turn_angle == 90) { robot.forward(speed); wait(0.03); turn_select('R'); }
jamesheavey 48:76cf4521d342 446 else if( turn_angle == 180) { turn_select('B'); }
jamesheavey 58:cb32aa0f4116 447 else if( turn_angle == 270) { robot.forward(speed); wait(0.03); turn_select('L'); }
jamesheavey 21:54ea75f7984f 448 }
jamesheavey 21:54ea75f7984f 449
jamesheavey 28:63ff8290964a 450 void back_track()
jamesheavey 28:63ff8290964a 451 {
jamesheavey 33:9fa9e09f2e8f 452 // 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 453 // also if no nodes have unexplored paths set complete to true
jamesheavey 29:ecf497c3fdc0 454
jamesheavey 62:69de8a0d2ced 455 bool fully_explored = true;
jamesheavey 62:69de8a0d2ced 456
jamesheavey 48:76cf4521d342 457 int d_node; // an index to the most recent (desired) node with unexplored paths
jamesheavey 29:ecf497c3fdc0 458
jamesheavey 34:63f7c61ee4da 459 for(int i = total_points; i >= 0; i--) { // start from the most recently discovered node
jamesheavey 58:cb32aa0f4116 460 if( explored[i] != type[i] ) {
jamesheavey 59:11e31ce4d675 461 fully_explored = false;
jamesheavey 48:76cf4521d342 462 d_node = i;
jamesheavey 34:63f7c61ee4da 463 break;
jamesheavey 34:63f7c61ee4da 464 }
jamesheavey 29:ecf497c3fdc0 465 }
jamesheavey 29:ecf497c3fdc0 466
jamesheavey 65:2103a02d72bc 467 if( fully_explored == true && first_g == true ) { first_g = false; looped_goal(); }
jamesheavey 34:63f7c61ee4da 468
jamesheavey 34:63f7c61ee4da 469 else {
jamesheavey 34:63f7c61ee4da 470 // compare node coordinates to previous node coordinates
jamesheavey 34:63f7c61ee4da 471 // determine which direction the previous node is compared to current
jamesheavey 34:63f7c61ee4da 472 // set the current nodes direction path to 0 and the opposite direction path to 0
jamesheavey 34:63f7c61ee4da 473 // decrement the count (setting the previous as current node and the next previous as previous)
jamesheavey 34:63f7c61ee4da 474 // when previous node == point[pointer1] break
jamesheavey 34:63f7c61ee4da 475 // choose turn should then do all those turns and arrive at correct node
jamesheavey 48:76cf4521d342 476
jamesheavey 48:76cf4521d342 477 // check if the current node exists before the discovery of the desired node
jamesheavey 48:76cf4521d342 478 // if it does, check the number of nodes between before and after
jamesheavey 48:76cf4521d342 479 // which ever is shorter, set the 0 of those nodes
jamesheavey 48:76cf4521d342 480
jamesheavey 55:18ea9529afb2 481 // robot.stop();
jamesheavey 62:69de8a0d2ced 482 // robot.lcd_print("bt",2);
jamesheavey 62:69de8a0d2ced 483 // wait(1);
jamesheavey 62:69de8a0d2ced 484 // robot.lcd_clear();
jamesheavey 62:69de8a0d2ced 485 //
jamesheavey 55:18ea9529afb2 486 // char buf1[2], buf2[2];
jamesheavey 55:18ea9529afb2 487 // sprintf(buf1,"%d",point[d_node]);
jamesheavey 55:18ea9529afb2 488 // sprintf(buf2,"%d",point[curr_index]);
jamesheavey 55:18ea9529afb2 489 // robot.lcd_print(buf1,2);
jamesheavey 55:18ea9529afb2 490 // robot.lcd_goto_xy(0,1);
jamesheavey 55:18ea9529afb2 491 // robot.lcd_print(buf2,2);
jamesheavey 55:18ea9529afb2 492 //
jamesheavey 55:18ea9529afb2 493 // wait(1);
jamesheavey 55:18ea9529afb2 494 // robot.lcd_clear();
jamesheavey 62:69de8a0d2ced 495
jamesheavey 55:18ea9529afb2 496 bool check = dead_end_removal(point[d_node], point[curr_index]);
jamesheavey 53:dbb63b181faf 497 char dir_diff;
jamesheavey 53:dbb63b181faf 498
jamesheavey 55:18ea9529afb2 499 if(check) {
jamesheavey 54:5ff74beb043e 500 for(int j = 0; j <= short_length; j++) {
jamesheavey 54:5ff74beb043e 501 int curr_node = path_to_point_index(shortest[j]);
jamesheavey 54:5ff74beb043e 502 int next_node = path_to_point_index(shortest[j+1]);
jamesheavey 54:5ff74beb043e 503 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 54:5ff74beb043e 504 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 54:5ff74beb043e 505 dir_diff = 'E';
jamesheavey 54:5ff74beb043e 506 } else {
jamesheavey 54:5ff74beb043e 507 dir_diff = 'W';
jamesheavey 54:5ff74beb043e 508 }
jamesheavey 54:5ff74beb043e 509 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 54:5ff74beb043e 510 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 54:5ff74beb043e 511 dir_diff = 'N';
jamesheavey 54:5ff74beb043e 512 } else {
jamesheavey 54:5ff74beb043e 513 dir_diff = 'S';
jamesheavey 54:5ff74beb043e 514 }
jamesheavey 54:5ff74beb043e 515 }
jamesheavey 54:5ff74beb043e 516
jamesheavey 54:5ff74beb043e 517 if( dir_diff == 'N' ) {
jamesheavey 54:5ff74beb043e 518 explored[curr_node] &= 0b1011;
jamesheavey 54:5ff74beb043e 519 }
jamesheavey 54:5ff74beb043e 520 else if( dir_diff == 'E' ) {
jamesheavey 54:5ff74beb043e 521 explored[curr_node] &= 0b1101;
jamesheavey 54:5ff74beb043e 522 }
jamesheavey 54:5ff74beb043e 523 else if( dir_diff == 'S' ) {
jamesheavey 54:5ff74beb043e 524 explored[curr_node] &= 0b1110;
jamesheavey 54:5ff74beb043e 525 }
jamesheavey 54:5ff74beb043e 526 else if( dir_diff == 'W' ) {
jamesheavey 54:5ff74beb043e 527 explored[curr_node] &= 0b0111;
jamesheavey 53:dbb63b181faf 528 }
jamesheavey 54:5ff74beb043e 529 if(point[next_node] == point[d_node]) { break; }
jamesheavey 54:5ff74beb043e 530 }
jamesheavey 54:5ff74beb043e 531 }
jamesheavey 54:5ff74beb043e 532
jamesheavey 54:5ff74beb043e 533 else {
jamesheavey 54:5ff74beb043e 534
jamesheavey 54:5ff74beb043e 535 for(int j = short_length; j >= 0; j--) {
jamesheavey 54:5ff74beb043e 536 int curr_node = path_to_point_index(shortest[j]);
jamesheavey 54:5ff74beb043e 537 int next_node = path_to_point_index(shortest[j-1]);
jamesheavey 54:5ff74beb043e 538 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 54:5ff74beb043e 539 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 54:5ff74beb043e 540 dir_diff = 'E';
jamesheavey 54:5ff74beb043e 541 } else {
jamesheavey 54:5ff74beb043e 542 dir_diff = 'W';
jamesheavey 54:5ff74beb043e 543 }
jamesheavey 54:5ff74beb043e 544 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 54:5ff74beb043e 545 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 54:5ff74beb043e 546 dir_diff = 'N';
jamesheavey 54:5ff74beb043e 547 } else {
jamesheavey 54:5ff74beb043e 548 dir_diff = 'S';
jamesheavey 54:5ff74beb043e 549 }
jamesheavey 53:dbb63b181faf 550 }
jamesheavey 54:5ff74beb043e 551
jamesheavey 54:5ff74beb043e 552 if( dir_diff == 'N' ) {
jamesheavey 54:5ff74beb043e 553 explored[curr_node] &= 0b1011;
jamesheavey 54:5ff74beb043e 554 }
jamesheavey 54:5ff74beb043e 555 else if( dir_diff == 'E' ) {
jamesheavey 54:5ff74beb043e 556 explored[curr_node] &= 0b1101;
jamesheavey 54:5ff74beb043e 557 }
jamesheavey 54:5ff74beb043e 558 else if( dir_diff == 'S' ) {
jamesheavey 54:5ff74beb043e 559 explored[curr_node] &= 0b1110;
jamesheavey 54:5ff74beb043e 560 }
jamesheavey 54:5ff74beb043e 561 else if( dir_diff == 'W' ) {
jamesheavey 54:5ff74beb043e 562 explored[curr_node] &= 0b0111;
jamesheavey 54:5ff74beb043e 563 }
jamesheavey 54:5ff74beb043e 564 if(point[next_node] == point[d_node]) { break; }
jamesheavey 53:dbb63b181faf 565 }
jamesheavey 53:dbb63b181faf 566 }
jamesheavey 54:5ff74beb043e 567
jamesheavey 54:5ff74beb043e 568 // print_data("aft bt");
jamesheavey 53:dbb63b181faf 569
jamesheavey 34:63f7c61ee4da 570 }
jamesheavey 28:63ff8290964a 571 }
jamesheavey 28:63ff8290964a 572
jamesheavey 55:18ea9529afb2 573 bool dead_end_removal( int point1, int point2 ) // change into dead_end_removal and have it take two indexes and a path and return an array
jamesheavey 48:76cf4521d342 574 {
jamesheavey 48:76cf4521d342 575 // dead end removal between start and final node
jamesheavey 48:76cf4521d342 576 // add the inverse of the result to end of the looped_path
jamesheavey 48:76cf4521d342 577 // then separate into before and after, simplify both and compare
jamesheavey 51:3148875bb9e5 578 robot.stop();
jamesheavey 49:89f0f54aa46b 579 int index1, index2;
jamesheavey 49:89f0f54aa46b 580 int d_index, before_index, after_index;
jamesheavey 49:89f0f54aa46b 581 bool before = false;
jamesheavey 49:89f0f54aa46b 582
jamesheavey 49:89f0f54aa46b 583 bool desired_discovered = false;
jamesheavey 49:89f0f54aa46b 584
jamesheavey 49:89f0f54aa46b 585 for(int k = 0; k <= path_length; k++) {
jamesheavey 52:79f21d15ede1 586 if(looped_path[k] == point1) { desired_discovered = true; d_index = k; } // maybe set a variable
jamesheavey 52:79f21d15ede1 587 if(desired_discovered == false && looped_path[k] == point2) { before = true; before_index = k; }
jamesheavey 63:0358f24b6181 588 if(desired_discovered == true && looped_path[k] == point2) { after_index = k; }
jamesheavey 49:89f0f54aa46b 589 }
jamesheavey 49:89f0f54aa46b 590
jamesheavey 51:3148875bb9e5 591 if(before){
jamesheavey 51:3148875bb9e5 592 if( (d_index - before_index) <= (after_index - d_index) ){
jamesheavey 51:3148875bb9e5 593 before = true;
jamesheavey 51:3148875bb9e5 594 }
jamesheavey 51:3148875bb9e5 595 else{
jamesheavey 51:3148875bb9e5 596 before = false;
jamesheavey 51:3148875bb9e5 597 }
jamesheavey 49:89f0f54aa46b 598 }
jamesheavey 49:89f0f54aa46b 599
jamesheavey 49:89f0f54aa46b 600 if(before) {
jamesheavey 49:89f0f54aa46b 601 index1 = before_index;
jamesheavey 49:89f0f54aa46b 602 index2 = d_index;
jamesheavey 55:18ea9529afb2 603
jamesheavey 49:89f0f54aa46b 604 }else{
jamesheavey 49:89f0f54aa46b 605 index1 = d_index;
jamesheavey 49:89f0f54aa46b 606 index2 = after_index;
jamesheavey 49:89f0f54aa46b 607 }
jamesheavey 49:89f0f54aa46b 608
jamesheavey 48:76cf4521d342 609 int temp_array[100];
jamesheavey 48:76cf4521d342 610
jamesheavey 48:76cf4521d342 611 for( int x = 0; x <= path_length; x++ ) { shortest[x] = NULL; }
jamesheavey 48:76cf4521d342 612
jamesheavey 50:fa124ea1df9f 613 for( int i = index1; i <= index2; i++ ) { shortest[i-index1] = looped_path[i]; }
jamesheavey 48:76cf4521d342 614
jamesheavey 55:18ea9529afb2 615 short_length = index2 - index1;
jamesheavey 48:76cf4521d342 616
jamesheavey 48:76cf4521d342 617 // for every node in path, check if repeated
jamesheavey 48:76cf4521d342 618 // if rpeated, move everyting before the first occurence to the final occurence
jamesheavey 48:76cf4521d342 619 // save new path and repeat
jamesheavey 48:76cf4521d342 620 // when no node is repeated, end
jamesheavey 48:76cf4521d342 621 // or end when iterated through the entire array
jamesheavey 48:76cf4521d342 622
jamesheavey 48:76cf4521d342 623 int i = 0;
jamesheavey 55:18ea9529afb2 624 while( i <= short_length ) {
jamesheavey 48:76cf4521d342 625 int count = i;
jamesheavey 55:18ea9529afb2 626 for( int j = 0; j <= short_length; j++ ) {
jamesheavey 48:76cf4521d342 627 if( shortest[i] == shortest[j] ){
jamesheavey 48:76cf4521d342 628 count = j;
jamesheavey 48:76cf4521d342 629 }
jamesheavey 39:005ad4610152 630 }
jamesheavey 48:76cf4521d342 631 if( count != i ) {
jamesheavey 48:76cf4521d342 632
jamesheavey 48:76cf4521d342 633 for( int k = 0; k <= i; k++ ) {
jamesheavey 48:76cf4521d342 634 temp_array[k] = shortest[k];
jamesheavey 48:76cf4521d342 635 }
jamesheavey 48:76cf4521d342 636 int ind = 1;
jamesheavey 48:76cf4521d342 637 for( int z = count+1; z <= path_length; z++ ) {
jamesheavey 48:76cf4521d342 638 temp_array[i+ind] = shortest[z];
jamesheavey 48:76cf4521d342 639 ind++;
jamesheavey 48:76cf4521d342 640 }
jamesheavey 48:76cf4521d342 641 // clear the array
jamesheavey 55:18ea9529afb2 642 for( int x = 0; x <= short_length; x++ ) { shortest[x] = NULL; }
jamesheavey 48:76cf4521d342 643
jamesheavey 55:18ea9529afb2 644 short_length -= (count-i);
jamesheavey 48:76cf4521d342 645 i = -1;
jamesheavey 48:76cf4521d342 646
jamesheavey 55:18ea9529afb2 647 for( int x = 0; x <= short_length; x++ ) { shortest[x] = temp_array[x]; }
jamesheavey 48:76cf4521d342 648
jamesheavey 55:18ea9529afb2 649 for( int x = 0; x <= short_length; x++ ) { temp_array[x] = NULL; }
jamesheavey 39:005ad4610152 650 }
jamesheavey 48:76cf4521d342 651 i++;
jamesheavey 39:005ad4610152 652 }
jamesheavey 39:005ad4610152 653
jamesheavey 63:0358f24b6181 654 //robot.lcd_print("len",10);
jamesheavey 55:18ea9529afb2 655 // wait(1);
jamesheavey 55:18ea9529afb2 656 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 657 //
jamesheavey 55:18ea9529afb2 658 // char buffe[3];
jamesheavey 55:18ea9529afb2 659 // sprintf(buffe,"%d", short_length);
jamesheavey 55:18ea9529afb2 660 // robot.lcd_print(buffe,3);
jamesheavey 55:18ea9529afb2 661 // wait(1);
jamesheavey 55:18ea9529afb2 662 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 663 //
jamesheavey 55:18ea9529afb2 664 // robot.lcd_print("path",10);
jamesheavey 55:18ea9529afb2 665 // wait(1);
jamesheavey 55:18ea9529afb2 666 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 667 //
jamesheavey 55:18ea9529afb2 668 // leds = 0b1111;
jamesheavey 55:18ea9529afb2 669 // for(int m = 0; m <= short_length; m++) {
jamesheavey 55:18ea9529afb2 670 // leds = ~leds;
jamesheavey 55:18ea9529afb2 671 // char buffer[3];
jamesheavey 55:18ea9529afb2 672 // sprintf(buffer,"%d", shortest[m]);
jamesheavey 55:18ea9529afb2 673 // robot.lcd_print(buffer,3);
jamesheavey 55:18ea9529afb2 674 // wait(1);
jamesheavey 55:18ea9529afb2 675 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 676 // }
jamesheavey 53:dbb63b181faf 677
jamesheavey 55:18ea9529afb2 678 return before;
jamesheavey 48:76cf4521d342 679 // print these to check
jamesheavey 48:76cf4521d342 680 // add inverted sub path to the real path and increment the path length by real_len -1
jamesheavey 39:005ad4610152 681 }
jamesheavey 39:005ad4610152 682
jamesheavey 65:2103a02d72bc 683 void shorten( int path[], int length )
jamesheavey 65:2103a02d72bc 684 {
jamesheavey 65:2103a02d72bc 685 int temp_array[100];
jamesheavey 65:2103a02d72bc 686 int i = 0;
jamesheavey 65:2103a02d72bc 687
jamesheavey 65:2103a02d72bc 688 while( i <= length ) {
jamesheavey 65:2103a02d72bc 689 int count = i;
jamesheavey 65:2103a02d72bc 690 for( int j = 0; j <= length; j++ ) {
jamesheavey 65:2103a02d72bc 691 if( path[i] == path[j] ){
jamesheavey 65:2103a02d72bc 692 count = j;
jamesheavey 65:2103a02d72bc 693 }
jamesheavey 65:2103a02d72bc 694 }
jamesheavey 65:2103a02d72bc 695 if( count != i ) {
jamesheavey 65:2103a02d72bc 696
jamesheavey 65:2103a02d72bc 697 for( int k = 0; k <= i; k++ ) {
jamesheavey 65:2103a02d72bc 698 temp_array[k] = path[k];
jamesheavey 65:2103a02d72bc 699 }
jamesheavey 65:2103a02d72bc 700 int ind = 1;
jamesheavey 65:2103a02d72bc 701 for( int z = count+1; z <= length; z++ ) {
jamesheavey 65:2103a02d72bc 702 temp_array[i+ind] = path[z];
jamesheavey 65:2103a02d72bc 703 ind++;
jamesheavey 65:2103a02d72bc 704 }
jamesheavey 65:2103a02d72bc 705 // clear the array
jamesheavey 65:2103a02d72bc 706 for( int x = 0; x <= length; x++ ) { path[x] = NULL; }
jamesheavey 65:2103a02d72bc 707
jamesheavey 65:2103a02d72bc 708 length -= (count-i);
jamesheavey 65:2103a02d72bc 709 i = -1;
jamesheavey 65:2103a02d72bc 710
jamesheavey 65:2103a02d72bc 711 for( int x = 0; x <= length; x++ ) { path[x] = temp_array[x]; }
jamesheavey 65:2103a02d72bc 712
jamesheavey 65:2103a02d72bc 713 for( int x = 0; x <= length; x++ ) { temp_array[x] = NULL; }
jamesheavey 65:2103a02d72bc 714 }
jamesheavey 65:2103a02d72bc 715 i++;
jamesheavey 65:2103a02d72bc 716 }
jamesheavey 65:2103a02d72bc 717
jamesheavey 65:2103a02d72bc 718 temp_length = length;
jamesheavey 65:2103a02d72bc 719
jamesheavey 65:2103a02d72bc 720 for( int x = 0; x <= 100; x++ ) { temp_path[x] = NULL; }
jamesheavey 65:2103a02d72bc 721
jamesheavey 65:2103a02d72bc 722 for( int x = 0; x <= length; x++ ) { temp_path[x] = path[x]; }
jamesheavey 65:2103a02d72bc 723 }
jamesheavey 65:2103a02d72bc 724
jamesheavey 58:cb32aa0f4116 725 void looped_goal_simplification() // change into dead_end_removal and have it take two indexes and a path and return an array
jamesheavey 58:cb32aa0f4116 726 {
jamesheavey 58:cb32aa0f4116 727 for( int i = 0; i <= path_length; i++ ) {
jamesheavey 58:cb32aa0f4116 728 goal_path1[i] = looped_path[i];
jamesheavey 58:cb32aa0f4116 729 if( looped_path[i] == 100 ) { goal_length1 = i; break; }
jamesheavey 58:cb32aa0f4116 730 }
jamesheavey 58:cb32aa0f4116 731
jamesheavey 64:a58d8538132a 732 for( int i = goal_length1; i >= 0; i-- ) {
jamesheavey 64:a58d8538132a 733 goal_path3[goal_length1-i] = goal_path1[i];
jamesheavey 64:a58d8538132a 734 }
jamesheavey 64:a58d8538132a 735 int goal_length3 = goal_length1;
jamesheavey 64:a58d8538132a 736
jamesheavey 58:cb32aa0f4116 737 for( int i = path_length; i >= 0; i-- ) {
jamesheavey 58:cb32aa0f4116 738 goal_path2[path_length-i] = looped_path[i];
jamesheavey 58:cb32aa0f4116 739 if( looped_path[i] == 100 ) { goal_length2 = path_length-i; break; }
jamesheavey 58:cb32aa0f4116 740 }
jamesheavey 58:cb32aa0f4116 741
jamesheavey 64:a58d8538132a 742 for( int i = goal_length2; i >= 0; i-- ) {
jamesheavey 64:a58d8538132a 743 goal_path4[goal_length2-i] = goal_path2[i];
jamesheavey 64:a58d8538132a 744 }
jamesheavey 64:a58d8538132a 745 int goal_length4 = goal_length2;
jamesheavey 64:a58d8538132a 746
jamesheavey 58:cb32aa0f4116 747 int temp_array[100];
jamesheavey 64:a58d8538132a 748 // path 1
jamesheavey 58:cb32aa0f4116 749 int i = 0;
jamesheavey 58:cb32aa0f4116 750 while( i <= goal_length1 ) {
jamesheavey 58:cb32aa0f4116 751 int count = i;
jamesheavey 58:cb32aa0f4116 752 for( int j = 0; j <= goal_length1; j++ ) {
jamesheavey 58:cb32aa0f4116 753 if( goal_path1[i] == goal_path1[j] ){
jamesheavey 58:cb32aa0f4116 754 count = j;
jamesheavey 58:cb32aa0f4116 755 }
jamesheavey 58:cb32aa0f4116 756 }
jamesheavey 58:cb32aa0f4116 757 if( count != i ) {
jamesheavey 58:cb32aa0f4116 758
jamesheavey 58:cb32aa0f4116 759 for( int k = 0; k <= i; k++ ) {
jamesheavey 58:cb32aa0f4116 760 temp_array[k] = goal_path1[k];
jamesheavey 58:cb32aa0f4116 761 }
jamesheavey 58:cb32aa0f4116 762 int ind = 1;
jamesheavey 58:cb32aa0f4116 763 for( int z = count+1; z <= goal_length1; z++ ) {
jamesheavey 58:cb32aa0f4116 764 temp_array[i+ind] = goal_path1[z];
jamesheavey 58:cb32aa0f4116 765 ind++;
jamesheavey 58:cb32aa0f4116 766 }
jamesheavey 58:cb32aa0f4116 767 // clear the array
jamesheavey 58:cb32aa0f4116 768 for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL; }
jamesheavey 58:cb32aa0f4116 769
jamesheavey 58:cb32aa0f4116 770 goal_length1 -= (count-i);
jamesheavey 58:cb32aa0f4116 771 i = -1;
jamesheavey 58:cb32aa0f4116 772
jamesheavey 58:cb32aa0f4116 773 for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = temp_array[x]; }
jamesheavey 58:cb32aa0f4116 774
jamesheavey 58:cb32aa0f4116 775 for( int x = 0; x <= goal_length1; x++ ) { temp_array[x] = NULL; }
jamesheavey 58:cb32aa0f4116 776 }
jamesheavey 58:cb32aa0f4116 777 i++;
jamesheavey 58:cb32aa0f4116 778 }
jamesheavey 64:a58d8538132a 779 // path 2
jamesheavey 58:cb32aa0f4116 780 i = 0;
jamesheavey 58:cb32aa0f4116 781 while( i <= goal_length2 ) {
jamesheavey 58:cb32aa0f4116 782 int count = i;
jamesheavey 58:cb32aa0f4116 783 for( int j = 0; j <= goal_length2; j++ ) {
jamesheavey 58:cb32aa0f4116 784 if( goal_path2[i] == goal_path2[j] ){
jamesheavey 58:cb32aa0f4116 785 count = j;
jamesheavey 58:cb32aa0f4116 786 }
jamesheavey 58:cb32aa0f4116 787 }
jamesheavey 58:cb32aa0f4116 788 if( count != i ) {
jamesheavey 58:cb32aa0f4116 789
jamesheavey 58:cb32aa0f4116 790 for( int k = 0; k <= i; k++ ) {
jamesheavey 58:cb32aa0f4116 791 temp_array[k] = goal_path2[k];
jamesheavey 58:cb32aa0f4116 792 }
jamesheavey 58:cb32aa0f4116 793 int ind = 1;
jamesheavey 58:cb32aa0f4116 794 for( int z = count+1; z <= goal_length2; z++ ) {
jamesheavey 58:cb32aa0f4116 795 temp_array[i+ind] = goal_path2[z];
jamesheavey 58:cb32aa0f4116 796 ind++;
jamesheavey 58:cb32aa0f4116 797 }
jamesheavey 58:cb32aa0f4116 798 // clear the array
jamesheavey 58:cb32aa0f4116 799 for( int x = 0; x <= goal_length2; x++ ) { goal_path2[x] = NULL; }
jamesheavey 58:cb32aa0f4116 800
jamesheavey 58:cb32aa0f4116 801 goal_length2 -= (count-i);
jamesheavey 58:cb32aa0f4116 802 i = -1;
jamesheavey 58:cb32aa0f4116 803
jamesheavey 58:cb32aa0f4116 804 for( int x = 0; x <= goal_length2; x++ ) { goal_path2[x] = temp_array[x]; }
jamesheavey 58:cb32aa0f4116 805
jamesheavey 58:cb32aa0f4116 806 for( int x = 0; x <= goal_length2; x++ ) { temp_array[x] = NULL; }
jamesheavey 58:cb32aa0f4116 807 }
jamesheavey 58:cb32aa0f4116 808 i++;
jamesheavey 58:cb32aa0f4116 809 }
jamesheavey 64:a58d8538132a 810 // path 3
jamesheavey 64:a58d8538132a 811 i = 0;
jamesheavey 64:a58d8538132a 812 while( i <= goal_length3 ) {
jamesheavey 64:a58d8538132a 813 int count = i;
jamesheavey 64:a58d8538132a 814 for( int j = 0; j <= goal_length3; j++ ) {
jamesheavey 64:a58d8538132a 815 if( goal_path3[i] == goal_path3[j] ){
jamesheavey 64:a58d8538132a 816 count = j;
jamesheavey 64:a58d8538132a 817 }
jamesheavey 64:a58d8538132a 818 }
jamesheavey 64:a58d8538132a 819 if( count != i ) {
jamesheavey 64:a58d8538132a 820
jamesheavey 64:a58d8538132a 821 for( int k = 0; k <= i; k++ ) {
jamesheavey 64:a58d8538132a 822 temp_array[k] = goal_path3[k];
jamesheavey 64:a58d8538132a 823 }
jamesheavey 64:a58d8538132a 824 int ind = 1;
jamesheavey 64:a58d8538132a 825 for( int z = count+1; z <= goal_length3; z++ ) {
jamesheavey 64:a58d8538132a 826 temp_array[i+ind] = goal_path3[z];
jamesheavey 64:a58d8538132a 827 ind++;
jamesheavey 64:a58d8538132a 828 }
jamesheavey 64:a58d8538132a 829 // clear the array
jamesheavey 64:a58d8538132a 830 for( int x = 0; x <= goal_length3; x++ ) { goal_path3[x] = NULL; }
jamesheavey 64:a58d8538132a 831
jamesheavey 64:a58d8538132a 832 goal_length3 -= (count-i);
jamesheavey 64:a58d8538132a 833 i = -1;
jamesheavey 64:a58d8538132a 834
jamesheavey 64:a58d8538132a 835 for( int x = 0; x <= goal_length3; x++ ) { goal_path3[x] = temp_array[x]; }
jamesheavey 64:a58d8538132a 836
jamesheavey 64:a58d8538132a 837 for( int x = 0; x <= goal_length3; x++ ) { temp_array[x] = NULL; }
jamesheavey 64:a58d8538132a 838 }
jamesheavey 64:a58d8538132a 839 i++;
jamesheavey 64:a58d8538132a 840 }
jamesheavey 64:a58d8538132a 841 i = 0;
jamesheavey 64:a58d8538132a 842 while( i <= goal_length4 ) {
jamesheavey 64:a58d8538132a 843 int count = i;
jamesheavey 64:a58d8538132a 844 for( int j = 0; j <= goal_length4; j++ ) {
jamesheavey 64:a58d8538132a 845 if( goal_path4[i] == goal_path4[j] ){
jamesheavey 64:a58d8538132a 846 count = j;
jamesheavey 64:a58d8538132a 847 }
jamesheavey 64:a58d8538132a 848 }
jamesheavey 64:a58d8538132a 849 if( count != i ) {
jamesheavey 64:a58d8538132a 850
jamesheavey 64:a58d8538132a 851 for( int k = 0; k <= i; k++ ) {
jamesheavey 64:a58d8538132a 852 temp_array[k] = goal_path4[k];
jamesheavey 64:a58d8538132a 853 }
jamesheavey 64:a58d8538132a 854 int ind = 1;
jamesheavey 64:a58d8538132a 855 for( int z = count+1; z <= goal_length4; z++ ) {
jamesheavey 64:a58d8538132a 856 temp_array[i+ind] = goal_path4[z];
jamesheavey 64:a58d8538132a 857 ind++;
jamesheavey 64:a58d8538132a 858 }
jamesheavey 64:a58d8538132a 859 // clear the array
jamesheavey 64:a58d8538132a 860 for( int x = 0; x <= goal_length4; x++ ) { goal_path4[x] = NULL; }
jamesheavey 64:a58d8538132a 861
jamesheavey 64:a58d8538132a 862 goal_length4 -= (count-i);
jamesheavey 64:a58d8538132a 863 i = -1;
jamesheavey 64:a58d8538132a 864
jamesheavey 64:a58d8538132a 865 for( int x = 0; x <= goal_length4; x++ ) { goal_path4[x] = temp_array[x]; }
jamesheavey 64:a58d8538132a 866
jamesheavey 64:a58d8538132a 867 for( int x = 0; x <= goal_length4; x++ ) { temp_array[x] = NULL; }
jamesheavey 64:a58d8538132a 868 }
jamesheavey 64:a58d8538132a 869 i++;
jamesheavey 64:a58d8538132a 870 }
jamesheavey 58:cb32aa0f4116 871
jamesheavey 64:a58d8538132a 872 if( goal_length2 <= goal_length1 && goal_length2 <= goal_length3 && goal_length2 <= goal_length4 ) {
jamesheavey 58:cb32aa0f4116 873 for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL; }
jamesheavey 58:cb32aa0f4116 874 for( int x = 0; x <= goal_length2; x++ ) {
jamesheavey 58:cb32aa0f4116 875 goal_path1[x] = goal_path2[x];
jamesheavey 58:cb32aa0f4116 876 }
jamesheavey 58:cb32aa0f4116 877 goal_length1 = goal_length2;
jamesheavey 61:b31d8c997279 878 }
jamesheavey 64:a58d8538132a 879 else if( goal_length3 <= goal_length1 && goal_length3 <= goal_length2 && goal_length3 <= goal_length4 ) {
jamesheavey 64:a58d8538132a 880 for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL; }
jamesheavey 64:a58d8538132a 881 for( int x = goal_length3; x >= 0; x-- ) {
jamesheavey 64:a58d8538132a 882 goal_path1[goal_length3 - x] = goal_path3[x];
jamesheavey 64:a58d8538132a 883 }
jamesheavey 64:a58d8538132a 884 goal_length1 = goal_length3;
jamesheavey 64:a58d8538132a 885 }
jamesheavey 64:a58d8538132a 886 else if( goal_length4 <= goal_length1 && goal_length4 <= goal_length2 && goal_length4 <= goal_length3 ) {
jamesheavey 64:a58d8538132a 887 for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL; }
jamesheavey 64:a58d8538132a 888 for( int x = goal_length4; x >= 0; x-- ) {
jamesheavey 64:a58d8538132a 889 goal_path1[goal_length4 - x] = goal_path4[x];
jamesheavey 64:a58d8538132a 890 }
jamesheavey 64:a58d8538132a 891 goal_length1 = goal_length4;
jamesheavey 64:a58d8538132a 892 }
jamesheavey 63:0358f24b6181 893
jamesheavey 64:a58d8538132a 894 //robot.lcd_print("path",10);
jamesheavey 64:a58d8538132a 895 // wait(1);
jamesheavey 64:a58d8538132a 896 // robot.lcd_clear();
jamesheavey 64:a58d8538132a 897 //
jamesheavey 64:a58d8538132a 898 // leds = 0b1111;
jamesheavey 64:a58d8538132a 899 // for(int m = 0; m <= path_length; m++) {
jamesheavey 64:a58d8538132a 900 // leds = ~leds;
jamesheavey 64:a58d8538132a 901 // char buffer[3];
jamesheavey 64:a58d8538132a 902 // sprintf(buffer,"%d", looped_path[m]);
jamesheavey 64:a58d8538132a 903 // robot.lcd_print(buffer,3);
jamesheavey 64:a58d8538132a 904 // wait(1);
jamesheavey 64:a58d8538132a 905 // robot.lcd_clear();
jamesheavey 64:a58d8538132a 906 // }
jamesheavey 63:0358f24b6181 907
jamesheavey 65:2103a02d72bc 908 // robot.lcd_print("g path",10);
jamesheavey 65:2103a02d72bc 909 // wait(1);
jamesheavey 65:2103a02d72bc 910 // robot.lcd_clear();
jamesheavey 65:2103a02d72bc 911 //
jamesheavey 65:2103a02d72bc 912 // leds = 0b1111;
jamesheavey 65:2103a02d72bc 913 // for(int m = 0; m <= goal_length1; m++) {
jamesheavey 65:2103a02d72bc 914 // leds = ~leds;
jamesheavey 65:2103a02d72bc 915 // char buffer[3];
jamesheavey 65:2103a02d72bc 916 // sprintf(buffer,"%d", goal_path1[m]);
jamesheavey 65:2103a02d72bc 917 // robot.lcd_print(buffer,3);
jamesheavey 65:2103a02d72bc 918 // wait(1);
jamesheavey 65:2103a02d72bc 919 // robot.lcd_clear();
jamesheavey 65:2103a02d72bc 920 // }
jamesheavey 63:0358f24b6181 921
jamesheavey 58:cb32aa0f4116 922 // print these to check
jamesheavey 58:cb32aa0f4116 923 // add inverted sub path to the real path and increment the path length by real_len -1
jamesheavey 58:cb32aa0f4116 924 }
jamesheavey 58:cb32aa0f4116 925
jamesheavey 10:691c02b352cb 926 void follow_line()
jamesheavey 0:df5216b20861 927 {
jamesheavey 10:691c02b352cb 928 robot.scan();
jamesheavey 10:691c02b352cb 929 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 930
jamesheavey 15:6c461501d12d 931 leds = 0b0110;
jamesheavey 15:6c461501d12d 932
jamesheavey 10:691c02b352cb 933 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 934 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 935 integral += proportional;
jamesheavey 10:691c02b352cb 936 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 937
jamesheavey 10:691c02b352cb 938 // calculate motor correction
jamesheavey 10:691c02b352cb 939 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 940
jamesheavey 10:691c02b352cb 941 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 942 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 943 motor_correction = speed;
jamesheavey 10:691c02b352cb 944 }
jamesheavey 10:691c02b352cb 945 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 946 motor_correction = -speed;
jamesheavey 10:691c02b352cb 947 }
jamesheavey 0:df5216b20861 948
jamesheavey 10:691c02b352cb 949 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 950 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 951 } else {
jamesheavey 10:691c02b352cb 952 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 953 }
jamesheavey 18:991658b628fc 954
jamesheavey 19:4c08275cb3c9 955 // read_encoders();
jamesheavey 19:4c08275cb3c9 956 // 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 957 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 958 }
jamesheavey 0:df5216b20861 959
jamesheavey 14:87052bb35211 960 bool junction_detect()
jamesheavey 6:c10b367747a0 961 {
jamesheavey 20:5cf6a378801d 962 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 963 return true;
jamesheavey 20:5cf6a378801d 964 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 965 return true;
jamesheavey 14:87052bb35211 966 } else {
jamesheavey 14:87052bb35211 967 return false;
jamesheavey 14:87052bb35211 968 }
jamesheavey 14:87052bb35211 969 }
jamesheavey 14:87052bb35211 970
jamesheavey 14:87052bb35211 971 char junction_logic()
jamesheavey 14:87052bb35211 972 {
jamesheavey 14:87052bb35211 973 bool straight = false;
jamesheavey 7:7fefd782532d 974 bool left = false;
jamesheavey 6:c10b367747a0 975 bool right = false;
jamesheavey 9:952586accbf9 976 bool goal = false;
jamesheavey 7:7fefd782532d 977
jamesheavey 20:5cf6a378801d 978 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 979 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 980 robot.forward(speed);
jamesheavey 9:952586accbf9 981 robot.scan();
jamesheavey 20:5cf6a378801d 982 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 983 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 984 }
jamesheavey 7:7fefd782532d 985
jamesheavey 20:5cf6a378801d 986 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 987 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 988 robot.scan();
jamesheavey 20:5cf6a378801d 989 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 990 goal = true;
jamesheavey 12:d80399686f32 991 }
jamesheavey 13:bd271266e161 992 }
jamesheavey 14:87052bb35211 993
jamesheavey 14:87052bb35211 994 robot.scan();
jamesheavey 14:87052bb35211 995
jamesheavey 20:5cf6a378801d 996 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 997 straight = true;
jamesheavey 14:87052bb35211 998 }
jamesheavey 14:87052bb35211 999
jamesheavey 20:5cf6a378801d 1000 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 1001 return 'B';
jamesheavey 6:c10b367747a0 1002 }
jamesheavey 7:7fefd782532d 1003
jamesheavey 9:952586accbf9 1004 if (goal) {
jamesheavey 9:952586accbf9 1005 return 'G';
jamesheavey 9:952586accbf9 1006 } else if (left) {
jamesheavey 9:952586accbf9 1007 return 'L';
jamesheavey 14:87052bb35211 1008 } else if (straight) {
jamesheavey 14:87052bb35211 1009 return 'S';
jamesheavey 9:952586accbf9 1010 } else if (right) {
jamesheavey 9:952586accbf9 1011 return 'R';
jamesheavey 6:c10b367747a0 1012 } else {
jamesheavey 9:952586accbf9 1013 return 'S';
jamesheavey 9:952586accbf9 1014 }
jamesheavey 6:c10b367747a0 1015 }
jamesheavey 6:c10b367747a0 1016
jamesheavey 0:df5216b20861 1017
jamesheavey 14:87052bb35211 1018 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 1019 {
jamesheavey 1:79219d0a33c8 1020 switch(turn) {
jamesheavey 5:ae417756235a 1021 case 'G':
jamesheavey 5:ae417756235a 1022 goal();
jamesheavey 1:79219d0a33c8 1023 case 'L':
jamesheavey 1:79219d0a33c8 1024 left();
jamesheavey 5:ae417756235a 1025 break;
jamesheavey 1:79219d0a33c8 1026 case 'S':
jamesheavey 1:79219d0a33c8 1027 break;
jamesheavey 1:79219d0a33c8 1028 case 'R':
jamesheavey 1:79219d0a33c8 1029 right();
jamesheavey 5:ae417756235a 1030 break;
jamesheavey 1:79219d0a33c8 1031 case 'B':
jamesheavey 1:79219d0a33c8 1032 back();
jamesheavey 5:ae417756235a 1033 break;
jamesheavey 1:79219d0a33c8 1034 }
jamesheavey 1:79219d0a33c8 1035 }
jamesheavey 1:79219d0a33c8 1036
jamesheavey 0:df5216b20861 1037 void left()
jamesheavey 0:df5216b20861 1038 {
jamesheavey 1:79219d0a33c8 1039 leds = 0b1100;
jamesheavey 3:a5e06482462e 1040
jamesheavey 20:5cf6a378801d 1041 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 1042
jamesheavey 39:005ad4610152 1043 robot.spin_left(TURN_SPEED);
jamesheavey 58:cb32aa0f4116 1044 wait(0.25);
jamesheavey 3:a5e06482462e 1045
jamesheavey 20:5cf6a378801d 1046 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 1047
jamesheavey 20:5cf6a378801d 1048 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 1049 }
jamesheavey 1:79219d0a33c8 1050
jamesheavey 1:79219d0a33c8 1051 void right()
jamesheavey 1:79219d0a33c8 1052 {
jamesheavey 1:79219d0a33c8 1053 leds = 0b0011;
jamesheavey 5:ae417756235a 1054
jamesheavey 20:5cf6a378801d 1055 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 1056
jamesheavey 20:5cf6a378801d 1057 robot.spin_right(TURN_SPEED);
jamesheavey 58:cb32aa0f4116 1058 wait(0.25);
jamesheavey 3:a5e06482462e 1059
jamesheavey 20:5cf6a378801d 1060 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 1061
jamesheavey 20:5cf6a378801d 1062 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 1063 }
jamesheavey 0:df5216b20861 1064
jamesheavey 0:df5216b20861 1065 void back()
jamesheavey 0:df5216b20861 1066 {
jamesheavey 1:79219d0a33c8 1067 leds = 0b1111;
jamesheavey 32:598bedb22c7c 1068 // robot.reverse(speed);
jamesheavey 32:598bedb22c7c 1069 // wait(0.1);
jamesheavey 20:5cf6a378801d 1070 robot.spin_right(TURN_SPEED);
jamesheavey 41:2b6b73dd897c 1071 if(loop_check == false) { // works better for looped
jamesheavey 53:dbb63b181faf 1072 wait(0.65);
jamesheavey 41:2b6b73dd897c 1073 }
jamesheavey 41:2b6b73dd897c 1074 else {
jamesheavey 41:2b6b73dd897c 1075 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 1076
jamesheavey 41:2b6b73dd897c 1077 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 1078 }
jamesheavey 0:df5216b20861 1079 }
jamesheavey 1:79219d0a33c8 1080
jamesheavey 2:940e46e21353 1081 void simplify()
jamesheavey 1:79219d0a33c8 1082 {
jamesheavey 2:940e46e21353 1083 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 1084 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 1085 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 1086
jamesheavey 10:691c02b352cb 1087 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 1088 int angle_change = 0;
jamesheavey 2:940e46e21353 1089
jamesheavey 10:691c02b352cb 1090 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 1091 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 1092 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 1093 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 1094 }
jamesheavey 4:38c29dbc5953 1095
jamesheavey 4:38c29dbc5953 1096 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 1097
jamesheavey 4:38c29dbc5953 1098 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 1099 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 1100 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 1101 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 1102
jamesheavey 4:38c29dbc5953 1103 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 1104
jamesheavey 4:38c29dbc5953 1105 path_length -= 2;
jamesheavey 2:940e46e21353 1106 }
jamesheavey 5:ae417756235a 1107 }
jamesheavey 5:ae417756235a 1108
jamesheavey 10:691c02b352cb 1109 void goal()
jamesheavey 10:691c02b352cb 1110 {
jamesheavey 10:691c02b352cb 1111 invert_path();
jamesheavey 11:c3299aca7d8f 1112
jamesheavey 11:c3299aca7d8f 1113 leds = 0b0000;
jamesheavey 10:691c02b352cb 1114
jamesheavey 10:691c02b352cb 1115 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 1116 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 1117
jamesheavey 11:c3299aca7d8f 1118 while(1) {
jamesheavey 15:6c461501d12d 1119 int pointer = 0;
jamesheavey 15:6c461501d12d 1120
jamesheavey 15:6c461501d12d 1121 robot.stop();
jamesheavey 15:6c461501d12d 1122
jamesheavey 15:6c461501d12d 1123 leds = 0b1001;
jamesheavey 15:6c461501d12d 1124 wait(0.2);
jamesheavey 15:6c461501d12d 1125 leds = 0b0110;
jamesheavey 15:6c461501d12d 1126 wait(0.2);
jamesheavey 15:6c461501d12d 1127
jamesheavey 15:6c461501d12d 1128 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 1129 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 1130 wait(0.05);
jamesheavey 15:6c461501d12d 1131
jamesheavey 20:5cf6a378801d 1132 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 1133 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 1134 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 1135 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 1136
jamesheavey 15:6c461501d12d 1137 robot.stop();
jamesheavey 15:6c461501d12d 1138
jamesheavey 15:6c461501d12d 1139 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 1140 follow_line();
jamesheavey 15:6c461501d12d 1141
jamesheavey 15:6c461501d12d 1142 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 1143 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 1144 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 1145 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 1146 robot.forward(speed);
jamesheavey 15:6c461501d12d 1147 leds = 0b1010;
jamesheavey 20:5cf6a378801d 1148 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 1149 }
jamesheavey 15:6c461501d12d 1150 pointer++;
jamesheavey 15:6c461501d12d 1151 }
jamesheavey 15:6c461501d12d 1152 }
jamesheavey 15:6c461501d12d 1153
jamesheavey 15:6c461501d12d 1154 back();
jamesheavey 12:d80399686f32 1155
jamesheavey 11:c3299aca7d8f 1156 robot.stop();
jamesheavey 20:5cf6a378801d 1157 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 1158 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 1159 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 1160 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 1161
jamesheavey 14:87052bb35211 1162 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 1163
jamesheavey 20:5cf6a378801d 1164 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 1165 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 1166
jamesheavey 15:6c461501d12d 1167 pointer = 0;
jamesheavey 15:6c461501d12d 1168
jamesheavey 11:c3299aca7d8f 1169 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 1170 wait(0.2);
jamesheavey 11:c3299aca7d8f 1171 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 1172 wait(0.2);
jamesheavey 11:c3299aca7d8f 1173 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 1174 wait(0.2);
jamesheavey 11:c3299aca7d8f 1175 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 1176 wait(0.2);
jamesheavey 11:c3299aca7d8f 1177
jamesheavey 11:c3299aca7d8f 1178 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 1179 follow_line();
jamesheavey 11:c3299aca7d8f 1180
jamesheavey 14:87052bb35211 1181 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 1182 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 1183 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 1184 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 1185 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 1186 leds = 0b1010;
jamesheavey 20:5cf6a378801d 1187 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 1188 }
jamesheavey 11:c3299aca7d8f 1189 pointer++;
jamesheavey 10:691c02b352cb 1190 }
jamesheavey 10:691c02b352cb 1191 }
jamesheavey 5:ae417756235a 1192 }
jamesheavey 58:cb32aa0f4116 1193 }
jamesheavey 5:ae417756235a 1194
jamesheavey 29:ecf497c3fdc0 1195 void looped_goal()
jamesheavey 29:ecf497c3fdc0 1196 {
jamesheavey 48:76cf4521d342 1197 // return to start (adding nodes to path)
jamesheavey 48:76cf4521d342 1198 // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
jamesheavey 48:76cf4521d342 1199 // loop between start and goal
jamesheavey 58:cb32aa0f4116 1200 turn_select('B');
jamesheavey 34:63f7c61ee4da 1201 robot.stop();
jamesheavey 58:cb32aa0f4116 1202
jamesheavey 58:cb32aa0f4116 1203 if( dir == 'N' ) { dir = 'S'; }
jamesheavey 58:cb32aa0f4116 1204 else if( dir == 'S' ) { dir = 'N'; }
jamesheavey 58:cb32aa0f4116 1205 else if( dir == 'E' ) { dir = 'W'; }
jamesheavey 58:cb32aa0f4116 1206 else if( dir == 'W' ) { dir = 'E'; }
jamesheavey 58:cb32aa0f4116 1207
jamesheavey 58:cb32aa0f4116 1208 looped_goal_simplification();
jamesheavey 58:cb32aa0f4116 1209
jamesheavey 58:cb32aa0f4116 1210 while(1) {
jamesheavey 59:11e31ce4d675 1211
jamesheavey 65:2103a02d72bc 1212 if( _switch == false ) {
jamesheavey 65:2103a02d72bc 1213 robot.lcd_clear();
jamesheavey 65:2103a02d72bc 1214 robot.lcd_goto_xy(0,0);
jamesheavey 65:2103a02d72bc 1215 robot.lcd_print(" ENTER ", 10);
jamesheavey 65:2103a02d72bc 1216 robot.lcd_goto_xy(0,1);
jamesheavey 65:2103a02d72bc 1217 robot.lcd_print("=restart", 10);
jamesheavey 59:11e31ce4d675 1218 while( button_enter.read() == 1 ) {
jamesheavey 59:11e31ce4d675 1219 leds = 0b1001;
jamesheavey 59:11e31ce4d675 1220 wait(0.2);
jamesheavey 59:11e31ce4d675 1221 leds = 0b0110;
jamesheavey 59:11e31ce4d675 1222 wait(0.2);
jamesheavey 59:11e31ce4d675 1223 }
jamesheavey 65:2103a02d72bc 1224 } else {
jamesheavey 65:2103a02d72bc 1225 leds = 0b1001;
jamesheavey 65:2103a02d72bc 1226 wait(0.2);
jamesheavey 65:2103a02d72bc 1227 leds = 0b0110;
jamesheavey 65:2103a02d72bc 1228 wait(0.2);
jamesheavey 58:cb32aa0f4116 1229 }
jamesheavey 65:2103a02d72bc 1230
jamesheavey 58:cb32aa0f4116 1231 wait(2);
jamesheavey 58:cb32aa0f4116 1232
jamesheavey 58:cb32aa0f4116 1233 t_restart = true;
jamesheavey 58:cb32aa0f4116 1234
jamesheavey 58:cb32aa0f4116 1235 goal_path_explored(_switch);
jamesheavey 58:cb32aa0f4116 1236
jamesheavey 58:cb32aa0f4116 1237 while(1) {
jamesheavey 58:cb32aa0f4116 1238
jamesheavey 58:cb32aa0f4116 1239 if ( t_restart ){ // only start the timer if it isnt already started
jamesheavey 58:cb32aa0f4116 1240 t_coord.start();
jamesheavey 58:cb32aa0f4116 1241 t_restart = false;
jamesheavey 58:cb32aa0f4116 1242 }
jamesheavey 58:cb32aa0f4116 1243
jamesheavey 58:cb32aa0f4116 1244 follow_line();
jamesheavey 58:cb32aa0f4116 1245
jamesheavey 58:cb32aa0f4116 1246 if( junction_detect() ) {
jamesheavey 58:cb32aa0f4116 1247 float time = t_coord.read();
jamesheavey 58:cb32aa0f4116 1248 int dist_est = ceil(time*2); // scaled so that a longer straight will have a different time to a shorter straight
jamesheavey 58:cb32aa0f4116 1249 t_coord.stop();
jamesheavey 58:cb32aa0f4116 1250 t_coord.reset();
jamesheavey 58:cb32aa0f4116 1251 t_restart = true; //restart the timer next loop
jamesheavey 58:cb32aa0f4116 1252
jamesheavey 58:cb32aa0f4116 1253 if (dir == 'N'){ curr_coords[1] += dist_est; } // y coord
jamesheavey 58:cb32aa0f4116 1254 if (dir == 'E'){ curr_coords[0] += dist_est; } // x coord
jamesheavey 58:cb32aa0f4116 1255 if (dir == 'S'){ curr_coords[1] -= dist_est; }
jamesheavey 58:cb32aa0f4116 1256 if (dir == 'W'){ curr_coords[0] -= dist_est; }
jamesheavey 58:cb32aa0f4116 1257
jamesheavey 58:cb32aa0f4116 1258 update_index();
jamesheavey 65:2103a02d72bc 1259
jamesheavey 65:2103a02d72bc 1260 if(point[curr_index] == 100 && _switch == false) { _switch = true; break;}
jamesheavey 65:2103a02d72bc 1261 if(point[curr_index] == 0 && _switch == true) { _switch = false; break;}
jamesheavey 65:2103a02d72bc 1262
jamesheavey 58:cb32aa0f4116 1263 choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
jamesheavey 58:cb32aa0f4116 1264
jamesheavey 58:cb32aa0f4116 1265 }
jamesheavey 58:cb32aa0f4116 1266 }
jamesheavey 58:cb32aa0f4116 1267
jamesheavey 65:2103a02d72bc 1268 turn_select('B');
jamesheavey 64:a58d8538132a 1269 robot.stop();
jamesheavey 65:2103a02d72bc 1270
jamesheavey 65:2103a02d72bc 1271 if( dir == 'N' ) { dir = 'S'; }
jamesheavey 65:2103a02d72bc 1272 else if( dir == 'S' ) { dir = 'N'; }
jamesheavey 65:2103a02d72bc 1273 else if( dir == 'E' ) { dir = 'W'; }
jamesheavey 65:2103a02d72bc 1274 else if( dir == 'W' ) { dir = 'E'; }
jamesheavey 64:a58d8538132a 1275
jamesheavey 58:cb32aa0f4116 1276 }
jamesheavey 58:cb32aa0f4116 1277 }
jamesheavey 58:cb32aa0f4116 1278
jamesheavey 58:cb32aa0f4116 1279 void goal_path_explored( bool inverse )
jamesheavey 58:cb32aa0f4116 1280 {
jamesheavey 58:cb32aa0f4116 1281 char dir_diff;
jamesheavey 58:cb32aa0f4116 1282
jamesheavey 65:2103a02d72bc 1283 if ( inverse ) {
jamesheavey 58:cb32aa0f4116 1284 for(int j = goal_length1; j >= 0; j--) {
jamesheavey 58:cb32aa0f4116 1285 int curr_node = path_to_point_index(goal_path1[j]);
jamesheavey 60:ea69bd1a9889 1286 int next_node = path_to_point_index(goal_path1[j-1]);
jamesheavey 58:cb32aa0f4116 1287 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 58:cb32aa0f4116 1288 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 58:cb32aa0f4116 1289 dir_diff = 'E';
jamesheavey 58:cb32aa0f4116 1290 } else {
jamesheavey 58:cb32aa0f4116 1291 dir_diff = 'W';
jamesheavey 58:cb32aa0f4116 1292 }
jamesheavey 58:cb32aa0f4116 1293 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 58:cb32aa0f4116 1294 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 58:cb32aa0f4116 1295 dir_diff = 'N';
jamesheavey 58:cb32aa0f4116 1296 } else {
jamesheavey 58:cb32aa0f4116 1297 dir_diff = 'S';
jamesheavey 58:cb32aa0f4116 1298 }
jamesheavey 58:cb32aa0f4116 1299 }
jamesheavey 58:cb32aa0f4116 1300
jamesheavey 58:cb32aa0f4116 1301 if( dir_diff == 'N' ) {
jamesheavey 58:cb32aa0f4116 1302 explored[curr_node] &= 0b1011;
jamesheavey 58:cb32aa0f4116 1303 }
jamesheavey 58:cb32aa0f4116 1304 else if( dir_diff == 'E' ) {
jamesheavey 58:cb32aa0f4116 1305 explored[curr_node] &= 0b1101;
jamesheavey 58:cb32aa0f4116 1306 }
jamesheavey 58:cb32aa0f4116 1307 else if( dir_diff == 'S' ) {
jamesheavey 58:cb32aa0f4116 1308 explored[curr_node] &= 0b1110;
jamesheavey 58:cb32aa0f4116 1309 }
jamesheavey 58:cb32aa0f4116 1310 else if( dir_diff == 'W' ) {
jamesheavey 58:cb32aa0f4116 1311 explored[curr_node] &= 0b0111;
jamesheavey 58:cb32aa0f4116 1312 }
jamesheavey 58:cb32aa0f4116 1313 }
jamesheavey 58:cb32aa0f4116 1314 }
jamesheavey 58:cb32aa0f4116 1315
jamesheavey 58:cb32aa0f4116 1316 else{
jamesheavey 58:cb32aa0f4116 1317
jamesheavey 58:cb32aa0f4116 1318 for(int j = 0; j <= goal_length1; j++) {
jamesheavey 58:cb32aa0f4116 1319 int curr_node = path_to_point_index(goal_path1[j]);
jamesheavey 60:ea69bd1a9889 1320 int next_node = path_to_point_index(goal_path1[j+1]);
jamesheavey 58:cb32aa0f4116 1321 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 58:cb32aa0f4116 1322 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 58:cb32aa0f4116 1323 dir_diff = 'E';
jamesheavey 58:cb32aa0f4116 1324 } else {
jamesheavey 58:cb32aa0f4116 1325 dir_diff = 'W';
jamesheavey 58:cb32aa0f4116 1326 }
jamesheavey 58:cb32aa0f4116 1327 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 58:cb32aa0f4116 1328 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 58:cb32aa0f4116 1329 dir_diff = 'N';
jamesheavey 58:cb32aa0f4116 1330 } else {
jamesheavey 58:cb32aa0f4116 1331 dir_diff = 'S';
jamesheavey 58:cb32aa0f4116 1332 }
jamesheavey 58:cb32aa0f4116 1333 }
jamesheavey 58:cb32aa0f4116 1334
jamesheavey 58:cb32aa0f4116 1335 if( dir_diff == 'N' ) {
jamesheavey 58:cb32aa0f4116 1336 explored[curr_node] &= 0b1011;
jamesheavey 58:cb32aa0f4116 1337 }
jamesheavey 58:cb32aa0f4116 1338 else if( dir_diff == 'E' ) {
jamesheavey 58:cb32aa0f4116 1339 explored[curr_node] &= 0b1101;
jamesheavey 58:cb32aa0f4116 1340 }
jamesheavey 58:cb32aa0f4116 1341 else if( dir_diff == 'S' ) {
jamesheavey 58:cb32aa0f4116 1342 explored[curr_node] &= 0b1110;
jamesheavey 58:cb32aa0f4116 1343 }
jamesheavey 58:cb32aa0f4116 1344 else if( dir_diff == 'W' ) {
jamesheavey 58:cb32aa0f4116 1345 explored[curr_node] &= 0b0111;
jamesheavey 58:cb32aa0f4116 1346 }
jamesheavey 58:cb32aa0f4116 1347 }
jamesheavey 29:ecf497c3fdc0 1348 }
jamesheavey 29:ecf497c3fdc0 1349 }
jamesheavey 29:ecf497c3fdc0 1350
jamesheavey 10:691c02b352cb 1351 void invert_path()
jamesheavey 5:ae417756235a 1352 {
jamesheavey 10:691c02b352cb 1353 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 1354 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 1355 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 1356 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 1357 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 1358 }
jamesheavey 28:63ff8290964a 1359 }
jamesheavey 37:2967f3f9c936 1360
jamesheavey 37:2967f3f9c936 1361 void print_data(char *word)
jamesheavey 37:2967f3f9c936 1362 {
jamesheavey 37:2967f3f9c936 1363 robot.lcd_clear();
jamesheavey 37:2967f3f9c936 1364 robot.lcd_print(word,10);
jamesheavey 37:2967f3f9c936 1365 robot.stop();
jamesheavey 37:2967f3f9c936 1366 wait(2);
jamesheavey 37:2967f3f9c936 1367
jamesheavey 37:2967f3f9c936 1368 char buffer1[2];
jamesheavey 37:2967f3f9c936 1369 char buffer2[2];
jamesheavey 37:2967f3f9c936 1370 char buffer3[2];
jamesheavey 37:2967f3f9c936 1371 robot.lcd_clear();
jamesheavey 54:5ff74beb043e 1372 sprintf(buffer1,"%x",type[curr_index]);
jamesheavey 54:5ff74beb043e 1373 sprintf(buffer2,"%x",explored[curr_index]);
jamesheavey 37:2967f3f9c936 1374
jamesheavey 54:5ff74beb043e 1375 // sprintf(buffer1,"%d",curr_coords[0]);
jamesheavey 54:5ff74beb043e 1376 // sprintf(buffer2,"%d",curr_coords[1]);
jamesheavey 37:2967f3f9c936 1377 sprintf(buffer3,"%d",curr_index);
jamesheavey 37:2967f3f9c936 1378 robot.lcd_print(buffer1,2);
jamesheavey 37:2967f3f9c936 1379 robot.lcd_goto_xy(0,1);
jamesheavey 37:2967f3f9c936 1380 robot.lcd_print(buffer2,2);
jamesheavey 37:2967f3f9c936 1381 robot.lcd_goto_xy(5,0);
jamesheavey 37:2967f3f9c936 1382 char *b = &dir;
jamesheavey 37:2967f3f9c936 1383 robot.lcd_print(b,2);
jamesheavey 37:2967f3f9c936 1384 robot.lcd_goto_xy(5,1);
jamesheavey 37:2967f3f9c936 1385 robot.lcd_print(buffer3,2);
jamesheavey 37:2967f3f9c936 1386
jamesheavey 37:2967f3f9c936 1387 robot.stop();
jamesheavey 37:2967f3f9c936 1388 wait(2);
jamesheavey 37:2967f3f9c936 1389 }