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@24:adb946be4ce5, 2020-04-09 (annotated)

Committer:
jamesheavey
Date:
Thu Apr 09 00:52:46 2020 +0000
Revision:
24:adb946be4ce5
Parent:
23:71e84953b3f3
Child:
25:7523239a2fc1
save before changing node logic and choose turn

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamesheavey 0:df5216b20861 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 24:adb946be4ce5 172 type[total_points] = 0b1000; // start is always 1 exit type in the north direction
jamesheavey 24:adb946be4ce5 173 explored[total_points] = 0b0000; // not sure if i set this as explored already or whether i do that at the next junction
jamesheavey 23:71e84953b3f3 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 24:adb946be4ce5 178 if ( t_restart ){ // only start the timer if it isnt already started
jamesheavey 21:54ea75f7984f 179 t_coord.start();
jamesheavey 21:54ea75f7984f 180 t_restart = false;
jamesheavey 21:54ea75f7984f 181 }
jamesheavey 21:54ea75f7984f 182
jamesheavey 21:54ea75f7984f 183 follow_line();
jamesheavey 21:54ea75f7984f 184
jamesheavey 21:54ea75f7984f 185 if ( junction_detect() ) {
jamesheavey 24:adb946be4ce5 186 path_length++; // increment the path position index
jamesheavey 22:02dda79d50b4 187
jamesheavey 23:71e84953b3f3 188 float time = t_coord.read();
jamesheavey 24:adb946be4ce5 189 int dist_est = floor(time);
jamesheavey 21:54ea75f7984f 190 t_coord.stop();
jamesheavey 21:54ea75f7984f 191 t_coord.reset();
jamesheavey 24:adb946be4ce5 192 t_restart = true; //restart the timer next loop
jamesheavey 21:54ea75f7984f 193
jamesheavey 21:54ea75f7984f 194 if (dir == 'N'){ curr_coords[1] += dist_est; } // y coord
jamesheavey 21:54ea75f7984f 195 if (dir == 'E'){ curr_coords[0] += dist_est; } // x coord
jamesheavey 21:54ea75f7984f 196 if (dir == 'S'){ curr_coords[1] -= dist_est; }
jamesheavey 21:54ea75f7984f 197 if (dir == 'W'){ curr_coords[0] -= dist_est; }
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 22:02dda79d50b4 200 if (coord_check()) { // coord_check returns true if curr_coords coordinates are not present in (should set a variable as true)
jamesheavey 22:02dda79d50b4 201 total_points++;
jamesheavey 24:adb946be4ce5 202 point[total_points] = total_points; // numbered 0 -> total_points
jamesheavey 22:02dda79d50b4 203 coords_x[total_points] = curr_coords[0];
jamesheavey 22:02dda79d50b4 204 coords_y[total_points] = curr_coords[1];
jamesheavey 23:71e84953b3f3 205 node_logic(); // determines what junction type we are at updates the explored (path entered on) and type arrays accordingly
jamesheavey 22:02dda79d50b4 206 }
jamesheavey 21:54ea75f7984f 207
jamesheavey 22:02dda79d50b4 208 // use current coords to find which point to place in path
jamesheavey 23:71e84953b3f3 209 looped_path[path_length] = coord_to_node(); //returns an int of which point we are at what its called
jamesheavey 23:71e84953b3f3 210 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 211
jamesheavey 24:adb946be4ce5 212 // check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
jamesheavey 23:71e84953b3f3 213 // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
jamesheavey 21:54ea75f7984f 214
jamesheavey 21:54ea75f7984f 215 // assign new node with new coordinates
jamesheavey 23:71e84953b3f3 216
jamesheavey 21:54ea75f7984f 217
jamesheavey 21:54ea75f7984f 218 }
jamesheavey 21:54ea75f7984f 219
jamesheavey 22:02dda79d50b4 220 // robot.lcd_clear();
jamesheavey 22:02dda79d50b4 221 // char xcors[100];
jamesheavey 22:02dda79d50b4 222 // char ycors[100];
jamesheavey 22:02dda79d50b4 223 //
jamesheavey 22:02dda79d50b4 224 // for( unsigned int a = 0; a <= total_points; a += 1 )
jamesheavey 22:02dda79d50b4 225 // {
jamesheavey 22:02dda79d50b4 226 // char x[1];
jamesheavey 22:02dda79d50b4 227 // char y[1];
jamesheavey 22:02dda79d50b4 228 // sprintf(x,"%d",coords_x[a]);
jamesheavey 22:02dda79d50b4 229 // sprintf(y,"%d",coords_y[a]);
jamesheavey 22:02dda79d50b4 230 // xcors[a] = x[1];
jamesheavey 22:02dda79d50b4 231 // ycors[a] = y[];
jamesheavey 22:02dda79d50b4 232 // }
jamesheavey 22:02dda79d50b4 233 // robot.lcd_goto_xy(0,0);
jamesheavey 22:02dda79d50b4 234 // robot.lcd_print(xcors,100);
jamesheavey 22:02dda79d50b4 235 // robot.lcd_goto_xy(0,1);
jamesheavey 22:02dda79d50b4 236 // robot.lcd_print(ycors,100);
jamesheavey 21:54ea75f7984f 237
jamesheavey 20:5cf6a378801d 238 //robot.display_data();
jamesheavey 20:5cf6a378801d 239 }
jamesheavey 21:54ea75f7984f 240
jamesheavey 23:71e84953b3f3 241 bool coord_check()
jamesheavey 23:71e84953b3f3 242 {
jamesheavey 23:71e84953b3f3 243 bool result = true;
jamesheavey 23:71e84953b3f3 244 //returns true if the current coords dont match a previous point
jamesheavey 23:71e84953b3f3 245 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 246 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 23:71e84953b3f3 247 result = false;
jamesheavey 23:71e84953b3f3 248 }
jamesheavey 23:71e84953b3f3 249 }
jamesheavey 23:71e84953b3f3 250
jamesheavey 23:71e84953b3f3 251 return result;
jamesheavey 23:71e84953b3f3 252 }
jamesheavey 23:71e84953b3f3 253
jamesheavey 23:71e84953b3f3 254 int coord_to_node()
jamesheavey 23:71e84953b3f3 255 {
jamesheavey 23:71e84953b3f3 256 int result;
jamesheavey 23:71e84953b3f3 257 // checks the curr_coords againts the coords array, returns the index to relate to the point array
jamesheavey 23:71e84953b3f3 258 for(int i = 0; i <= total_points; i++) {
jamesheavey 23:71e84953b3f3 259 if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) {
jamesheavey 23:71e84953b3f3 260 result = i;
jamesheavey 23:71e84953b3f3 261 }
jamesheavey 23:71e84953b3f3 262 }
jamesheavey 23:71e84953b3f3 263
jamesheavey 24:adb946be4ce5 264 return point[result];
jamesheavey 23:71e84953b3f3 265 }
jamesheavey 23:71e84953b3f3 266
jamesheavey 21:54ea75f7984f 267 void node_logic()
jamesheavey 21:54ea75f7984f 268 {
jamesheavey 24:adb946be4ce5 269 // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
jamesheavey 21:54ea75f7984f 270 bool north = false;
jamesheavey 21:54ea75f7984f 271 bool south = false;
jamesheavey 21:54ea75f7984f 272 bool east = false;
jamesheavey 21:54ea75f7984f 273 bool west = false;
jamesheavey 21:54ea75f7984f 274
jamesheavey 21:54ea75f7984f 275 bool left = false;
jamesheavey 21:54ea75f7984f 276 bool straight = false;
jamesheavey 21:54ea75f7984f 277 bool right = false;
jamesheavey 21:54ea75f7984f 278
jamesheavey 21:54ea75f7984f 279 int total = 1; // starts at 1 because path entered on is counted
jamesheavey 21:54ea75f7984f 280
jamesheavey 21:54ea75f7984f 281 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 21:54ea75f7984f 282 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 21:54ea75f7984f 283 robot.forward(speed);
jamesheavey 21:54ea75f7984f 284 robot.scan();
jamesheavey 21:54ea75f7984f 285 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 21:54ea75f7984f 286 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 21:54ea75f7984f 287 }
jamesheavey 21:54ea75f7984f 288
jamesheavey 21:54ea75f7984f 289 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 290 wait(0.05); // maybe change or replace w something better
jamesheavey 21:54ea75f7984f 291 robot.scan();
jamesheavey 21:54ea75f7984f 292 }
jamesheavey 21:54ea75f7984f 293
jamesheavey 21:54ea75f7984f 294 robot.scan();
jamesheavey 21:54ea75f7984f 295
jamesheavey 21:54ea75f7984f 296 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 297 straight = true;
jamesheavey 21:54ea75f7984f 298 }
jamesheavey 21:54ea75f7984f 299 }
jamesheavey 21:54ea75f7984f 300
jamesheavey 21:54ea75f7984f 301 if (left) { total += 1; }
jamesheavey 21:54ea75f7984f 302 if (straight) { total += 1; }
jamesheavey 21:54ea75f7984f 303 if (right) { total += 1; }
jamesheavey 21:54ea75f7984f 304
jamesheavey 21:54ea75f7984f 305 type[total_points] = total;
jamesheavey 21:54ea75f7984f 306
jamesheavey 21:54ea75f7984f 307 int angle = 0;
jamesheavey 21:54ea75f7984f 308
jamesheavey 21:54ea75f7984f 309 if (dir == 'E') { angle = 90;}
jamesheavey 21:54ea75f7984f 310 else if (dir == 'S') { angle = 180;}
jamesheavey 21:54ea75f7984f 311 else if (dir == 'W') { angle = 270;}
jamesheavey 21:54ea75f7984f 312
jamesheavey 21:54ea75f7984f 313 if (left) {
jamesheavey 21:54ea75f7984f 314 angle += 270;
jamesheavey 21:54ea75f7984f 315 angle = angle % 360;
jamesheavey 21:54ea75f7984f 316 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 317 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 318 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 319 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 320 angle -= 270;
jamesheavey 21:54ea75f7984f 321 angle = angle % 360;
jamesheavey 21:54ea75f7984f 322 }
jamesheavey 21:54ea75f7984f 323
jamesheavey 21:54ea75f7984f 324 if (right) {
jamesheavey 21:54ea75f7984f 325 angle += 90;
jamesheavey 21:54ea75f7984f 326 angle = angle % 360;
jamesheavey 21:54ea75f7984f 327 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 328 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 329 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 330 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 331 angle -= 90;
jamesheavey 21:54ea75f7984f 332 angle = angle % 360;
jamesheavey 21:54ea75f7984f 333 }
jamesheavey 0:df5216b20861 334
jamesheavey 21:54ea75f7984f 335 if (straight) {
jamesheavey 21:54ea75f7984f 336 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 337 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 338 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 339 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 340 }
jamesheavey 21:54ea75f7984f 341
jamesheavey 21:54ea75f7984f 342 int turn_count = type[total_points] - explored[total_points];
jamesheavey 21:54ea75f7984f 343
jamesheavey 21:54ea75f7984f 344 if (west == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 345 if (north == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 346 if (east == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 347 if (south == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 348
jamesheavey 21:54ea75f7984f 349 turn_count = turn_count%4;
jamesheavey 21:54ea75f7984f 350
jamesheavey 21:54ea75f7984f 351 char do_turn = turn_priority[turn_count];
jamesheavey 21:54ea75f7984f 352
jamesheavey 21:54ea75f7984f 353 if (dir == 'E') { angle = 90;}
jamesheavey 21:54ea75f7984f 354 else if (dir == 'S') { angle = 180;}
jamesheavey 21:54ea75f7984f 355 else if (dir == 'W') { angle = 270;}
jamesheavey 21:54ea75f7984f 356
jamesheavey 21:54ea75f7984f 357 unsigned int result = 0;
jamesheavey 21:54ea75f7984f 358
jamesheavey 21:54ea75f7984f 359 if (do_turn == 'N') { dir = 'N'; result = 0; }
jamesheavey 21:54ea75f7984f 360 else if (do_turn == 'S') { dir = 'S'; result = 180; }
jamesheavey 21:54ea75f7984f 361 else if (do_turn == 'E') { dir = 'E'; result = 90; }
jamesheavey 21:54ea75f7984f 362 else if (do_turn == 'W') { dir = 'W'; result = 270; }
jamesheavey 21:54ea75f7984f 363
jamesheavey 21:54ea75f7984f 364 result = result - angle;
jamesheavey 21:54ea75f7984f 365
jamesheavey 21:54ea75f7984f 366 if (result == 0) { turn_select('S'); }
jamesheavey 21:54ea75f7984f 367 else if (result == 90) { turn_select('R'); }
jamesheavey 21:54ea75f7984f 368 else if (result == 180) { turn_select('B'); }
jamesheavey 21:54ea75f7984f 369 else if (result == 270) { turn_select('L'); }
jamesheavey 21:54ea75f7984f 370 // figure out a way to check what directions correlate to which turns based on direction currently facing.
jamesheavey 21:54ea75f7984f 371 }
jamesheavey 21:54ea75f7984f 372
jamesheavey 21:54ea75f7984f 373 void choose_turn()
jamesheavey 21:54ea75f7984f 374 {
jamesheavey 24:adb946be4ce5 375 // 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 21:54ea75f7984f 376
jamesheavey 21:54ea75f7984f 377 }
jamesheavey 21:54ea75f7984f 378
jamesheavey 10:691c02b352cb 379 void follow_line()
jamesheavey 0:df5216b20861 380 {
jamesheavey 10:691c02b352cb 381 robot.scan();
jamesheavey 10:691c02b352cb 382 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 383
jamesheavey 15:6c461501d12d 384 leds = 0b0110;
jamesheavey 15:6c461501d12d 385
jamesheavey 10:691c02b352cb 386 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 387 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 388 integral += proportional;
jamesheavey 10:691c02b352cb 389 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 390
jamesheavey 10:691c02b352cb 391 // calculate motor correction
jamesheavey 10:691c02b352cb 392 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 393
jamesheavey 10:691c02b352cb 394 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 395 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 396 motor_correction = speed;
jamesheavey 10:691c02b352cb 397 }
jamesheavey 10:691c02b352cb 398 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 399 motor_correction = -speed;
jamesheavey 10:691c02b352cb 400 }
jamesheavey 0:df5216b20861 401
jamesheavey 10:691c02b352cb 402 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 403 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 404 } else {
jamesheavey 10:691c02b352cb 405 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 406 }
jamesheavey 18:991658b628fc 407
jamesheavey 19:4c08275cb3c9 408 // read_encoders();
jamesheavey 19:4c08275cb3c9 409 // 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 410 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 411 }
jamesheavey 0:df5216b20861 412
jamesheavey 14:87052bb35211 413 bool junction_detect()
jamesheavey 6:c10b367747a0 414 {
jamesheavey 20:5cf6a378801d 415 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 416 return true;
jamesheavey 20:5cf6a378801d 417 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 418 return true;
jamesheavey 14:87052bb35211 419 } else {
jamesheavey 14:87052bb35211 420 return false;
jamesheavey 14:87052bb35211 421 }
jamesheavey 14:87052bb35211 422 }
jamesheavey 14:87052bb35211 423
jamesheavey 14:87052bb35211 424 char junction_logic()
jamesheavey 14:87052bb35211 425 {
jamesheavey 14:87052bb35211 426 bool straight = false;
jamesheavey 7:7fefd782532d 427 bool left = false;
jamesheavey 6:c10b367747a0 428 bool right = false;
jamesheavey 9:952586accbf9 429 bool goal = false;
jamesheavey 7:7fefd782532d 430
jamesheavey 20:5cf6a378801d 431 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 432 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 433 robot.forward(speed);
jamesheavey 9:952586accbf9 434 robot.scan();
jamesheavey 20:5cf6a378801d 435 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 436 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 437 }
jamesheavey 7:7fefd782532d 438
jamesheavey 20:5cf6a378801d 439 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 440 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 441 robot.scan();
jamesheavey 20:5cf6a378801d 442 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 443 goal = true;
jamesheavey 12:d80399686f32 444 }
jamesheavey 13:bd271266e161 445 }
jamesheavey 14:87052bb35211 446
jamesheavey 14:87052bb35211 447 robot.scan();
jamesheavey 14:87052bb35211 448
jamesheavey 20:5cf6a378801d 449 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 450 straight = true;
jamesheavey 14:87052bb35211 451 }
jamesheavey 14:87052bb35211 452
jamesheavey 20:5cf6a378801d 453 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 454 return 'B';
jamesheavey 6:c10b367747a0 455 }
jamesheavey 7:7fefd782532d 456
jamesheavey 9:952586accbf9 457 if (goal) {
jamesheavey 9:952586accbf9 458 return 'G';
jamesheavey 9:952586accbf9 459 } else if (left) {
jamesheavey 9:952586accbf9 460 return 'L';
jamesheavey 14:87052bb35211 461 } else if (straight) {
jamesheavey 14:87052bb35211 462 return 'S';
jamesheavey 9:952586accbf9 463 } else if (right) {
jamesheavey 9:952586accbf9 464 return 'R';
jamesheavey 6:c10b367747a0 465 } else {
jamesheavey 9:952586accbf9 466 return 'S';
jamesheavey 9:952586accbf9 467 }
jamesheavey 6:c10b367747a0 468 }
jamesheavey 6:c10b367747a0 469
jamesheavey 0:df5216b20861 470
jamesheavey 14:87052bb35211 471 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 472 {
jamesheavey 1:79219d0a33c8 473 switch(turn) {
jamesheavey 5:ae417756235a 474 case 'G':
jamesheavey 5:ae417756235a 475 goal();
jamesheavey 1:79219d0a33c8 476 case 'L':
jamesheavey 1:79219d0a33c8 477 left();
jamesheavey 5:ae417756235a 478 break;
jamesheavey 1:79219d0a33c8 479 case 'S':
jamesheavey 1:79219d0a33c8 480 break;
jamesheavey 1:79219d0a33c8 481 case 'R':
jamesheavey 1:79219d0a33c8 482 right();
jamesheavey 5:ae417756235a 483 break;
jamesheavey 1:79219d0a33c8 484 case 'B':
jamesheavey 1:79219d0a33c8 485 back();
jamesheavey 5:ae417756235a 486 break;
jamesheavey 1:79219d0a33c8 487 }
jamesheavey 1:79219d0a33c8 488 }
jamesheavey 1:79219d0a33c8 489
jamesheavey 0:df5216b20861 490 void left()
jamesheavey 0:df5216b20861 491 {
jamesheavey 1:79219d0a33c8 492 leds = 0b1100;
jamesheavey 3:a5e06482462e 493
jamesheavey 20:5cf6a378801d 494 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 495
jamesheavey 20:5cf6a378801d 496 robot.spin_left(0.2);
jamesheavey 3:a5e06482462e 497 wait(0.1);
jamesheavey 3:a5e06482462e 498
jamesheavey 20:5cf6a378801d 499 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 500
jamesheavey 20:5cf6a378801d 501 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 502 }
jamesheavey 1:79219d0a33c8 503
jamesheavey 1:79219d0a33c8 504 void right()
jamesheavey 1:79219d0a33c8 505 {
jamesheavey 1:79219d0a33c8 506 leds = 0b0011;
jamesheavey 5:ae417756235a 507
jamesheavey 20:5cf6a378801d 508 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 509
jamesheavey 20:5cf6a378801d 510 robot.spin_right(TURN_SPEED);
jamesheavey 3:a5e06482462e 511 wait(0.1);
jamesheavey 3:a5e06482462e 512
jamesheavey 20:5cf6a378801d 513 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 514
jamesheavey 20:5cf6a378801d 515 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 516 }
jamesheavey 0:df5216b20861 517
jamesheavey 0:df5216b20861 518 void back()
jamesheavey 0:df5216b20861 519 {
jamesheavey 1:79219d0a33c8 520 leds = 0b1111;
jamesheavey 5:ae417756235a 521 robot.reverse(speed);
jamesheavey 5:ae417756235a 522 wait(0.1);
jamesheavey 20:5cf6a378801d 523 robot.spin_right(TURN_SPEED);
jamesheavey 3:a5e06482462e 524
jamesheavey 20:5cf6a378801d 525 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 526
jamesheavey 20:5cf6a378801d 527 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 528 }
jamesheavey 1:79219d0a33c8 529
jamesheavey 2:940e46e21353 530 void simplify()
jamesheavey 1:79219d0a33c8 531 {
jamesheavey 2:940e46e21353 532 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 533 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 534 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 535
jamesheavey 10:691c02b352cb 536 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 537 int angle_change = 0;
jamesheavey 2:940e46e21353 538
jamesheavey 10:691c02b352cb 539 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 540 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 541 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 542 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 543 }
jamesheavey 4:38c29dbc5953 544
jamesheavey 4:38c29dbc5953 545 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 546
jamesheavey 4:38c29dbc5953 547 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 548 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 549 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 550 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 551
jamesheavey 4:38c29dbc5953 552 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 553
jamesheavey 4:38c29dbc5953 554 path_length -= 2;
jamesheavey 2:940e46e21353 555 }
jamesheavey 5:ae417756235a 556 }
jamesheavey 5:ae417756235a 557
jamesheavey 10:691c02b352cb 558 void goal()
jamesheavey 10:691c02b352cb 559 {
jamesheavey 10:691c02b352cb 560 invert_path();
jamesheavey 11:c3299aca7d8f 561
jamesheavey 11:c3299aca7d8f 562 leds = 0b0000;
jamesheavey 10:691c02b352cb 563
jamesheavey 10:691c02b352cb 564 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 565 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 566
jamesheavey 11:c3299aca7d8f 567 while(1) {
jamesheavey 15:6c461501d12d 568 int pointer = 0;
jamesheavey 15:6c461501d12d 569
jamesheavey 15:6c461501d12d 570 robot.stop();
jamesheavey 15:6c461501d12d 571
jamesheavey 15:6c461501d12d 572 leds = 0b1001;
jamesheavey 15:6c461501d12d 573 wait(0.2);
jamesheavey 15:6c461501d12d 574 leds = 0b0110;
jamesheavey 15:6c461501d12d 575 wait(0.2);
jamesheavey 15:6c461501d12d 576
jamesheavey 15:6c461501d12d 577 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 578 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 579 wait(0.05);
jamesheavey 15:6c461501d12d 580
jamesheavey 20:5cf6a378801d 581 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 582 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 583 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 584 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 585
jamesheavey 15:6c461501d12d 586 robot.stop();
jamesheavey 15:6c461501d12d 587
jamesheavey 15:6c461501d12d 588 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 589 follow_line();
jamesheavey 15:6c461501d12d 590
jamesheavey 15:6c461501d12d 591 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 592 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 593 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 594 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 595 robot.forward(speed);
jamesheavey 15:6c461501d12d 596 leds = 0b1010;
jamesheavey 20:5cf6a378801d 597 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 598 }
jamesheavey 15:6c461501d12d 599 pointer++;
jamesheavey 15:6c461501d12d 600 }
jamesheavey 15:6c461501d12d 601 }
jamesheavey 15:6c461501d12d 602
jamesheavey 15:6c461501d12d 603 back();
jamesheavey 12:d80399686f32 604
jamesheavey 11:c3299aca7d8f 605 robot.stop();
jamesheavey 20:5cf6a378801d 606 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 607 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 608 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 609 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 610
jamesheavey 14:87052bb35211 611 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 612
jamesheavey 20:5cf6a378801d 613 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 614 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 615
jamesheavey 15:6c461501d12d 616 pointer = 0;
jamesheavey 15:6c461501d12d 617
jamesheavey 11:c3299aca7d8f 618 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 619 wait(0.2);
jamesheavey 11:c3299aca7d8f 620 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 621 wait(0.2);
jamesheavey 11:c3299aca7d8f 622 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 623 wait(0.2);
jamesheavey 11:c3299aca7d8f 624 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 625 wait(0.2);
jamesheavey 11:c3299aca7d8f 626
jamesheavey 11:c3299aca7d8f 627 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 628 follow_line();
jamesheavey 11:c3299aca7d8f 629
jamesheavey 14:87052bb35211 630 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 631 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 14:87052bb35211 632 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 633 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 634 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 635 leds = 0b1010;
jamesheavey 20:5cf6a378801d 636 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 637 }
jamesheavey 11:c3299aca7d8f 638 pointer++;
jamesheavey 10:691c02b352cb 639 }
jamesheavey 10:691c02b352cb 640 }
jamesheavey 5:ae417756235a 641 }
jamesheavey 5:ae417756235a 642 }
jamesheavey 5:ae417756235a 643
jamesheavey 10:691c02b352cb 644 void invert_path()
jamesheavey 5:ae417756235a 645 {
jamesheavey 10:691c02b352cb 646 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 647 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 648 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 649 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 650 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 651 }
jamesheavey 5:ae417756235a 652 }