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@42:69bffd3679bf, 2020-04-15 (annotated)

Committer:
jamesheavey
Date:
Wed Apr 15 15:56:25 2020 +0000
Revision:
42:69bffd3679bf
Parent:
41:2b6b73dd897c
Child:
43:ec047ba15db1
minor nug fix, add goal node and return to start, simplify withing looped_goal()

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 12:d80399686f32 54 speed = (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 30:d62f122e8d60 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 30:d62f122e8d60 205 // else { point[total_points] = total_points; } // numbered 0 -> total_points
jamesheavey 30:d62f122e8d60 206 point[total_points] = total_points;
jamesheavey 30:d62f122e8d60 207
jamesheavey 22:02dda79d50b4 208 coords_x[total_points] = curr_coords[0];
jamesheavey 22:02dda79d50b4 209 coords_y[total_points] = curr_coords[1];
jamesheavey 22:02dda79d50b4 210 }
jamesheavey 21:54ea75f7984f 211
jamesheavey 32:598bedb22c7c 212 update_index();
jamesheavey 32:598bedb22c7c 213
jamesheavey 22:02dda79d50b4 214 // use current coords to find which point to place in path
jamesheavey 31:1e6d0ef05996 215
jamesheavey 25:7523239a2fc1 216 looped_path[path_length] = point[curr_index]; //returns an int of which point we are at what its called
jamesheavey 23:71e84953b3f3 217 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 218
jamesheavey 24:adb946be4ce5 219 // check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
jamesheavey 23:71e84953b3f3 220 // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
jamesheavey 25:7523239a2fc1 221 // 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 222
jamesheavey 23:71e84953b3f3 223
jamesheavey 26:582560881379 224
jamesheavey 26:582560881379 225 // 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 226
jamesheavey 21:54ea75f7984f 227 }
jamesheavey 32:598bedb22c7c 228
jamesheavey 31:1e6d0ef05996 229
jamesheavey 31:1e6d0ef05996 230 // robot.lcd_clear();
jamesheavey 31:1e6d0ef05996 231 // char *b = &dir;
jamesheavey 31:1e6d0ef05996 232 // robot.lcd_print(b,1);
jamesheavey 31:1e6d0ef05996 233
jamesheavey 31:1e6d0ef05996 234
jamesheavey 20:5cf6a378801d 235 //robot.display_data();
jamesheavey 20:5cf6a378801d 236 }
jamesheavey 21:54ea75f7984f 237
jamesheavey 23:71e84953b3f3 238 bool coord_check()
jamesheavey 23:71e84953b3f3 239 {
jamesheavey 23:71e84953b3f3 240 bool result = true;
jamesheavey 23:71e84953b3f3 241 //returns true if the current coords dont match a previous point
jamesheavey 23:71e84953b3f3 242 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 243 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 23:71e84953b3f3 244 result = false;
jamesheavey 23:71e84953b3f3 245 }
jamesheavey 23:71e84953b3f3 246 }
jamesheavey 23:71e84953b3f3 247
jamesheavey 23:71e84953b3f3 248 return result;
jamesheavey 23:71e84953b3f3 249 }
jamesheavey 23:71e84953b3f3 250
jamesheavey 25:7523239a2fc1 251 void update_index() // update index (pointer to current point/type/coords/explored)
jamesheavey 23:71e84953b3f3 252 {
jamesheavey 23:71e84953b3f3 253 // checks the curr_coords againts the coords array, returns the index to relate to the point array
jamesheavey 23:71e84953b3f3 254 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 255 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 25:7523239a2fc1 256 curr_index = i;
jamesheavey 23:71e84953b3f3 257 }
jamesheavey 23:71e84953b3f3 258 }
jamesheavey 23:71e84953b3f3 259 }
jamesheavey 23:71e84953b3f3 260
jamesheavey 29:ecf497c3fdc0 261 int path_to_point_index( int path_point )
jamesheavey 29:ecf497c3fdc0 262 {
jamesheavey 29:ecf497c3fdc0 263 for(int i = 0; i <= total_points; i++) {
jamesheavey 29:ecf497c3fdc0 264 if(path_point == point[i]) {
jamesheavey 29:ecf497c3fdc0 265 return i;
jamesheavey 29:ecf497c3fdc0 266 }
jamesheavey 29:ecf497c3fdc0 267 }
jamesheavey 29:ecf497c3fdc0 268
jamesheavey 29:ecf497c3fdc0 269 return curr_index; // default
jamesheavey 29:ecf497c3fdc0 270 }
jamesheavey 29:ecf497c3fdc0 271
jamesheavey 21:54ea75f7984f 272 void node_logic()
jamesheavey 21:54ea75f7984f 273 {
jamesheavey 24:adb946be4ce5 274 // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
jamesheavey 25:7523239a2fc1 275
jamesheavey 25:7523239a2fc1 276 // first determine what turns are available relative to the robots current direction (left, straight etc.)
jamesheavey 25:7523239a2fc1 277 // convert these relative available turns into available absolute diections (N,E etc.)
jamesheavey 25:7523239a2fc1 278 // set _type to the appropriate value based on available directions (including entry direction = opposite of current)
jamesheavey 25:7523239a2fc1 279 // set _explored entry path as 1
jamesheavey 25:7523239a2fc1 280 // set type[total_points] = _type; & explored[total_points] = _explored;
jamesheavey 25:7523239a2fc1 281
jamesheavey 21:54ea75f7984f 282 bool north = false;
jamesheavey 21:54ea75f7984f 283 bool south = false;
jamesheavey 21:54ea75f7984f 284 bool east = false;
jamesheavey 21:54ea75f7984f 285 bool west = false;
jamesheavey 21:54ea75f7984f 286
jamesheavey 21:54ea75f7984f 287 bool left = false;
jamesheavey 21:54ea75f7984f 288 bool straight = false;
jamesheavey 21:54ea75f7984f 289 bool right = false;
jamesheavey 30:d62f122e8d60 290 //bool goal = false;
jamesheavey 21:54ea75f7984f 291
jamesheavey 25:7523239a2fc1 292 int _type = 0b0000;
jamesheavey 25:7523239a2fc1 293 int _explored = 0b0000;
jamesheavey 21:54ea75f7984f 294
jamesheavey 21:54ea75f7984f 295 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 21:54ea75f7984f 296 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 21:54ea75f7984f 297 robot.forward(speed);
jamesheavey 21:54ea75f7984f 298 robot.scan();
jamesheavey 21:54ea75f7984f 299 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 21:54ea75f7984f 300 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 21:54ea75f7984f 301 }
jamesheavey 21:54ea75f7984f 302
jamesheavey 21:54ea75f7984f 303 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 304 wait(0.05); // maybe change or replace w something better
jamesheavey 21:54ea75f7984f 305 robot.scan();
jamesheavey 27:0a3f028f9365 306 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 30:d62f122e8d60 307 //goal = true;
jamesheavey 27:0a3f028f9365 308 }
jamesheavey 21:54ea75f7984f 309 }
jamesheavey 21:54ea75f7984f 310 robot.scan();
jamesheavey 21:54ea75f7984f 311
jamesheavey 21:54ea75f7984f 312 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 313 straight = true;
jamesheavey 21:54ea75f7984f 314 }
jamesheavey 21:54ea75f7984f 315 }
jamesheavey 21:54ea75f7984f 316
jamesheavey 30:d62f122e8d60 317 // if(goal) {
jamesheavey 30:d62f122e8d60 318 // if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 30:d62f122e8d60 319 // else if ( dir == 'E' ) { west = true; _explored |= 0b1000; }
jamesheavey 30:d62f122e8d60 320 // else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 30:d62f122e8d60 321 // else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 30:d62f122e8d60 322 //
jamesheavey 30:d62f122e8d60 323 // if ( west ) { _type |= 0b1000; }
jamesheavey 30:d62f122e8d60 324 // if ( north ) { _type |= 0b0100; }
jamesheavey 30:d62f122e8d60 325 // if ( east ) { _type |= 0b0010; }
jamesheavey 30:d62f122e8d60 326 // if ( south ) { _type |= 0b0001; }
jamesheavey 30:d62f122e8d60 327 //
jamesheavey 30:d62f122e8d60 328 // goal_node = true;
jamesheavey 30:d62f122e8d60 329 //
jamesheavey 30:d62f122e8d60 330 // robot.reverse(speed);
jamesheavey 30:d62f122e8d60 331 // wait(0.1);
jamesheavey 30:d62f122e8d60 332 //
jamesheavey 30:d62f122e8d60 333 // // set a variable so that a different value is set in the point array
jamesheavey 30:d62f122e8d60 334 // }
jamesheavey 21:54ea75f7984f 335
jamesheavey 30:d62f122e8d60 336 // else {
jamesheavey 27:0a3f028f9365 337 int angle = 0;
jamesheavey 28:63ff8290964a 338 int reset_ang = 0;
jamesheavey 27:0a3f028f9365 339
jamesheavey 34:63f7c61ee4da 340 if (dir == 'E') { angle = 90; }
jamesheavey 34:63f7c61ee4da 341 else if (dir == 'S') { angle = 180; }
jamesheavey 34:63f7c61ee4da 342 else if (dir == 'W') { angle = 270; }
jamesheavey 34:63f7c61ee4da 343
jamesheavey 34:63f7c61ee4da 344 reset_ang = angle;
jamesheavey 27:0a3f028f9365 345
jamesheavey 27:0a3f028f9365 346 if (left) {
jamesheavey 27:0a3f028f9365 347 angle += 270;
jamesheavey 27:0a3f028f9365 348 angle = angle % 360;
jamesheavey 27:0a3f028f9365 349 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 350 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 351 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 352 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 353 angle = reset_ang;
jamesheavey 27:0a3f028f9365 354 }
jamesheavey 27:0a3f028f9365 355
jamesheavey 27:0a3f028f9365 356 if (right) {
jamesheavey 27:0a3f028f9365 357 angle += 90;
jamesheavey 27:0a3f028f9365 358 angle = angle % 360;
jamesheavey 27:0a3f028f9365 359 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 360 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 361 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 362 if (angle == 270) { west = true; }
jamesheavey 28:63ff8290964a 363 angle = reset_ang;
jamesheavey 27:0a3f028f9365 364 }
jamesheavey 27:0a3f028f9365 365
jamesheavey 27:0a3f028f9365 366 if (straight) {
jamesheavey 27:0a3f028f9365 367 if (angle == 0) { north = true; }
jamesheavey 27:0a3f028f9365 368 if (angle == 180) { south = true; }
jamesheavey 27:0a3f028f9365 369 if (angle == 90) { east = true; }
jamesheavey 27:0a3f028f9365 370 if (angle == 270) { west = true; }
jamesheavey 27:0a3f028f9365 371 }
jamesheavey 27:0a3f028f9365 372
jamesheavey 27:0a3f028f9365 373 if( dir == 'N' ) { south = true; _explored |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 374 else if ( dir == 'E' ) { west = true; _explored |= 0b1000; } // this is acc done in choose turn so might not be needed here
jamesheavey 27:0a3f028f9365 375 else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
jamesheavey 27:0a3f028f9365 376 else if ( dir == 'W' ) { east = true; _explored |= 0b0010; }
jamesheavey 27:0a3f028f9365 377
jamesheavey 27:0a3f028f9365 378 if ( west ) { _type |= 0b1000; }
jamesheavey 27:0a3f028f9365 379 if ( north ) { _type |= 0b0100; }
jamesheavey 27:0a3f028f9365 380 if ( east ) { _type |= 0b0010; }
jamesheavey 27:0a3f028f9365 381 if ( south ) { _type |= 0b0001; }
jamesheavey 30:d62f122e8d60 382 // }
jamesheavey 27:0a3f028f9365 383
jamesheavey 26:582560881379 384 type[total_points] = _type; // maybe update_index and use curr_index instead of total_points
jamesheavey 25:7523239a2fc1 385 explored[total_points] = _explored;
jamesheavey 21:54ea75f7984f 386
jamesheavey 21:54ea75f7984f 387 }
jamesheavey 21:54ea75f7984f 388
jamesheavey 21:54ea75f7984f 389 void choose_turn()
jamesheavey 21:54ea75f7984f 390 {
jamesheavey 25:7523239a2fc1 391 // look at cuurent coords, find what node we are at
jamesheavey 24:adb946be4ce5 392 // 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 393 // sets the explored of the current node to 1 in whatever path is chosen
jamesheavey 25:7523239a2fc1 394 // also update dir
jamesheavey 37:2967f3f9c936 395
jamesheavey 34:63f7c61ee4da 396 if( dir == 'N' ) { explored[curr_index] |= 0b0001; } // sets the direction opposite to entry direction as an explored path
jamesheavey 34:63f7c61ee4da 397 else if ( dir == 'E' ) { explored[curr_index] |= 0b1000; }
jamesheavey 34:63f7c61ee4da 398 else if ( dir == 'S' ) { explored[curr_index] |= 0b0100; }
jamesheavey 34:63f7c61ee4da 399 else if ( dir == 'W' ) { explored[curr_index] |= 0b0010; }
jamesheavey 39:005ad4610152 400 // print_data("enter junc");
jamesheavey 25:7523239a2fc1 401 int unexp_paths = type[curr_index] & ~( explored[curr_index] ); // produces a binary of 1's in the available unexplored paths
jamesheavey 25:7523239a2fc1 402
jamesheavey 32:598bedb22c7c 403 if (unexp_paths == 0b0000) {
jamesheavey 34:63f7c61ee4da 404 back_track();
jamesheavey 36:1af069be0fdb 405 unexp_paths = type[curr_index] & ~( explored[curr_index] );
jamesheavey 30:d62f122e8d60 406 }
jamesheavey 32:598bedb22c7c 407
jamesheavey 32:598bedb22c7c 408 int curr_angle = 0;
jamesheavey 32:598bedb22c7c 409 if ( dir == 'E' ) { curr_angle = 90;}
jamesheavey 32:598bedb22c7c 410 else if ( dir == 'S' ) { curr_angle = 180;}
jamesheavey 32:598bedb22c7c 411 else if ( dir == 'W' ) { curr_angle = 270;}
jamesheavey 25:7523239a2fc1 412
jamesheavey 32:598bedb22c7c 413 int desired_angle = 0;
jamesheavey 32:598bedb22c7c 414 if ( (unexp_paths & 0b1000) == 0b1000) {
jamesheavey 32:598bedb22c7c 415 desired_angle = 270;
jamesheavey 32:598bedb22c7c 416 dir = 'W';
jamesheavey 32:598bedb22c7c 417 explored[curr_index] |= 0b1000;
jamesheavey 32:598bedb22c7c 418 }
jamesheavey 32:598bedb22c7c 419 else if ( (unexp_paths & 0b0100) == 0b0100) {
jamesheavey 32:598bedb22c7c 420 desired_angle = 0;
jamesheavey 32:598bedb22c7c 421 dir = 'N';
jamesheavey 32:598bedb22c7c 422 explored[curr_index] |= 0b0100;
jamesheavey 25:7523239a2fc1 423 }
jamesheavey 32:598bedb22c7c 424 else if ( (unexp_paths & 0b0010) == 0b0010) {
jamesheavey 32:598bedb22c7c 425 desired_angle = 90;
jamesheavey 32:598bedb22c7c 426 dir = 'E';
jamesheavey 32:598bedb22c7c 427 explored[curr_index] |= 0b0010;
jamesheavey 32:598bedb22c7c 428 }
jamesheavey 32:598bedb22c7c 429 else if ( (unexp_paths & 0b0001) == 0b0001) {
jamesheavey 32:598bedb22c7c 430 desired_angle = 180;
jamesheavey 32:598bedb22c7c 431 dir = 'S';
jamesheavey 32:598bedb22c7c 432 explored[curr_index] |= 0b0001;
jamesheavey 32:598bedb22c7c 433 }
jamesheavey 32:598bedb22c7c 434
jamesheavey 32:598bedb22c7c 435 int turn_angle = (desired_angle - curr_angle + 360) % 360;
jamesheavey 32:598bedb22c7c 436
jamesheavey 39:005ad4610152 437 // robot.lcd_clear();
jamesheavey 39:005ad4610152 438 // robot.lcd_print("turn" , 4);
jamesheavey 39:005ad4610152 439 // wait(2);
jamesheavey 37:2967f3f9c936 440
jamesheavey 39:005ad4610152 441 // robot.lcd_clear();
jamesheavey 39:005ad4610152 442 // if( turn_angle == 0) { robot.lcd_print("S",1); }
jamesheavey 39:005ad4610152 443 // else if( turn_angle == 90) { robot.lcd_print("R",1); }
jamesheavey 39:005ad4610152 444 // else if( turn_angle == 180) { robot.lcd_print("B",1); }
jamesheavey 39:005ad4610152 445 // else if( turn_angle == 270) { robot.lcd_print("L",1); }
jamesheavey 39:005ad4610152 446 // wait(2);
jamesheavey 39:005ad4610152 447 //
jamesheavey 39:005ad4610152 448 // print_data("After Turn");
jamesheavey 37:2967f3f9c936 449
jamesheavey 37:2967f3f9c936 450 if( turn_angle == 0) {
jamesheavey 37:2967f3f9c936 451 robot.forward(speed);
jamesheavey 37:2967f3f9c936 452 wait(0.1);
jamesheavey 37:2967f3f9c936 453 turn_select('S');
jamesheavey 37:2967f3f9c936 454 }
jamesheavey 32:598bedb22c7c 455 else if( turn_angle == 90) { turn_select('R'); }
jamesheavey 39:005ad4610152 456 else if( turn_angle == 180) {
jamesheavey 42:69bffd3679bf 457 // robot.reverse(speed);
jamesheavey 42:69bffd3679bf 458 // wait(0.1);
jamesheavey 39:005ad4610152 459 turn_select('B');
jamesheavey 39:005ad4610152 460 }
jamesheavey 32:598bedb22c7c 461 else if( turn_angle == 270) { turn_select('L'); }
jamesheavey 21:54ea75f7984f 462 }
jamesheavey 21:54ea75f7984f 463
jamesheavey 28:63ff8290964a 464 void back_track()
jamesheavey 28:63ff8290964a 465 {
jamesheavey 33:9fa9e09f2e8f 466 // 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 467 // also if no nodes have unexplored paths set complete to true
jamesheavey 29:ecf497c3fdc0 468
jamesheavey 35:2fd4ee9ac889 469 bool complete = false;
jamesheavey 34:63f7c61ee4da 470 int pointer; // an index to the most recent node with unexplored paths
jamesheavey 29:ecf497c3fdc0 471
jamesheavey 34:63f7c61ee4da 472 for(int i = total_points; i >= 0; i--) { // start from the most recently discovered node
jamesheavey 34:63f7c61ee4da 473 if( explored[i] != type[i] ) {
jamesheavey 35:2fd4ee9ac889 474 complete = true;
jamesheavey 34:63f7c61ee4da 475 pointer = i;
jamesheavey 34:63f7c61ee4da 476 break;
jamesheavey 34:63f7c61ee4da 477 }
jamesheavey 29:ecf497c3fdc0 478 }
jamesheavey 29:ecf497c3fdc0 479
jamesheavey 35:2fd4ee9ac889 480 if( complete == false ) { looped_goal(); }
jamesheavey 34:63f7c61ee4da 481
jamesheavey 34:63f7c61ee4da 482 else {
jamesheavey 34:63f7c61ee4da 483 // compare node coordinates to previous node coordinates
jamesheavey 34:63f7c61ee4da 484 // determine which direction the previous node is compared to current
jamesheavey 34:63f7c61ee4da 485 // set the current nodes direction path to 0 and the opposite direction path to 0
jamesheavey 34:63f7c61ee4da 486 // decrement the count (setting the previous as current node and the next previous as previous)
jamesheavey 34:63f7c61ee4da 487 // when previous node == point[pointer1] break
jamesheavey 34:63f7c61ee4da 488 // choose turn should then do all those turns and arrive at correct node
jamesheavey 34:63f7c61ee4da 489 char dir_diff;
jamesheavey 34:63f7c61ee4da 490
jamesheavey 34:63f7c61ee4da 491 for(int j = path_length; j >= 0; j--) {
jamesheavey 37:2967f3f9c936 492 int curr_node = path_to_point_index(looped_path[j]);
jamesheavey 39:005ad4610152 493 // check if curr node is repeated, if it is skip to the earliest one thats infront of the unxplored node
jamesheavey 39:005ad4610152 494 int correct_ind = node_repeat_check(curr_node, point[pointer]);
jamesheavey 39:005ad4610152 495 while(j != correct_ind) { j--; }
jamesheavey 39:005ad4610152 496
jamesheavey 37:2967f3f9c936 497 int prev_node = path_to_point_index(looped_path[j-1]);
jamesheavey 34:63f7c61ee4da 498 if(coords_x[prev_node] != coords_x[curr_node]) {
jamesheavey 34:63f7c61ee4da 499 if(coords_x[prev_node] - coords_x[curr_node] > 0){
jamesheavey 34:63f7c61ee4da 500 dir_diff = 'E';
jamesheavey 34:63f7c61ee4da 501 } else {
jamesheavey 34:63f7c61ee4da 502 dir_diff = 'W';
jamesheavey 34:63f7c61ee4da 503 }
jamesheavey 34:63f7c61ee4da 504 } else if( coords_y[prev_node] != coords_y[curr_node] ) {
jamesheavey 34:63f7c61ee4da 505 if(coords_y[prev_node] - coords_y[curr_node] > 0){
jamesheavey 34:63f7c61ee4da 506 dir_diff = 'N';
jamesheavey 34:63f7c61ee4da 507 } else {
jamesheavey 34:63f7c61ee4da 508 dir_diff = 'S';
jamesheavey 34:63f7c61ee4da 509 }
jamesheavey 34:63f7c61ee4da 510 }
jamesheavey 34:63f7c61ee4da 511
jamesheavey 34:63f7c61ee4da 512 if( dir_diff == 'N' ) {
jamesheavey 34:63f7c61ee4da 513 explored[curr_node] &= 0b1011;
jamesheavey 36:1af069be0fdb 514 //explored[prev_node] &= 0b1110;
jamesheavey 34:63f7c61ee4da 515 }
jamesheavey 34:63f7c61ee4da 516 else if( dir_diff == 'E' ) {
jamesheavey 34:63f7c61ee4da 517 explored[curr_node] &= 0b1101;
jamesheavey 36:1af069be0fdb 518 //explored[prev_node] &= 0b0111;
jamesheavey 34:63f7c61ee4da 519 }
jamesheavey 34:63f7c61ee4da 520 else if( dir_diff == 'S' ) {
jamesheavey 34:63f7c61ee4da 521 explored[curr_node] &= 0b1110;
jamesheavey 36:1af069be0fdb 522 //explored[prev_node] &= 0b1011;
jamesheavey 34:63f7c61ee4da 523 }
jamesheavey 34:63f7c61ee4da 524 else if( dir_diff == 'W' ) {
jamesheavey 34:63f7c61ee4da 525 explored[curr_node] &= 0b0111;
jamesheavey 36:1af069be0fdb 526 //explored[prev_node] &= 0b1101;
jamesheavey 34:63f7c61ee4da 527 }
jamesheavey 39:005ad4610152 528 // print_data("after bt");
jamesheavey 34:63f7c61ee4da 529 if(point[prev_node] == point[pointer]) { break; }
jamesheavey 34:63f7c61ee4da 530 }
jamesheavey 34:63f7c61ee4da 531 }
jamesheavey 28:63ff8290964a 532 }
jamesheavey 28:63ff8290964a 533
jamesheavey 39:005ad4610152 534 int node_repeat_check(int curr_node, int desired_node) // will skip deadends in bt
jamesheavey 39:005ad4610152 535 {
jamesheavey 39:005ad4610152 536 int repeats = 0;
jamesheavey 39:005ad4610152 537 int index[10]; // array of all the indexes that contain the curr_node
jamesheavey 39:005ad4610152 538 int earliest_d; // index of desired node
jamesheavey 39:005ad4610152 539 int result;
jamesheavey 39:005ad4610152 540
jamesheavey 39:005ad4610152 541 for( int i = path_length; i >= 0; i-- ) {
jamesheavey 39:005ad4610152 542 if(looped_path[i] == point[curr_node]) {
jamesheavey 39:005ad4610152 543 index[repeats] = i;
jamesheavey 39:005ad4610152 544 repeats++;
jamesheavey 39:005ad4610152 545 }
jamesheavey 39:005ad4610152 546 if(looped_path[i] == point[desired_node]) {
jamesheavey 39:005ad4610152 547 earliest_d = i;
jamesheavey 39:005ad4610152 548 }
jamesheavey 39:005ad4610152 549 }
jamesheavey 39:005ad4610152 550
jamesheavey 39:005ad4610152 551 // return the index of the earliest 'c' that comes after a 'd'
jamesheavey 39:005ad4610152 552
jamesheavey 39:005ad4610152 553 for( int j = repeats - 1; j >= 0; j-- ) {
jamesheavey 39:005ad4610152 554 if( earliest_d < index[j] ) {
jamesheavey 39:005ad4610152 555 result = index[j];
jamesheavey 39:005ad4610152 556 break;
jamesheavey 39:005ad4610152 557 }
jamesheavey 39:005ad4610152 558 }
jamesheavey 39:005ad4610152 559
jamesheavey 39:005ad4610152 560 return result; // returns the correct path index of the node
jamesheavey 39:005ad4610152 561 }
jamesheavey 39:005ad4610152 562
jamesheavey 10:691c02b352cb 563 void follow_line()
jamesheavey 0:df5216b20861 564 {
jamesheavey 10:691c02b352cb 565 robot.scan();
jamesheavey 10:691c02b352cb 566 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 567
jamesheavey 15:6c461501d12d 568 leds = 0b0110;
jamesheavey 15:6c461501d12d 569
jamesheavey 10:691c02b352cb 570 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 571 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 572 integral += proportional;
jamesheavey 10:691c02b352cb 573 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 574
jamesheavey 10:691c02b352cb 575 // calculate motor correction
jamesheavey 10:691c02b352cb 576 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 577
jamesheavey 10:691c02b352cb 578 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 579 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 580 motor_correction = speed;
jamesheavey 10:691c02b352cb 581 }
jamesheavey 10:691c02b352cb 582 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 583 motor_correction = -speed;
jamesheavey 10:691c02b352cb 584 }
jamesheavey 0:df5216b20861 585
jamesheavey 10:691c02b352cb 586 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 587 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 588 } else {
jamesheavey 10:691c02b352cb 589 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 590 }
jamesheavey 18:991658b628fc 591
jamesheavey 19:4c08275cb3c9 592 // read_encoders();
jamesheavey 19:4c08275cb3c9 593 // 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 594 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 595 }
jamesheavey 0:df5216b20861 596
jamesheavey 14:87052bb35211 597 bool junction_detect()
jamesheavey 6:c10b367747a0 598 {
jamesheavey 20:5cf6a378801d 599 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 600 return true;
jamesheavey 20:5cf6a378801d 601 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 602 return true;
jamesheavey 14:87052bb35211 603 } else {
jamesheavey 14:87052bb35211 604 return false;
jamesheavey 14:87052bb35211 605 }
jamesheavey 14:87052bb35211 606 }
jamesheavey 14:87052bb35211 607
jamesheavey 14:87052bb35211 608 char junction_logic()
jamesheavey 14:87052bb35211 609 {
jamesheavey 14:87052bb35211 610 bool straight = false;
jamesheavey 7:7fefd782532d 611 bool left = false;
jamesheavey 6:c10b367747a0 612 bool right = false;
jamesheavey 9:952586accbf9 613 bool goal = false;
jamesheavey 7:7fefd782532d 614
jamesheavey 20:5cf6a378801d 615 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 616 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 617 robot.forward(speed);
jamesheavey 9:952586accbf9 618 robot.scan();
jamesheavey 20:5cf6a378801d 619 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 620 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 621 }
jamesheavey 7:7fefd782532d 622
jamesheavey 20:5cf6a378801d 623 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 624 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 625 robot.scan();
jamesheavey 20:5cf6a378801d 626 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 627 goal = true;
jamesheavey 12:d80399686f32 628 }
jamesheavey 13:bd271266e161 629 }
jamesheavey 14:87052bb35211 630
jamesheavey 14:87052bb35211 631 robot.scan();
jamesheavey 14:87052bb35211 632
jamesheavey 20:5cf6a378801d 633 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 634 straight = true;
jamesheavey 14:87052bb35211 635 }
jamesheavey 14:87052bb35211 636
jamesheavey 20:5cf6a378801d 637 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 638 return 'B';
jamesheavey 6:c10b367747a0 639 }
jamesheavey 7:7fefd782532d 640
jamesheavey 9:952586accbf9 641 if (goal) {
jamesheavey 9:952586accbf9 642 return 'G';
jamesheavey 9:952586accbf9 643 } else if (left) {
jamesheavey 9:952586accbf9 644 return 'L';
jamesheavey 14:87052bb35211 645 } else if (straight) {
jamesheavey 14:87052bb35211 646 return 'S';
jamesheavey 9:952586accbf9 647 } else if (right) {
jamesheavey 9:952586accbf9 648 return 'R';
jamesheavey 6:c10b367747a0 649 } else {
jamesheavey 9:952586accbf9 650 return 'S';
jamesheavey 9:952586accbf9 651 }
jamesheavey 6:c10b367747a0 652 }
jamesheavey 6:c10b367747a0 653
jamesheavey 0:df5216b20861 654
jamesheavey 14:87052bb35211 655 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 656 {
jamesheavey 1:79219d0a33c8 657 switch(turn) {
jamesheavey 5:ae417756235a 658 case 'G':
jamesheavey 5:ae417756235a 659 goal();
jamesheavey 1:79219d0a33c8 660 case 'L':
jamesheavey 1:79219d0a33c8 661 left();
jamesheavey 5:ae417756235a 662 break;
jamesheavey 1:79219d0a33c8 663 case 'S':
jamesheavey 1:79219d0a33c8 664 break;
jamesheavey 1:79219d0a33c8 665 case 'R':
jamesheavey 1:79219d0a33c8 666 right();
jamesheavey 5:ae417756235a 667 break;
jamesheavey 1:79219d0a33c8 668 case 'B':
jamesheavey 1:79219d0a33c8 669 back();
jamesheavey 5:ae417756235a 670 break;
jamesheavey 1:79219d0a33c8 671 }
jamesheavey 1:79219d0a33c8 672 }
jamesheavey 1:79219d0a33c8 673
jamesheavey 0:df5216b20861 674 void left()
jamesheavey 0:df5216b20861 675 {
jamesheavey 1:79219d0a33c8 676 leds = 0b1100;
jamesheavey 3:a5e06482462e 677
jamesheavey 20:5cf6a378801d 678 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 679
jamesheavey 39:005ad4610152 680 robot.spin_left(TURN_SPEED);
jamesheavey 39:005ad4610152 681 wait(0.2);
jamesheavey 3:a5e06482462e 682
jamesheavey 20:5cf6a378801d 683 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 684
jamesheavey 20:5cf6a378801d 685 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 686 }
jamesheavey 1:79219d0a33c8 687
jamesheavey 1:79219d0a33c8 688 void right()
jamesheavey 1:79219d0a33c8 689 {
jamesheavey 1:79219d0a33c8 690 leds = 0b0011;
jamesheavey 5:ae417756235a 691
jamesheavey 20:5cf6a378801d 692 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 693
jamesheavey 20:5cf6a378801d 694 robot.spin_right(TURN_SPEED);
jamesheavey 39:005ad4610152 695 wait(0.2);
jamesheavey 3:a5e06482462e 696
jamesheavey 20:5cf6a378801d 697 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 698
jamesheavey 20:5cf6a378801d 699 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 700 }
jamesheavey 0:df5216b20861 701
jamesheavey 0:df5216b20861 702 void back()
jamesheavey 0:df5216b20861 703 {
jamesheavey 1:79219d0a33c8 704 leds = 0b1111;
jamesheavey 32:598bedb22c7c 705 // robot.reverse(speed);
jamesheavey 32:598bedb22c7c 706 // wait(0.1);
jamesheavey 20:5cf6a378801d 707 robot.spin_right(TURN_SPEED);
jamesheavey 41:2b6b73dd897c 708 if(loop_check == false) { // works better for looped
jamesheavey 42:69bffd3679bf 709 wait(0.7);
jamesheavey 41:2b6b73dd897c 710 }
jamesheavey 41:2b6b73dd897c 711 else {
jamesheavey 41:2b6b73dd897c 712 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 713
jamesheavey 41:2b6b73dd897c 714 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 41:2b6b73dd897c 715 }
jamesheavey 0:df5216b20861 716 }
jamesheavey 1:79219d0a33c8 717
jamesheavey 2:940e46e21353 718 void simplify()
jamesheavey 1:79219d0a33c8 719 {
jamesheavey 2:940e46e21353 720 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 721 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 722 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 723
jamesheavey 10:691c02b352cb 724 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 725 int angle_change = 0;
jamesheavey 2:940e46e21353 726
jamesheavey 10:691c02b352cb 727 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 728 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 729 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 730 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 731 }
jamesheavey 4:38c29dbc5953 732
jamesheavey 4:38c29dbc5953 733 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 734
jamesheavey 4:38c29dbc5953 735 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 736 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 737 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 738 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 739
jamesheavey 4:38c29dbc5953 740 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 741
jamesheavey 4:38c29dbc5953 742 path_length -= 2;
jamesheavey 2:940e46e21353 743 }
jamesheavey 5:ae417756235a 744 }
jamesheavey 5:ae417756235a 745
jamesheavey 10:691c02b352cb 746 void goal()
jamesheavey 10:691c02b352cb 747 {
jamesheavey 10:691c02b352cb 748 invert_path();
jamesheavey 11:c3299aca7d8f 749
jamesheavey 11:c3299aca7d8f 750 leds = 0b0000;
jamesheavey 10:691c02b352cb 751
jamesheavey 10:691c02b352cb 752 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 753 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 754
jamesheavey 11:c3299aca7d8f 755 while(1) {
jamesheavey 15:6c461501d12d 756 int pointer = 0;
jamesheavey 15:6c461501d12d 757
jamesheavey 15:6c461501d12d 758 robot.stop();
jamesheavey 15:6c461501d12d 759
jamesheavey 15:6c461501d12d 760 leds = 0b1001;
jamesheavey 15:6c461501d12d 761 wait(0.2);
jamesheavey 15:6c461501d12d 762 leds = 0b0110;
jamesheavey 15:6c461501d12d 763 wait(0.2);
jamesheavey 15:6c461501d12d 764
jamesheavey 15:6c461501d12d 765 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 766 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 767 wait(0.05);
jamesheavey 15:6c461501d12d 768
jamesheavey 20:5cf6a378801d 769 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 770 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 771 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 772 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 773
jamesheavey 15:6c461501d12d 774 robot.stop();
jamesheavey 15:6c461501d12d 775
jamesheavey 15:6c461501d12d 776 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 777 follow_line();
jamesheavey 15:6c461501d12d 778
jamesheavey 15:6c461501d12d 779 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 780 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 781 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 782 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 783 robot.forward(speed);
jamesheavey 15:6c461501d12d 784 leds = 0b1010;
jamesheavey 20:5cf6a378801d 785 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 786 }
jamesheavey 15:6c461501d12d 787 pointer++;
jamesheavey 15:6c461501d12d 788 }
jamesheavey 15:6c461501d12d 789 }
jamesheavey 15:6c461501d12d 790
jamesheavey 15:6c461501d12d 791 back();
jamesheavey 12:d80399686f32 792
jamesheavey 11:c3299aca7d8f 793 robot.stop();
jamesheavey 20:5cf6a378801d 794 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 795 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 796 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 797 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 798
jamesheavey 14:87052bb35211 799 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 800
jamesheavey 20:5cf6a378801d 801 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 802 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 803
jamesheavey 15:6c461501d12d 804 pointer = 0;
jamesheavey 15:6c461501d12d 805
jamesheavey 11:c3299aca7d8f 806 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 807 wait(0.2);
jamesheavey 11:c3299aca7d8f 808 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 809 wait(0.2);
jamesheavey 11:c3299aca7d8f 810 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 811 wait(0.2);
jamesheavey 11:c3299aca7d8f 812 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 813 wait(0.2);
jamesheavey 11:c3299aca7d8f 814
jamesheavey 11:c3299aca7d8f 815 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 816 follow_line();
jamesheavey 11:c3299aca7d8f 817
jamesheavey 14:87052bb35211 818 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 819 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 820 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 821 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 822 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 823 leds = 0b1010;
jamesheavey 20:5cf6a378801d 824 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 825 }
jamesheavey 11:c3299aca7d8f 826 pointer++;
jamesheavey 10:691c02b352cb 827 }
jamesheavey 10:691c02b352cb 828 }
jamesheavey 5:ae417756235a 829 }
jamesheavey 5:ae417756235a 830 }
jamesheavey 5:ae417756235a 831
jamesheavey 29:ecf497c3fdc0 832 void looped_goal()
jamesheavey 29:ecf497c3fdc0 833 {
jamesheavey 34:63f7c61ee4da 834 robot.stop();
jamesheavey 34:63f7c61ee4da 835
jamesheavey 29:ecf497c3fdc0 836 while(1) {
jamesheavey 29:ecf497c3fdc0 837 leds = 0b1001;
jamesheavey 29:ecf497c3fdc0 838 wait(0.2);
jamesheavey 29:ecf497c3fdc0 839 leds = 0b0110;
jamesheavey 29:ecf497c3fdc0 840 wait(0.2);
jamesheavey 29:ecf497c3fdc0 841 }
jamesheavey 29:ecf497c3fdc0 842 }
jamesheavey 29:ecf497c3fdc0 843
jamesheavey 10:691c02b352cb 844 void invert_path()
jamesheavey 5:ae417756235a 845 {
jamesheavey 10:691c02b352cb 846 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 847 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 848 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 849 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 850 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 851 }
jamesheavey 28:63ff8290964a 852 }
jamesheavey 37:2967f3f9c936 853
jamesheavey 37:2967f3f9c936 854 void print_data(char *word)
jamesheavey 37:2967f3f9c936 855 {
jamesheavey 37:2967f3f9c936 856 robot.lcd_clear();
jamesheavey 37:2967f3f9c936 857 robot.lcd_print(word,10);
jamesheavey 37:2967f3f9c936 858 robot.stop();
jamesheavey 37:2967f3f9c936 859 wait(2);
jamesheavey 37:2967f3f9c936 860
jamesheavey 37:2967f3f9c936 861 char buffer1[2];
jamesheavey 37:2967f3f9c936 862 char buffer2[2];
jamesheavey 37:2967f3f9c936 863 char buffer3[2];
jamesheavey 37:2967f3f9c936 864 robot.lcd_clear();
jamesheavey 38:b5b06625d06e 865 // sprintf(buffer1,"%x",type[curr_index]);
jamesheavey 38:b5b06625d06e 866 // sprintf(buffer2,"%x",explored[curr_index]);
jamesheavey 37:2967f3f9c936 867
jamesheavey 38:b5b06625d06e 868 sprintf(buffer1,"%d",curr_coords[0]);
jamesheavey 38:b5b06625d06e 869 sprintf(buffer2,"%d",curr_coords[1]);
jamesheavey 37:2967f3f9c936 870 sprintf(buffer3,"%d",curr_index);
jamesheavey 37:2967f3f9c936 871 robot.lcd_print(buffer1,2);
jamesheavey 37:2967f3f9c936 872 robot.lcd_goto_xy(0,1);
jamesheavey 37:2967f3f9c936 873 robot.lcd_print(buffer2,2);
jamesheavey 37:2967f3f9c936 874 robot.lcd_goto_xy(5,0);
jamesheavey 37:2967f3f9c936 875 char *b = &dir;
jamesheavey 37:2967f3f9c936 876 robot.lcd_print(b,2);
jamesheavey 37:2967f3f9c936 877 robot.lcd_goto_xy(5,1);
jamesheavey 37:2967f3f9c936 878 robot.lcd_print(buffer3,2);
jamesheavey 37:2967f3f9c936 879
jamesheavey 37:2967f3f9c936 880 robot.stop();
jamesheavey 37:2967f3f9c936 881 wait(2);
jamesheavey 37:2967f3f9c936 882 }