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@33:9fa9e09f2e8f, 2020-04-13 (annotated)

Committer:
jamesheavey
Date:
Mon Apr 13 15:31:30 2020 +0000
Revision:
33:9fa9e09f2e8f
Parent:
32:598bedb22c7c
Child:
34:63f7c61ee4da
try new back track idea and set all explored paths back to a node with unexplore paths to 0;

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