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@40:02f6e268011b, 2020-04-15 (annotated)

Committer:
jamesheavey
Date:
Wed Apr 15 14:55:58 2020 +0000
Revision:
40:02f6e268011b
Parent:
39:005ad4610152
Child:
41:2b6b73dd897c
yessss;

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