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@68:14f2a5315d88, 2020-06-01 (annotated)

Committer:
jamesheavey
Date:
Mon Jun 01 20:34:11 2020 +0000
Revision:
68:14f2a5315d88
Parent:
67:f307ff6ec083
Child:
69:d1e941e61507
music

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