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@55:18ea9529afb2, 2020-04-17 (annotated)

Committer:
jamesheavey
Date:
Fri Apr 17 23:52:23 2020 +0000
Revision:
55:18ea9529afb2
Parent:
54:5ff74beb043e
Child:
56:1cd434b28322
YESSS

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 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 54:5ff74beb043e 444 // if( turn_angle != 180 ) { robot.forward(speed); wait(0.05); } // maybe remove
jamesheavey 54:5ff74beb043e 445
jamesheavey 54:5ff74beb043e 446 if( turn_angle == 0) { robot.forward(speed); wait(0.1); turn_select('S'); }
jamesheavey 32:598bedb22c7c 447 else if( turn_angle == 90) { turn_select('R'); }
jamesheavey 48:76cf4521d342 448 else if( turn_angle == 180) { turn_select('B'); }
jamesheavey 32:598bedb22c7c 449 else if( turn_angle == 270) { turn_select('L'); }
jamesheavey 21:54ea75f7984f 450 }
jamesheavey 21:54ea75f7984f 451
jamesheavey 28:63ff8290964a 452 void back_track()
jamesheavey 28:63ff8290964a 453 {
jamesheavey 33:9fa9e09f2e8f 454 // 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 455 // also if no nodes have unexplored paths set complete to true
jamesheavey 29:ecf497c3fdc0 456
jamesheavey 35:2fd4ee9ac889 457 bool complete = false;
jamesheavey 48:76cf4521d342 458
jamesheavey 48:76cf4521d342 459 int d_node; // an index to the most recent (desired) node with unexplored paths
jamesheavey 29:ecf497c3fdc0 460
jamesheavey 34:63f7c61ee4da 461 for(int i = total_points; i >= 0; i--) { // start from the most recently discovered node
jamesheavey 34:63f7c61ee4da 462 if( explored[i] != type[i] ) {
jamesheavey 35:2fd4ee9ac889 463 complete = true;
jamesheavey 48:76cf4521d342 464 d_node = i;
jamesheavey 34:63f7c61ee4da 465 break;
jamesheavey 34:63f7c61ee4da 466 }
jamesheavey 29:ecf497c3fdc0 467 }
jamesheavey 29:ecf497c3fdc0 468
jamesheavey 35:2fd4ee9ac889 469 if( complete == false ) { looped_goal(); }
jamesheavey 34:63f7c61ee4da 470
jamesheavey 34:63f7c61ee4da 471 else {
jamesheavey 34:63f7c61ee4da 472 // compare node coordinates to previous node coordinates
jamesheavey 34:63f7c61ee4da 473 // determine which direction the previous node is compared to current
jamesheavey 34:63f7c61ee4da 474 // set the current nodes direction path to 0 and the opposite direction path to 0
jamesheavey 34:63f7c61ee4da 475 // decrement the count (setting the previous as current node and the next previous as previous)
jamesheavey 34:63f7c61ee4da 476 // when previous node == point[pointer1] break
jamesheavey 34:63f7c61ee4da 477 // choose turn should then do all those turns and arrive at correct node
jamesheavey 48:76cf4521d342 478
jamesheavey 48:76cf4521d342 479 // check if the current node exists before the discovery of the desired node
jamesheavey 48:76cf4521d342 480 // if it does, check the number of nodes between before and after
jamesheavey 48:76cf4521d342 481 // which ever is shorter, set the 0 of those nodes
jamesheavey 48:76cf4521d342 482
jamesheavey 55:18ea9529afb2 483 // robot.stop();
jamesheavey 55:18ea9529afb2 484 // char buf1[2], buf2[2];
jamesheavey 55:18ea9529afb2 485 // sprintf(buf1,"%d",point[d_node]);
jamesheavey 55:18ea9529afb2 486 // sprintf(buf2,"%d",point[curr_index]);
jamesheavey 55:18ea9529afb2 487 // robot.lcd_print(buf1,2);
jamesheavey 55:18ea9529afb2 488 // robot.lcd_goto_xy(0,1);
jamesheavey 55:18ea9529afb2 489 // robot.lcd_print(buf2,2);
jamesheavey 55:18ea9529afb2 490 //
jamesheavey 55:18ea9529afb2 491 // wait(1);
jamesheavey 55:18ea9529afb2 492 // robot.lcd_clear();
jamesheavey 54:5ff74beb043e 493
jamesheavey 53:dbb63b181faf 494
jamesheavey 55:18ea9529afb2 495 bool check = dead_end_removal(point[d_node], point[curr_index]);
jamesheavey 53:dbb63b181faf 496 char dir_diff;
jamesheavey 53:dbb63b181faf 497
jamesheavey 55:18ea9529afb2 498 if(check) {
jamesheavey 54:5ff74beb043e 499 for(int j = 0; j <= short_length; j++) {
jamesheavey 54:5ff74beb043e 500 int curr_node = path_to_point_index(shortest[j]);
jamesheavey 54:5ff74beb043e 501 int next_node = path_to_point_index(shortest[j+1]);
jamesheavey 54:5ff74beb043e 502 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 54:5ff74beb043e 503 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 54:5ff74beb043e 504 dir_diff = 'E';
jamesheavey 54:5ff74beb043e 505 } else {
jamesheavey 54:5ff74beb043e 506 dir_diff = 'W';
jamesheavey 54:5ff74beb043e 507 }
jamesheavey 54:5ff74beb043e 508 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 54:5ff74beb043e 509 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 54:5ff74beb043e 510 dir_diff = 'N';
jamesheavey 54:5ff74beb043e 511 } else {
jamesheavey 54:5ff74beb043e 512 dir_diff = 'S';
jamesheavey 54:5ff74beb043e 513 }
jamesheavey 54:5ff74beb043e 514 }
jamesheavey 54:5ff74beb043e 515
jamesheavey 54:5ff74beb043e 516 if( dir_diff == 'N' ) {
jamesheavey 54:5ff74beb043e 517 explored[curr_node] &= 0b1011;
jamesheavey 54:5ff74beb043e 518 }
jamesheavey 54:5ff74beb043e 519 else if( dir_diff == 'E' ) {
jamesheavey 54:5ff74beb043e 520 explored[curr_node] &= 0b1101;
jamesheavey 54:5ff74beb043e 521 }
jamesheavey 54:5ff74beb043e 522 else if( dir_diff == 'S' ) {
jamesheavey 54:5ff74beb043e 523 explored[curr_node] &= 0b1110;
jamesheavey 54:5ff74beb043e 524 }
jamesheavey 54:5ff74beb043e 525 else if( dir_diff == 'W' ) {
jamesheavey 54:5ff74beb043e 526 explored[curr_node] &= 0b0111;
jamesheavey 53:dbb63b181faf 527 }
jamesheavey 54:5ff74beb043e 528 if(point[next_node] == point[d_node]) { break; }
jamesheavey 54:5ff74beb043e 529 }
jamesheavey 54:5ff74beb043e 530 }
jamesheavey 54:5ff74beb043e 531
jamesheavey 54:5ff74beb043e 532 else {
jamesheavey 54:5ff74beb043e 533
jamesheavey 54:5ff74beb043e 534 for(int j = short_length; j >= 0; j--) {
jamesheavey 54:5ff74beb043e 535 int curr_node = path_to_point_index(shortest[j]);
jamesheavey 54:5ff74beb043e 536 int next_node = path_to_point_index(shortest[j-1]);
jamesheavey 54:5ff74beb043e 537 if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 54:5ff74beb043e 538 if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 54:5ff74beb043e 539 dir_diff = 'E';
jamesheavey 54:5ff74beb043e 540 } else {
jamesheavey 54:5ff74beb043e 541 dir_diff = 'W';
jamesheavey 54:5ff74beb043e 542 }
jamesheavey 54:5ff74beb043e 543 } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 54:5ff74beb043e 544 if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 54:5ff74beb043e 545 dir_diff = 'N';
jamesheavey 54:5ff74beb043e 546 } else {
jamesheavey 54:5ff74beb043e 547 dir_diff = 'S';
jamesheavey 54:5ff74beb043e 548 }
jamesheavey 53:dbb63b181faf 549 }
jamesheavey 54:5ff74beb043e 550
jamesheavey 54:5ff74beb043e 551 if( dir_diff == 'N' ) {
jamesheavey 54:5ff74beb043e 552 explored[curr_node] &= 0b1011;
jamesheavey 54:5ff74beb043e 553 }
jamesheavey 54:5ff74beb043e 554 else if( dir_diff == 'E' ) {
jamesheavey 54:5ff74beb043e 555 explored[curr_node] &= 0b1101;
jamesheavey 54:5ff74beb043e 556 }
jamesheavey 54:5ff74beb043e 557 else if( dir_diff == 'S' ) {
jamesheavey 54:5ff74beb043e 558 explored[curr_node] &= 0b1110;
jamesheavey 54:5ff74beb043e 559 }
jamesheavey 54:5ff74beb043e 560 else if( dir_diff == 'W' ) {
jamesheavey 54:5ff74beb043e 561 explored[curr_node] &= 0b0111;
jamesheavey 54:5ff74beb043e 562 }
jamesheavey 54:5ff74beb043e 563 if(point[next_node] == point[d_node]) { break; }
jamesheavey 53:dbb63b181faf 564 }
jamesheavey 53:dbb63b181faf 565 }
jamesheavey 54:5ff74beb043e 566
jamesheavey 54:5ff74beb043e 567 // print_data("aft bt");
jamesheavey 53:dbb63b181faf 568
jamesheavey 53:dbb63b181faf 569 //int curr_node = curr_index;
jamesheavey 53:dbb63b181faf 570 // bool before = false;
jamesheavey 53:dbb63b181faf 571 // bool after = false;
jamesheavey 53:dbb63b181faf 572 // int before_index;
jamesheavey 53:dbb63b181faf 573 // int after_index;
jamesheavey 52:79f21d15ede1 574 //
jamesheavey 53:dbb63b181faf 575 // bool desired_discovered = false;
jamesheavey 53:dbb63b181faf 576 // for(int k = 0; k <= path_length; k++) {
jamesheavey 53:dbb63b181faf 577 // if(looped_path[k] == point[d_node]) { desired_discovered = true; } // maybe set a variable
jamesheavey 53:dbb63b181faf 578 // if(desired_discovered == false && looped_path[k] == point[curr_node]) { before = true; before_index = k; }
jamesheavey 53:dbb63b181faf 579 // if(desired_discovered == true && looped_path[k] == point[curr_node]) { after_index = k; break; }
jamesheavey 53:dbb63b181faf 580 // }
jamesheavey 53:dbb63b181faf 581 //
jamesheavey 53:dbb63b181faf 582 //
jamesheavey 53:dbb63b181faf 583 // if( before_index < after_index ){
jamesheavey 53:dbb63b181faf 584 // before = true;
jamesheavey 53:dbb63b181faf 585 // after = false;
jamesheavey 53:dbb63b181faf 586 // }
jamesheavey 53:dbb63b181faf 587 // else{
jamesheavey 53:dbb63b181faf 588 // before = false;
jamesheavey 53:dbb63b181faf 589 // after = true;
jamesheavey 53:dbb63b181faf 590 // }
jamesheavey 53:dbb63b181faf 591 //
jamesheavey 53:dbb63b181faf 592 //
jamesheavey 49:89f0f54aa46b 593 // char dir_diff;
jamesheavey 49:89f0f54aa46b 594 //
jamesheavey 53:dbb63b181faf 595 // if(after == true) {
jamesheavey 53:dbb63b181faf 596 //
jamesheavey 53:dbb63b181faf 597 // for(int j = after_index; j >= 0; j--) {
jamesheavey 53:dbb63b181faf 598 // curr_node = path_to_point_index(looped_path[j]);
jamesheavey 53:dbb63b181faf 599 //
jamesheavey 53:dbb63b181faf 600 // int prev_node = path_to_point_index(looped_path[j-1]);
jamesheavey 53:dbb63b181faf 601 // if(coords_x[prev_node] != coords_x[curr_node]) {
jamesheavey 53:dbb63b181faf 602 // if(coords_x[prev_node] - coords_x[curr_node] > 0){
jamesheavey 53:dbb63b181faf 603 // dir_diff = 'E';
jamesheavey 53:dbb63b181faf 604 // } else {
jamesheavey 53:dbb63b181faf 605 // dir_diff = 'W';
jamesheavey 53:dbb63b181faf 606 // }
jamesheavey 53:dbb63b181faf 607 // } else if( coords_y[prev_node] != coords_y[curr_node] ) {
jamesheavey 53:dbb63b181faf 608 // if(coords_y[prev_node] - coords_y[curr_node] > 0){
jamesheavey 53:dbb63b181faf 609 // dir_diff = 'N';
jamesheavey 53:dbb63b181faf 610 // } else {
jamesheavey 53:dbb63b181faf 611 // dir_diff = 'S';
jamesheavey 53:dbb63b181faf 612 // }
jamesheavey 49:89f0f54aa46b 613 // }
jamesheavey 53:dbb63b181faf 614 //
jamesheavey 53:dbb63b181faf 615 // if( dir_diff == 'N' ) {
jamesheavey 53:dbb63b181faf 616 // explored[curr_node] &= 0b1011;
jamesheavey 53:dbb63b181faf 617 // }
jamesheavey 53:dbb63b181faf 618 // else if( dir_diff == 'E' ) {
jamesheavey 53:dbb63b181faf 619 // explored[curr_node] &= 0b1101;
jamesheavey 53:dbb63b181faf 620 // }
jamesheavey 53:dbb63b181faf 621 // else if( dir_diff == 'S' ) {
jamesheavey 53:dbb63b181faf 622 // explored[curr_node] &= 0b1110;
jamesheavey 53:dbb63b181faf 623 // }
jamesheavey 53:dbb63b181faf 624 // else if( dir_diff == 'W' ) {
jamesheavey 53:dbb63b181faf 625 // explored[curr_node] &= 0b0111;
jamesheavey 53:dbb63b181faf 626 // }
jamesheavey 53:dbb63b181faf 627 // if(point[prev_node] == point[d_node]) { break; }
jamesheavey 49:89f0f54aa46b 628 // }
jamesheavey 49:89f0f54aa46b 629 // }
jamesheavey 53:dbb63b181faf 630 //
jamesheavey 53:dbb63b181faf 631 // else if( before == true ) {
jamesheavey 53:dbb63b181faf 632 // for(int j = before_index; j <= path_length; j++) {
jamesheavey 53:dbb63b181faf 633 // curr_node = path_to_point_index(looped_path[j]);
jamesheavey 53:dbb63b181faf 634 //
jamesheavey 53:dbb63b181faf 635 // int next_node = path_to_point_index(looped_path[j+1]);
jamesheavey 53:dbb63b181faf 636 // if(coords_x[next_node] != coords_x[curr_node]) {
jamesheavey 53:dbb63b181faf 637 // if(coords_x[next_node] - coords_x[curr_node] > 0){
jamesheavey 53:dbb63b181faf 638 // dir_diff = 'E';
jamesheavey 53:dbb63b181faf 639 // } else {
jamesheavey 53:dbb63b181faf 640 // dir_diff = 'W';
jamesheavey 53:dbb63b181faf 641 // }
jamesheavey 53:dbb63b181faf 642 // } else if( coords_y[next_node] != coords_y[curr_node] ) {
jamesheavey 53:dbb63b181faf 643 // if(coords_y[next_node] - coords_y[curr_node] > 0){
jamesheavey 53:dbb63b181faf 644 // dir_diff = 'N';
jamesheavey 53:dbb63b181faf 645 // } else {
jamesheavey 53:dbb63b181faf 646 // dir_diff = 'S';
jamesheavey 53:dbb63b181faf 647 // }
jamesheavey 53:dbb63b181faf 648 // }
jamesheavey 53:dbb63b181faf 649 //
jamesheavey 53:dbb63b181faf 650 // if( dir_diff == 'N' ) {
jamesheavey 53:dbb63b181faf 651 // explored[curr_node] &= 0b1011;
jamesheavey 53:dbb63b181faf 652 // }
jamesheavey 53:dbb63b181faf 653 // else if( dir_diff == 'E' ) {
jamesheavey 53:dbb63b181faf 654 // explored[curr_node] &= 0b1101;
jamesheavey 53:dbb63b181faf 655 // }
jamesheavey 53:dbb63b181faf 656 // else if( dir_diff == 'S' ) {
jamesheavey 53:dbb63b181faf 657 // explored[curr_node] &= 0b1110;
jamesheavey 53:dbb63b181faf 658 // }
jamesheavey 53:dbb63b181faf 659 // else if( dir_diff == 'W' ) {
jamesheavey 53:dbb63b181faf 660 // explored[curr_node] &= 0b0111;
jamesheavey 53:dbb63b181faf 661 // }
jamesheavey 53:dbb63b181faf 662 // if(point[next_node] == point[d_node]) { break; }
jamesheavey 53:dbb63b181faf 663 // }
jamesheavey 53:dbb63b181faf 664 // }
jamesheavey 34:63f7c61ee4da 665 }
jamesheavey 28:63ff8290964a 666 }
jamesheavey 28:63ff8290964a 667
jamesheavey 55:18ea9529afb2 668 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 669 {
jamesheavey 48:76cf4521d342 670 // dead end removal between start and final node
jamesheavey 48:76cf4521d342 671 // add the inverse of the result to end of the looped_path
jamesheavey 48:76cf4521d342 672 // then separate into before and after, simplify both and compare
jamesheavey 51:3148875bb9e5 673 robot.stop();
jamesheavey 49:89f0f54aa46b 674 int index1, index2;
jamesheavey 49:89f0f54aa46b 675 int d_index, before_index, after_index;
jamesheavey 49:89f0f54aa46b 676 bool before = false;
jamesheavey 49:89f0f54aa46b 677
jamesheavey 49:89f0f54aa46b 678 bool desired_discovered = false;
jamesheavey 49:89f0f54aa46b 679
jamesheavey 49:89f0f54aa46b 680 for(int k = 0; k <= path_length; k++) {
jamesheavey 52:79f21d15ede1 681 if(looped_path[k] == point1) { desired_discovered = true; d_index = k; } // maybe set a variable
jamesheavey 52:79f21d15ede1 682 if(desired_discovered == false && looped_path[k] == point2) { before = true; before_index = k; }
jamesheavey 52:79f21d15ede1 683 if(desired_discovered == true && looped_path[k] == point2) { after_index = k; break; }
jamesheavey 49:89f0f54aa46b 684 }
jamesheavey 49:89f0f54aa46b 685
jamesheavey 51:3148875bb9e5 686 if(before){
jamesheavey 51:3148875bb9e5 687 if( (d_index - before_index) <= (after_index - d_index) ){
jamesheavey 51:3148875bb9e5 688 before = true;
jamesheavey 51:3148875bb9e5 689 }
jamesheavey 51:3148875bb9e5 690 else{
jamesheavey 51:3148875bb9e5 691 before = false;
jamesheavey 51:3148875bb9e5 692 }
jamesheavey 49:89f0f54aa46b 693 }
jamesheavey 49:89f0f54aa46b 694
jamesheavey 49:89f0f54aa46b 695 if(before) {
jamesheavey 49:89f0f54aa46b 696 index1 = before_index;
jamesheavey 49:89f0f54aa46b 697 index2 = d_index;
jamesheavey 55:18ea9529afb2 698
jamesheavey 49:89f0f54aa46b 699 }else{
jamesheavey 49:89f0f54aa46b 700 index1 = d_index;
jamesheavey 49:89f0f54aa46b 701 index2 = after_index;
jamesheavey 49:89f0f54aa46b 702 }
jamesheavey 49:89f0f54aa46b 703
jamesheavey 48:76cf4521d342 704 int temp_array[100];
jamesheavey 48:76cf4521d342 705
jamesheavey 48:76cf4521d342 706 for( int x = 0; x <= path_length; x++ ) { shortest[x] = NULL; }
jamesheavey 48:76cf4521d342 707
jamesheavey 50:fa124ea1df9f 708 for( int i = index1; i <= index2; i++ ) { shortest[i-index1] = looped_path[i]; }
jamesheavey 48:76cf4521d342 709
jamesheavey 55:18ea9529afb2 710 short_length = index2 - index1;
jamesheavey 48:76cf4521d342 711
jamesheavey 54:5ff74beb043e 712 // robot.lcd_print("l bef",10);
jamesheavey 54:5ff74beb043e 713 // wait(2);
jamesheavey 54:5ff74beb043e 714 // robot.lcd_clear();
jamesheavey 54:5ff74beb043e 715 //
jamesheavey 54:5ff74beb043e 716 // char buffer[3];
jamesheavey 55:18ea9529afb2 717 // sprintf(buffer,"%d", short_length);
jamesheavey 54:5ff74beb043e 718 // robot.lcd_print(buffer,3);
jamesheavey 54:5ff74beb043e 719 // wait(2);
jamesheavey 54:5ff74beb043e 720 // robot.lcd_clear();
jamesheavey 48:76cf4521d342 721
jamesheavey 48:76cf4521d342 722 // for every node in path, check if repeated
jamesheavey 48:76cf4521d342 723 // if rpeated, move everyting before the first occurence to the final occurence
jamesheavey 48:76cf4521d342 724 // save new path and repeat
jamesheavey 48:76cf4521d342 725 // when no node is repeated, end
jamesheavey 48:76cf4521d342 726 // or end when iterated through the entire array
jamesheavey 48:76cf4521d342 727
jamesheavey 48:76cf4521d342 728 int i = 0;
jamesheavey 55:18ea9529afb2 729 while( i <= short_length ) {
jamesheavey 48:76cf4521d342 730 int count = i;
jamesheavey 55:18ea9529afb2 731 for( int j = 0; j <= short_length; j++ ) {
jamesheavey 48:76cf4521d342 732 if( shortest[i] == shortest[j] ){
jamesheavey 48:76cf4521d342 733 count = j;
jamesheavey 48:76cf4521d342 734 }
jamesheavey 39:005ad4610152 735 }
jamesheavey 48:76cf4521d342 736 if( count != i ) {
jamesheavey 48:76cf4521d342 737
jamesheavey 48:76cf4521d342 738 for( int k = 0; k <= i; k++ ) {
jamesheavey 48:76cf4521d342 739 temp_array[k] = shortest[k];
jamesheavey 48:76cf4521d342 740 }
jamesheavey 48:76cf4521d342 741 int ind = 1;
jamesheavey 48:76cf4521d342 742 for( int z = count+1; z <= path_length; z++ ) {
jamesheavey 48:76cf4521d342 743 temp_array[i+ind] = shortest[z];
jamesheavey 48:76cf4521d342 744 ind++;
jamesheavey 48:76cf4521d342 745 }
jamesheavey 48:76cf4521d342 746 // clear the array
jamesheavey 55:18ea9529afb2 747 for( int x = 0; x <= short_length; x++ ) { shortest[x] = NULL; }
jamesheavey 48:76cf4521d342 748
jamesheavey 55:18ea9529afb2 749 short_length -= (count-i);
jamesheavey 48:76cf4521d342 750 i = -1;
jamesheavey 48:76cf4521d342 751
jamesheavey 55:18ea9529afb2 752 for( int x = 0; x <= short_length; x++ ) { shortest[x] = temp_array[x]; }
jamesheavey 48:76cf4521d342 753
jamesheavey 55:18ea9529afb2 754 for( int x = 0; x <= short_length; x++ ) { temp_array[x] = NULL; }
jamesheavey 39:005ad4610152 755 }
jamesheavey 48:76cf4521d342 756 i++;
jamesheavey 39:005ad4610152 757 }
jamesheavey 39:005ad4610152 758
jamesheavey 55:18ea9529afb2 759 // robot.lcd_print("l af",10);
jamesheavey 55:18ea9529afb2 760 // wait(1);
jamesheavey 55:18ea9529afb2 761 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 762 //
jamesheavey 55:18ea9529afb2 763 // char buffe[3];
jamesheavey 55:18ea9529afb2 764 // sprintf(buffe,"%d", short_length);
jamesheavey 55:18ea9529afb2 765 // robot.lcd_print(buffe,3);
jamesheavey 55:18ea9529afb2 766 // wait(1);
jamesheavey 55:18ea9529afb2 767 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 768 //
jamesheavey 55:18ea9529afb2 769 // robot.lcd_print("path",10);
jamesheavey 55:18ea9529afb2 770 // wait(1);
jamesheavey 55:18ea9529afb2 771 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 772 //
jamesheavey 55:18ea9529afb2 773 // leds = 0b1111;
jamesheavey 55:18ea9529afb2 774 // for(int m = 0; m <= short_length; m++) {
jamesheavey 55:18ea9529afb2 775 // leds = ~leds;
jamesheavey 55:18ea9529afb2 776 // char buffer[3];
jamesheavey 55:18ea9529afb2 777 // sprintf(buffer,"%d", shortest[m]);
jamesheavey 55:18ea9529afb2 778 // robot.lcd_print(buffer,3);
jamesheavey 55:18ea9529afb2 779 // wait(1);
jamesheavey 55:18ea9529afb2 780 // robot.lcd_clear();
jamesheavey 55:18ea9529afb2 781 // }
jamesheavey 53:dbb63b181faf 782
jamesheavey 55:18ea9529afb2 783 return before;
jamesheavey 48:76cf4521d342 784 // print these to check
jamesheavey 48:76cf4521d342 785 // add inverted sub path to the real path and increment the path length by real_len -1
jamesheavey 39:005ad4610152 786 }
jamesheavey 39:005ad4610152 787
jamesheavey 10:691c02b352cb 788 void follow_line()
jamesheavey 0:df5216b20861 789 {
jamesheavey 10:691c02b352cb 790 robot.scan();
jamesheavey 10:691c02b352cb 791 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 792
jamesheavey 15:6c461501d12d 793 leds = 0b0110;
jamesheavey 15:6c461501d12d 794
jamesheavey 10:691c02b352cb 795 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 796 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 797 integral += proportional;
jamesheavey 10:691c02b352cb 798 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 799
jamesheavey 10:691c02b352cb 800 // calculate motor correction
jamesheavey 10:691c02b352cb 801 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 802
jamesheavey 10:691c02b352cb 803 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 804 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 805 motor_correction = speed;
jamesheavey 10:691c02b352cb 806 }
jamesheavey 10:691c02b352cb 807 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 808 motor_correction = -speed;
jamesheavey 10:691c02b352cb 809 }
jamesheavey 0:df5216b20861 810
jamesheavey 10:691c02b352cb 811 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 812 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 813 } else {
jamesheavey 10:691c02b352cb 814 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 815 }
jamesheavey 18:991658b628fc 816
jamesheavey 19:4c08275cb3c9 817 // read_encoders();
jamesheavey 19:4c08275cb3c9 818 // 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 819 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 820 }
jamesheavey 0:df5216b20861 821
jamesheavey 14:87052bb35211 822 bool junction_detect()
jamesheavey 6:c10b367747a0 823 {
jamesheavey 20:5cf6a378801d 824 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 825 return true;
jamesheavey 20:5cf6a378801d 826 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 827 return true;
jamesheavey 14:87052bb35211 828 } else {
jamesheavey 14:87052bb35211 829 return false;
jamesheavey 14:87052bb35211 830 }
jamesheavey 14:87052bb35211 831 }
jamesheavey 14:87052bb35211 832
jamesheavey 14:87052bb35211 833 char junction_logic()
jamesheavey 14:87052bb35211 834 {
jamesheavey 14:87052bb35211 835 bool straight = false;
jamesheavey 7:7fefd782532d 836 bool left = false;
jamesheavey 6:c10b367747a0 837 bool right = false;
jamesheavey 9:952586accbf9 838 bool goal = false;
jamesheavey 7:7fefd782532d 839
jamesheavey 20:5cf6a378801d 840 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 841 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 842 robot.forward(speed);
jamesheavey 9:952586accbf9 843 robot.scan();
jamesheavey 20:5cf6a378801d 844 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 845 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 846 }
jamesheavey 7:7fefd782532d 847
jamesheavey 20:5cf6a378801d 848 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 849 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 850 robot.scan();
jamesheavey 20:5cf6a378801d 851 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 852 goal = true;
jamesheavey 12:d80399686f32 853 }
jamesheavey 13:bd271266e161 854 }
jamesheavey 14:87052bb35211 855
jamesheavey 14:87052bb35211 856 robot.scan();
jamesheavey 14:87052bb35211 857
jamesheavey 20:5cf6a378801d 858 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 859 straight = true;
jamesheavey 14:87052bb35211 860 }
jamesheavey 14:87052bb35211 861
jamesheavey 20:5cf6a378801d 862 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 863 return 'B';
jamesheavey 6:c10b367747a0 864 }
jamesheavey 7:7fefd782532d 865
jamesheavey 9:952586accbf9 866 if (goal) {
jamesheavey 9:952586accbf9 867 return 'G';
jamesheavey 9:952586accbf9 868 } else if (left) {
jamesheavey 9:952586accbf9 869 return 'L';
jamesheavey 14:87052bb35211 870 } else if (straight) {
jamesheavey 14:87052bb35211 871 return 'S';
jamesheavey 9:952586accbf9 872 } else if (right) {
jamesheavey 9:952586accbf9 873 return 'R';
jamesheavey 6:c10b367747a0 874 } else {
jamesheavey 9:952586accbf9 875 return 'S';
jamesheavey 9:952586accbf9 876 }
jamesheavey 6:c10b367747a0 877 }
jamesheavey 6:c10b367747a0 878
jamesheavey 0:df5216b20861 879
jamesheavey 14:87052bb35211 880 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 881 {
jamesheavey 1:79219d0a33c8 882 switch(turn) {
jamesheavey 5:ae417756235a 883 case 'G':
jamesheavey 5:ae417756235a 884 goal();
jamesheavey 1:79219d0a33c8 885 case 'L':
jamesheavey 1:79219d0a33c8 886 left();
jamesheavey 5:ae417756235a 887 break;
jamesheavey 1:79219d0a33c8 888 case 'S':
jamesheavey 1:79219d0a33c8 889 break;
jamesheavey 1:79219d0a33c8 890 case 'R':
jamesheavey 1:79219d0a33c8 891 right();
jamesheavey 5:ae417756235a 892 break;
jamesheavey 1:79219d0a33c8 893 case 'B':
jamesheavey 1:79219d0a33c8 894 back();
jamesheavey 5:ae417756235a 895 break;
jamesheavey 1:79219d0a33c8 896 }
jamesheavey 1:79219d0a33c8 897 }
jamesheavey 1:79219d0a33c8 898
jamesheavey 0:df5216b20861 899 void left()
jamesheavey 0:df5216b20861 900 {
jamesheavey 1:79219d0a33c8 901 leds = 0b1100;
jamesheavey 3:a5e06482462e 902
jamesheavey 20:5cf6a378801d 903 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 904
jamesheavey 39:005ad4610152 905 robot.spin_left(TURN_SPEED);
jamesheavey 39:005ad4610152 906 wait(0.2);
jamesheavey 3:a5e06482462e 907
jamesheavey 20:5cf6a378801d 908 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 909
jamesheavey 20:5cf6a378801d 910 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 911 }
jamesheavey 1:79219d0a33c8 912
jamesheavey 1:79219d0a33c8 913 void right()
jamesheavey 1:79219d0a33c8 914 {
jamesheavey 1:79219d0a33c8 915 leds = 0b0011;
jamesheavey 5:ae417756235a 916
jamesheavey 20:5cf6a378801d 917 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 918
jamesheavey 20:5cf6a378801d 919 robot.spin_right(TURN_SPEED);
jamesheavey 39:005ad4610152 920 wait(0.2);
jamesheavey 3:a5e06482462e 921
jamesheavey 20:5cf6a378801d 922 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 923
jamesheavey 20:5cf6a378801d 924 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 925 }
jamesheavey 0:df5216b20861 926
jamesheavey 0:df5216b20861 927 void back()
jamesheavey 0:df5216b20861 928 {
jamesheavey 1:79219d0a33c8 929 leds = 0b1111;
jamesheavey 32:598bedb22c7c 930 // robot.reverse(speed);
jamesheavey 32:598bedb22c7c 931 // wait(0.1);
jamesheavey 20:5cf6a378801d 932 robot.spin_right(TURN_SPEED);
jamesheavey 41:2b6b73dd897c 933 if(loop_check == false) { // works better for looped
jamesheavey 53:dbb63b181faf 934 wait(0.65);
jamesheavey 41:2b6b73dd897c 935 }
jamesheavey 41:2b6b73dd897c 936 else {
jamesheavey 41:2b6b73dd897c 937 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 938
jamesheavey 41:2b6b73dd897c 939 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 940 }
jamesheavey 0:df5216b20861 941 }
jamesheavey 1:79219d0a33c8 942
jamesheavey 2:940e46e21353 943 void simplify()
jamesheavey 1:79219d0a33c8 944 {
jamesheavey 2:940e46e21353 945 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 946 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 947 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 948
jamesheavey 10:691c02b352cb 949 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 950 int angle_change = 0;
jamesheavey 2:940e46e21353 951
jamesheavey 10:691c02b352cb 952 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 953 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 954 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 955 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 956 }
jamesheavey 4:38c29dbc5953 957
jamesheavey 4:38c29dbc5953 958 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 959
jamesheavey 4:38c29dbc5953 960 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 961 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 962 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 963 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 964
jamesheavey 4:38c29dbc5953 965 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 966
jamesheavey 4:38c29dbc5953 967 path_length -= 2;
jamesheavey 2:940e46e21353 968 }
jamesheavey 5:ae417756235a 969 }
jamesheavey 5:ae417756235a 970
jamesheavey 10:691c02b352cb 971 void goal()
jamesheavey 10:691c02b352cb 972 {
jamesheavey 10:691c02b352cb 973 invert_path();
jamesheavey 11:c3299aca7d8f 974
jamesheavey 11:c3299aca7d8f 975 leds = 0b0000;
jamesheavey 10:691c02b352cb 976
jamesheavey 10:691c02b352cb 977 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 978 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 979
jamesheavey 11:c3299aca7d8f 980 while(1) {
jamesheavey 15:6c461501d12d 981 int pointer = 0;
jamesheavey 15:6c461501d12d 982
jamesheavey 15:6c461501d12d 983 robot.stop();
jamesheavey 15:6c461501d12d 984
jamesheavey 15:6c461501d12d 985 leds = 0b1001;
jamesheavey 15:6c461501d12d 986 wait(0.2);
jamesheavey 15:6c461501d12d 987 leds = 0b0110;
jamesheavey 15:6c461501d12d 988 wait(0.2);
jamesheavey 15:6c461501d12d 989
jamesheavey 15:6c461501d12d 990 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 991 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 992 wait(0.05);
jamesheavey 15:6c461501d12d 993
jamesheavey 20:5cf6a378801d 994 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 995 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 996 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 997 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 998
jamesheavey 15:6c461501d12d 999 robot.stop();
jamesheavey 15:6c461501d12d 1000
jamesheavey 15:6c461501d12d 1001 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 1002 follow_line();
jamesheavey 15:6c461501d12d 1003
jamesheavey 15:6c461501d12d 1004 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 1005 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 1006 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 1007 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 1008 robot.forward(speed);
jamesheavey 15:6c461501d12d 1009 leds = 0b1010;
jamesheavey 20:5cf6a378801d 1010 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 1011 }
jamesheavey 15:6c461501d12d 1012 pointer++;
jamesheavey 15:6c461501d12d 1013 }
jamesheavey 15:6c461501d12d 1014 }
jamesheavey 15:6c461501d12d 1015
jamesheavey 15:6c461501d12d 1016 back();
jamesheavey 12:d80399686f32 1017
jamesheavey 11:c3299aca7d8f 1018 robot.stop();
jamesheavey 20:5cf6a378801d 1019 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 1020 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 1021 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 1022 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 1023
jamesheavey 14:87052bb35211 1024 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 1025
jamesheavey 20:5cf6a378801d 1026 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 1027 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 1028
jamesheavey 15:6c461501d12d 1029 pointer = 0;
jamesheavey 15:6c461501d12d 1030
jamesheavey 11:c3299aca7d8f 1031 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 1032 wait(0.2);
jamesheavey 11:c3299aca7d8f 1033 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 1034 wait(0.2);
jamesheavey 11:c3299aca7d8f 1035 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 1036 wait(0.2);
jamesheavey 11:c3299aca7d8f 1037 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 1038 wait(0.2);
jamesheavey 11:c3299aca7d8f 1039
jamesheavey 11:c3299aca7d8f 1040 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 1041 follow_line();
jamesheavey 11:c3299aca7d8f 1042
jamesheavey 14:87052bb35211 1043 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 1044 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 1045 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 1046 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 1047 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 1048 leds = 0b1010;
jamesheavey 20:5cf6a378801d 1049 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 1050 }
jamesheavey 11:c3299aca7d8f 1051 pointer++;
jamesheavey 10:691c02b352cb 1052 }
jamesheavey 10:691c02b352cb 1053 }
jamesheavey 5:ae417756235a 1054 }
jamesheavey 5:ae417756235a 1055 }
jamesheavey 5:ae417756235a 1056
jamesheavey 29:ecf497c3fdc0 1057 void looped_goal()
jamesheavey 29:ecf497c3fdc0 1058 {
jamesheavey 48:76cf4521d342 1059 // return to start (adding nodes to path)
jamesheavey 48:76cf4521d342 1060 // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
jamesheavey 48:76cf4521d342 1061 // loop between start and goal
jamesheavey 34:63f7c61ee4da 1062 robot.stop();
jamesheavey 55:18ea9529afb2 1063 bool na = dead_end_removal( point[0], point[total_points] );
jamesheavey 50:fa124ea1df9f 1064 leds = 0b1001;
jamesheavey 50:fa124ea1df9f 1065 wait(0.2);
jamesheavey 50:fa124ea1df9f 1066 leds = 0b0110;
jamesheavey 50:fa124ea1df9f 1067 wait(0.2);
jamesheavey 52:79f21d15ede1 1068 na = dead_end_removal( point[2], point[1] );
jamesheavey 50:fa124ea1df9f 1069 leds = 0b1001;
jamesheavey 50:fa124ea1df9f 1070 wait(0.2);
jamesheavey 50:fa124ea1df9f 1071 leds = 0b0110;
jamesheavey 50:fa124ea1df9f 1072 wait(0.2);
jamesheavey 52:79f21d15ede1 1073 na = dead_end_removal( point[4], 100 );
jamesheavey 48:76cf4521d342 1074 while( button_enter.read() == 1 ) {
jamesheavey 29:ecf497c3fdc0 1075 leds = 0b1001;
jamesheavey 29:ecf497c3fdc0 1076 wait(0.2);
jamesheavey 29:ecf497c3fdc0 1077 leds = 0b0110;
jamesheavey 29:ecf497c3fdc0 1078 wait(0.2);
jamesheavey 29:ecf497c3fdc0 1079 }
jamesheavey 29:ecf497c3fdc0 1080 }
jamesheavey 29:ecf497c3fdc0 1081
jamesheavey 10:691c02b352cb 1082 void invert_path()
jamesheavey 5:ae417756235a 1083 {
jamesheavey 10:691c02b352cb 1084 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 1085 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 1086 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 1087 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 1088 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 1089 }
jamesheavey 28:63ff8290964a 1090 }
jamesheavey 37:2967f3f9c936 1091
jamesheavey 37:2967f3f9c936 1092 void print_data(char *word)
jamesheavey 37:2967f3f9c936 1093 {
jamesheavey 37:2967f3f9c936 1094 robot.lcd_clear();
jamesheavey 37:2967f3f9c936 1095 robot.lcd_print(word,10);
jamesheavey 37:2967f3f9c936 1096 robot.stop();
jamesheavey 37:2967f3f9c936 1097 wait(2);
jamesheavey 37:2967f3f9c936 1098
jamesheavey 37:2967f3f9c936 1099 char buffer1[2];
jamesheavey 37:2967f3f9c936 1100 char buffer2[2];
jamesheavey 37:2967f3f9c936 1101 char buffer3[2];
jamesheavey 37:2967f3f9c936 1102 robot.lcd_clear();
jamesheavey 54:5ff74beb043e 1103 sprintf(buffer1,"%x",type[curr_index]);
jamesheavey 54:5ff74beb043e 1104 sprintf(buffer2,"%x",explored[curr_index]);
jamesheavey 37:2967f3f9c936 1105
jamesheavey 54:5ff74beb043e 1106 // sprintf(buffer1,"%d",curr_coords[0]);
jamesheavey 54:5ff74beb043e 1107 // sprintf(buffer2,"%d",curr_coords[1]);
jamesheavey 37:2967f3f9c936 1108 sprintf(buffer3,"%d",curr_index);
jamesheavey 37:2967f3f9c936 1109 robot.lcd_print(buffer1,2);
jamesheavey 37:2967f3f9c936 1110 robot.lcd_goto_xy(0,1);
jamesheavey 37:2967f3f9c936 1111 robot.lcd_print(buffer2,2);
jamesheavey 37:2967f3f9c936 1112 robot.lcd_goto_xy(5,0);
jamesheavey 37:2967f3f9c936 1113 char *b = &dir;
jamesheavey 37:2967f3f9c936 1114 robot.lcd_print(b,2);
jamesheavey 37:2967f3f9c936 1115 robot.lcd_goto_xy(5,1);
jamesheavey 37:2967f3f9c936 1116 robot.lcd_print(buffer3,2);
jamesheavey 37:2967f3f9c936 1117
jamesheavey 37:2967f3f9c936 1118 robot.stop();
jamesheavey 37:2967f3f9c936 1119 wait(2);
jamesheavey 37:2967f3f9c936 1120 }