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@22:02dda79d50b4, 2020-04-08 (annotated)

Committer:
jamesheavey
Date:
Wed Apr 08 06:03:58 2020 +0000
Revision:
22:02dda79d50b4
Parent:
21:54ea75f7984f
Child:
23:71e84953b3f3
new mapping plan

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamesheavey 0:df5216b20861 1 #include "main.h"
jamesheavey 0:df5216b20861 2
jamesheavey 0:df5216b20861 3 // API
jamesheavey 0:df5216b20861 4 m3pi robot;
jamesheavey 0:df5216b20861 5
jamesheavey 0:df5216b20861 6 // LEDs
jamesheavey 0:df5216b20861 7 BusOut leds(LED4,LED3,LED2,LED1);
jamesheavey 0:df5216b20861 8
jamesheavey 0:df5216b20861 9 // Buttons
jamesheavey 0:df5216b20861 10 DigitalIn button_A(p18);
jamesheavey 0:df5216b20861 11 DigitalIn button_B(p17);
jamesheavey 0:df5216b20861 12 DigitalIn button_X(p21);
jamesheavey 0:df5216b20861 13 DigitalIn button_Y(p22);
jamesheavey 0:df5216b20861 14 DigitalIn button_enter(p24);
jamesheavey 0:df5216b20861 15 DigitalIn button_back(p23);
jamesheavey 0:df5216b20861 16
jamesheavey 0:df5216b20861 17 // Potentiometers
jamesheavey 0:df5216b20861 18 AnalogIn pot_P(p15);
jamesheavey 0:df5216b20861 19 AnalogIn pot_I(p16);
jamesheavey 0:df5216b20861 20 AnalogIn pot_D(p19);
jamesheavey 0:df5216b20861 21 AnalogIn pot_S(p20);
jamesheavey 0:df5216b20861 22
jamesheavey 17:77b8515a9568 23 // Sensors
jamesheavey 18:991658b628fc 24 DigitalInOut enc_L(p26); //connected to digital P26
jamesheavey 18:991658b628fc 25 DigitalInOut enc_R(p25); //connected to digital P25
jamesheavey 18:991658b628fc 26
jamesheavey 2:940e46e21353 27 // Main
jamesheavey 2:940e46e21353 28
jamesheavey 0:df5216b20861 29 int main()
jamesheavey 0:df5216b20861 30 {
jamesheavey 0:df5216b20861 31 init();
jamesheavey 20:5cf6a378801d 32
jamesheavey 20:5cf6a378801d 33 bool loop_check;
jamesheavey 20:5cf6a378801d 34
jamesheavey 20:5cf6a378801d 35 robot.lcd_goto_xy(0,0);
jamesheavey 22:02dda79d50b4 36 robot.lcd_print("A=SIMPLE", 10);
jamesheavey 20:5cf6a378801d 37 robot.lcd_goto_xy(0,1);
jamesheavey 22:02dda79d50b4 38 robot.lcd_print("B=LOOPED", 10);
jamesheavey 20:5cf6a378801d 39
jamesheavey 20:5cf6a378801d 40 while(button_A.read() == 1 && button_B.read() == 1) {}
jamesheavey 20:5cf6a378801d 41
jamesheavey 20:5cf6a378801d 42 if (button_B.read()) { loop_check = true; } // non-looped
jamesheavey 20:5cf6a378801d 43 if (button_A.read()) { loop_check = false; } // looped
jamesheavey 20:5cf6a378801d 44
jamesheavey 20:5cf6a378801d 45 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 46 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 47 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 48 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 49 robot.lcd_print("= start ", 10);
jamesheavey 20:5cf6a378801d 50
jamesheavey 0:df5216b20861 51 calibrate();
jamesheavey 15:6c461501d12d 52
jamesheavey 20:5cf6a378801d 53 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 54
jamesheavey 12:d80399686f32 55 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 56
jamesheavey 0:df5216b20861 57 float dt = 1/50; // updating 50 times a second
jamesheavey 20:5cf6a378801d 58
jamesheavey 0:df5216b20861 59 while (1) {
jamesheavey 21:54ea75f7984f 60
jamesheavey 20:5cf6a378801d 61 if (loop_check == true) {
jamesheavey 20:5cf6a378801d 62 non_looped();
jamesheavey 20:5cf6a378801d 63 } else {
jamesheavey 20:5cf6a378801d 64 looped();
jamesheavey 18:991658b628fc 65 }
jamesheavey 0:df5216b20861 66
jamesheavey 0:df5216b20861 67 wait(dt);
jamesheavey 0:df5216b20861 68 }
jamesheavey 0:df5216b20861 69 }
jamesheavey 0:df5216b20861 70
jamesheavey 18:991658b628fc 71 void read_encoders()
jamesheavey 18:991658b628fc 72 {
jamesheavey 18:991658b628fc 73 enc_R.output(); // Set the I/O line to an output
jamesheavey 18:991658b628fc 74 enc_L.output();
jamesheavey 18:991658b628fc 75 enc_R.mode(PullUp);
jamesheavey 18:991658b628fc 76 enc_L.mode(PullUp);
jamesheavey 18:991658b628fc 77
jamesheavey 19:4c08275cb3c9 78 wait_us(10); // Must be atleast 10us for the 10 nF capacitor to charge
jamesheavey 18:991658b628fc 79 enc_R.mode(PullNone);
jamesheavey 18:991658b628fc 80 enc_L.mode(PullNone);
jamesheavey 18:991658b628fc 81 enc_R = 1; // Drive the line high
jamesheavey 18:991658b628fc 82 enc_L = 1;
jamesheavey 18:991658b628fc 83
jamesheavey 18:991658b628fc 84 t_R.start();
jamesheavey 18:991658b628fc 85 enc_R.input(); // Make the I/O line an input (high impedance)
jamesheavey 19:4c08275cb3c9 86 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 87 // 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 88
jamesheavey 19:4c08275cb3c9 89 // 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 90 // this would increase sampling time
jamesheavey 19:4c08275cb3c9 91
jamesheavey 19:4c08275cb3c9 92 // 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 93 // if both are triggered it will run fast, otherwise it will have to wait 1000+ us for each sensor
jamesheavey 19:4c08275cb3c9 94
jamesheavey 19:4c08275cb3c9 95 // 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 96 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 97 t_R.stop();
jamesheavey 18:991658b628fc 98 t_R.reset();
jamesheavey 18:991658b628fc 99
jamesheavey 18:991658b628fc 100 t_L.start();
jamesheavey 18:991658b628fc 101 enc_L.input();
jamesheavey 19:4c08275cb3c9 102 while (enc_L == 1 || t_L.read_us() < 1000);
jamesheavey 18:991658b628fc 103 encoder[1] = t_L.read_us();
jamesheavey 18:991658b628fc 104 t_L.stop();
jamesheavey 18:991658b628fc 105 t_L.reset();
jamesheavey 18:991658b628fc 106 }
jamesheavey 18:991658b628fc 107
jamesheavey 0:df5216b20861 108 void init()
jamesheavey 0:df5216b20861 109 {
jamesheavey 0:df5216b20861 110 robot.init();
jamesheavey 0:df5216b20861 111
jamesheavey 0:df5216b20861 112 button_A.mode(PullUp);
jamesheavey 0:df5216b20861 113 button_B.mode(PullUp);
jamesheavey 0:df5216b20861 114 button_X.mode(PullUp);
jamesheavey 0:df5216b20861 115 button_Y.mode(PullUp);
jamesheavey 0:df5216b20861 116 button_enter.mode(PullUp);
jamesheavey 0:df5216b20861 117 button_back.mode(PullUp);
jamesheavey 0:df5216b20861 118
jamesheavey 10:691c02b352cb 119 leds = 0b0000;
jamesheavey 0:df5216b20861 120 }
jamesheavey 0:df5216b20861 121
jamesheavey 0:df5216b20861 122 void calibrate()
jamesheavey 0:df5216b20861 123 {
jamesheavey 0:df5216b20861 124 leds = 0b1111;
jamesheavey 0:df5216b20861 125 robot.reset_calibration();
jamesheavey 0:df5216b20861 126
jamesheavey 15:6c461501d12d 127 while (button_enter.read() == 1) {} // wait for enter to be pressed
jamesheavey 15:6c461501d12d 128
jamesheavey 15:6c461501d12d 129 wait(2.0);
jamesheavey 0:df5216b20861 130
jamesheavey 15:6c461501d12d 131 robot.auto_calibrate();
jamesheavey 17:77b8515a9568 132
jamesheavey 15:6c461501d12d 133 robot.stop();
jamesheavey 15:6c461501d12d 134 wait(0.05);
jamesheavey 15:6c461501d12d 135 robot.scan();
jamesheavey 17:77b8515a9568 136
jamesheavey 17:77b8515a9568 137 leds = 0b0000;
jamesheavey 0:df5216b20861 138 }
jamesheavey 20:5cf6a378801d 139
jamesheavey 20:5cf6a378801d 140 void non_looped()
jamesheavey 20:5cf6a378801d 141 {
jamesheavey 20:5cf6a378801d 142 follow_line();
jamesheavey 20:5cf6a378801d 143
jamesheavey 20:5cf6a378801d 144 if ( junction_detect() ) {
jamesheavey 20:5cf6a378801d 145 char turn = junction_logic();
jamesheavey 20:5cf6a378801d 146 turn_select(turn);
jamesheavey 20:5cf6a378801d 147
jamesheavey 20:5cf6a378801d 148 path[path_length] = turn;
jamesheavey 20:5cf6a378801d 149 path_length ++;
jamesheavey 20:5cf6a378801d 150 }
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 20:5cf6a378801d 161 void looped()
jamesheavey 20:5cf6a378801d 162 {
jamesheavey 20:5cf6a378801d 163 // follow line until reaching a junction, determine its type and coordinates
jamesheavey 21:54ea75f7984f 164 if (t_restart){ // only start the timer if it isnt already started
jamesheavey 21:54ea75f7984f 165 t_coord.start();
jamesheavey 21:54ea75f7984f 166 t_restart = false;
jamesheavey 21:54ea75f7984f 167 }
jamesheavey 21:54ea75f7984f 168
jamesheavey 21:54ea75f7984f 169 follow_line();
jamesheavey 21:54ea75f7984f 170
jamesheavey 21:54ea75f7984f 171 if ( junction_detect() ) {
jamesheavey 22:02dda79d50b4 172 index++; // index to path position basically path_length
jamesheavey 22:02dda79d50b4 173
jamesheavey 21:54ea75f7984f 174 int dist_est = t_coord.read();
jamesheavey 21:54ea75f7984f 175 t_coord.stop();
jamesheavey 21:54ea75f7984f 176 t_coord.reset();
jamesheavey 21:54ea75f7984f 177
jamesheavey 21:54ea75f7984f 178 //auto dist_est = time - (time%1); // round the value (nearest second)
jamesheavey 21:54ea75f7984f 179
jamesheavey 21:54ea75f7984f 180 if (dir == 'N'){ curr_coords[1] += dist_est; } // y coord
jamesheavey 21:54ea75f7984f 181 if (dir == 'E'){ curr_coords[0] += dist_est; } // x coord
jamesheavey 21:54ea75f7984f 182 if (dir == 'S'){ curr_coords[1] -= dist_est; }
jamesheavey 21:54ea75f7984f 183 if (dir == 'W'){ curr_coords[0] -= dist_est; }
jamesheavey 21:54ea75f7984f 184
jamesheavey 21:54ea75f7984f 185 // 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 186 if (coord_check()) { // coord_check returns true if curr_coords coordinates are not present in (should set a variable as true)
jamesheavey 22:02dda79d50b4 187 total_points++;
jamesheavey 22:02dda79d50b4 188 point[total_points] = total_points; // numbered 1 -> x
jamesheavey 22:02dda79d50b4 189 coords_x[total_points] = curr_coords[0];
jamesheavey 22:02dda79d50b4 190 coords_y[total_points] = curr_coords[1];
jamesheavey 22:02dda79d50b4 191 node_logic(); // determines what junction type we are at updates the explored and type arrays accordingly
jamesheavey 22:02dda79d50b4 192 // update type and explored aswell might need to be done in node logic
jamesheavey 22:02dda79d50b4 193 }
jamesheavey 21:54ea75f7984f 194
jamesheavey 22:02dda79d50b4 195 // use current coords to find which point to place in path
jamesheavey 22:02dda79d50b4 196 looped_path[index] = coord_to_node(); //returns an int of which point we are at
jamesheavey 22:02dda79d50b4 197 do_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn
jamesheavey 21:54ea75f7984f 198
jamesheavey 22:02dda79d50b4 199 check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
jamesheavey 21:54ea75f7984f 200
jamesheavey 21:54ea75f7984f 201 // assign new node with new coordinates
jamesheavey 21:54ea75f7984f 202 t_restart = true; //restart the timer
jamesheavey 21:54ea75f7984f 203
jamesheavey 21:54ea75f7984f 204 }
jamesheavey 21:54ea75f7984f 205
jamesheavey 22:02dda79d50b4 206 // robot.lcd_clear();
jamesheavey 22:02dda79d50b4 207 // char xcors[100];
jamesheavey 22:02dda79d50b4 208 // char ycors[100];
jamesheavey 22:02dda79d50b4 209 //
jamesheavey 22:02dda79d50b4 210 // for( unsigned int a = 0; a <= total_points; a += 1 )
jamesheavey 22:02dda79d50b4 211 // {
jamesheavey 22:02dda79d50b4 212 // char x[1];
jamesheavey 22:02dda79d50b4 213 // char y[1];
jamesheavey 22:02dda79d50b4 214 // sprintf(x,"%d",coords_x[a]);
jamesheavey 22:02dda79d50b4 215 // sprintf(y,"%d",coords_y[a]);
jamesheavey 22:02dda79d50b4 216 // xcors[a] = x[1];
jamesheavey 22:02dda79d50b4 217 // ycors[a] = y[];
jamesheavey 22:02dda79d50b4 218 // }
jamesheavey 22:02dda79d50b4 219 // robot.lcd_goto_xy(0,0);
jamesheavey 22:02dda79d50b4 220 // robot.lcd_print(xcors,100);
jamesheavey 22:02dda79d50b4 221 // robot.lcd_goto_xy(0,1);
jamesheavey 22:02dda79d50b4 222 // robot.lcd_print(ycors,100);
jamesheavey 21:54ea75f7984f 223
jamesheavey 20:5cf6a378801d 224 //robot.display_data();
jamesheavey 20:5cf6a378801d 225 }
jamesheavey 21:54ea75f7984f 226
jamesheavey 21:54ea75f7984f 227 void node_logic()
jamesheavey 21:54ea75f7984f 228 {
jamesheavey 21:54ea75f7984f 229 bool north = false;
jamesheavey 21:54ea75f7984f 230 bool south = false;
jamesheavey 21:54ea75f7984f 231 bool east = false;
jamesheavey 21:54ea75f7984f 232 bool west = false;
jamesheavey 21:54ea75f7984f 233
jamesheavey 21:54ea75f7984f 234 bool left = false;
jamesheavey 21:54ea75f7984f 235 bool straight = false;
jamesheavey 21:54ea75f7984f 236 bool right = false;
jamesheavey 21:54ea75f7984f 237
jamesheavey 21:54ea75f7984f 238 int total = 1; // starts at 1 because path entered on is counted
jamesheavey 21:54ea75f7984f 239
jamesheavey 21:54ea75f7984f 240 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 21:54ea75f7984f 241 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 21:54ea75f7984f 242 robot.forward(speed);
jamesheavey 21:54ea75f7984f 243 robot.scan();
jamesheavey 21:54ea75f7984f 244 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 21:54ea75f7984f 245 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 21:54ea75f7984f 246 }
jamesheavey 21:54ea75f7984f 247
jamesheavey 21:54ea75f7984f 248 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 249 wait(0.05); // maybe change or replace w something better
jamesheavey 21:54ea75f7984f 250 robot.scan();
jamesheavey 21:54ea75f7984f 251 }
jamesheavey 21:54ea75f7984f 252
jamesheavey 21:54ea75f7984f 253 robot.scan();
jamesheavey 21:54ea75f7984f 254
jamesheavey 21:54ea75f7984f 255 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 21:54ea75f7984f 256 straight = true;
jamesheavey 21:54ea75f7984f 257 }
jamesheavey 21:54ea75f7984f 258 }
jamesheavey 21:54ea75f7984f 259
jamesheavey 21:54ea75f7984f 260 if (left) { total += 1; }
jamesheavey 21:54ea75f7984f 261 if (straight) { total += 1; }
jamesheavey 21:54ea75f7984f 262 if (right) { total += 1; }
jamesheavey 21:54ea75f7984f 263
jamesheavey 21:54ea75f7984f 264 type[total_points] = total;
jamesheavey 21:54ea75f7984f 265
jamesheavey 21:54ea75f7984f 266 int angle = 0;
jamesheavey 21:54ea75f7984f 267
jamesheavey 21:54ea75f7984f 268 if (dir == 'E') { angle = 90;}
jamesheavey 21:54ea75f7984f 269 else if (dir == 'S') { angle = 180;}
jamesheavey 21:54ea75f7984f 270 else if (dir == 'W') { angle = 270;}
jamesheavey 21:54ea75f7984f 271
jamesheavey 21:54ea75f7984f 272 if (left) {
jamesheavey 21:54ea75f7984f 273 angle += 270;
jamesheavey 21:54ea75f7984f 274 angle = angle % 360;
jamesheavey 21:54ea75f7984f 275 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 276 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 277 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 278 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 279 angle -= 270;
jamesheavey 21:54ea75f7984f 280 angle = angle % 360;
jamesheavey 21:54ea75f7984f 281 }
jamesheavey 21:54ea75f7984f 282
jamesheavey 21:54ea75f7984f 283 if (right) {
jamesheavey 21:54ea75f7984f 284 angle += 90;
jamesheavey 21:54ea75f7984f 285 angle = angle % 360;
jamesheavey 21:54ea75f7984f 286 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 287 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 288 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 289 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 290 angle -= 90;
jamesheavey 21:54ea75f7984f 291 angle = angle % 360;
jamesheavey 21:54ea75f7984f 292 }
jamesheavey 0:df5216b20861 293
jamesheavey 21:54ea75f7984f 294 if (straight) {
jamesheavey 21:54ea75f7984f 295 if (angle == 0) { north = true; }
jamesheavey 21:54ea75f7984f 296 if (angle == 180) { south = true; }
jamesheavey 21:54ea75f7984f 297 if (angle == 90) { east = true; }
jamesheavey 21:54ea75f7984f 298 if (angle == 270) { west = true; }
jamesheavey 21:54ea75f7984f 299 }
jamesheavey 21:54ea75f7984f 300
jamesheavey 21:54ea75f7984f 301 int turn_count = type[total_points] - explored[total_points];
jamesheavey 21:54ea75f7984f 302
jamesheavey 21:54ea75f7984f 303 if (west == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 304 if (north == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 305 if (east == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 306 if (south == false) { turn_count += 1; }
jamesheavey 21:54ea75f7984f 307
jamesheavey 21:54ea75f7984f 308 turn_count = turn_count%4;
jamesheavey 21:54ea75f7984f 309
jamesheavey 21:54ea75f7984f 310 char do_turn = turn_priority[turn_count];
jamesheavey 21:54ea75f7984f 311
jamesheavey 21:54ea75f7984f 312 if (dir == 'E') { angle = 90;}
jamesheavey 21:54ea75f7984f 313 else if (dir == 'S') { angle = 180;}
jamesheavey 21:54ea75f7984f 314 else if (dir == 'W') { angle = 270;}
jamesheavey 21:54ea75f7984f 315
jamesheavey 21:54ea75f7984f 316 unsigned int result = 0;
jamesheavey 21:54ea75f7984f 317
jamesheavey 21:54ea75f7984f 318 if (do_turn == 'N') { dir = 'N'; result = 0; }
jamesheavey 21:54ea75f7984f 319 else if (do_turn == 'S') { dir = 'S'; result = 180; }
jamesheavey 21:54ea75f7984f 320 else if (do_turn == 'E') { dir = 'E'; result = 90; }
jamesheavey 21:54ea75f7984f 321 else if (do_turn == 'W') { dir = 'W'; result = 270; }
jamesheavey 21:54ea75f7984f 322
jamesheavey 21:54ea75f7984f 323 result = result - angle;
jamesheavey 21:54ea75f7984f 324
jamesheavey 21:54ea75f7984f 325 if (result == 0) { turn_select('S'); }
jamesheavey 21:54ea75f7984f 326 else if (result == 90) { turn_select('R'); }
jamesheavey 21:54ea75f7984f 327 else if (result == 180) { turn_select('B'); }
jamesheavey 21:54ea75f7984f 328 else if (result == 270) { turn_select('L'); }
jamesheavey 21:54ea75f7984f 329 // figure out a way to check what directions correlate to which turns based on direction currently facing.
jamesheavey 21:54ea75f7984f 330 }
jamesheavey 21:54ea75f7984f 331
jamesheavey 21:54ea75f7984f 332 void choose_turn()
jamesheavey 21:54ea75f7984f 333 {
jamesheavey 21:54ea75f7984f 334 // compares the type to the explored (type - explored), then uses that as an index for the struct of the turn order priority (turn_order[3] = {'E' , 'N', 'W', 'S'}
jamesheavey 21:54ea75f7984f 335 // also takes into account current direction (if facing north, it wont take the south path for example)
jamesheavey 21:54ea75f7984f 336 // do this in point type
jamesheavey 21:54ea75f7984f 337
jamesheavey 21:54ea75f7984f 338 }
jamesheavey 21:54ea75f7984f 339
jamesheavey 10:691c02b352cb 340 void follow_line()
jamesheavey 0:df5216b20861 341 {
jamesheavey 10:691c02b352cb 342 robot.scan();
jamesheavey 10:691c02b352cb 343 sensor = robot.get_sensors(); // returns the current values of all the sensors from 0-1000
jamesheavey 10:691c02b352cb 344
jamesheavey 15:6c461501d12d 345 leds = 0b0110;
jamesheavey 15:6c461501d12d 346
jamesheavey 10:691c02b352cb 347 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 348 derivative = proportional - prev_proportional;
jamesheavey 10:691c02b352cb 349 integral += proportional;
jamesheavey 10:691c02b352cb 350 prev_proportional = proportional;
jamesheavey 10:691c02b352cb 351
jamesheavey 10:691c02b352cb 352 // calculate motor correction
jamesheavey 10:691c02b352cb 353 float motor_correction = proportional*A + integral*B + derivative*C;
jamesheavey 10:691c02b352cb 354
jamesheavey 10:691c02b352cb 355 // make sure the correction is never greater than the max speed as the motor will reverse
jamesheavey 10:691c02b352cb 356 if( motor_correction > speed ) {
jamesheavey 10:691c02b352cb 357 motor_correction = speed;
jamesheavey 10:691c02b352cb 358 }
jamesheavey 10:691c02b352cb 359 if( motor_correction < -speed ) {
jamesheavey 10:691c02b352cb 360 motor_correction = -speed;
jamesheavey 10:691c02b352cb 361 }
jamesheavey 0:df5216b20861 362
jamesheavey 10:691c02b352cb 363 if( proportional < 0 ) {
jamesheavey 10:691c02b352cb 364 robot.motors(speed+motor_correction,speed);
jamesheavey 10:691c02b352cb 365 } else {
jamesheavey 10:691c02b352cb 366 robot.motors(speed,speed-motor_correction);
jamesheavey 10:691c02b352cb 367 }
jamesheavey 18:991658b628fc 368
jamesheavey 19:4c08275cb3c9 369 // read_encoders();
jamesheavey 19:4c08275cb3c9 370 // 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 371 // if (encoder[1] > 3100) { dist_est_2 += 1; } // might not need to actually use 2pir/3 could just add arbitrary numbers
jamesheavey 1:79219d0a33c8 372 }
jamesheavey 0:df5216b20861 373
jamesheavey 14:87052bb35211 374 bool junction_detect()
jamesheavey 6:c10b367747a0 375 {
jamesheavey 20:5cf6a378801d 376 if ( sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 377 return true;
jamesheavey 20:5cf6a378801d 378 } else if ( sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH ) {
jamesheavey 14:87052bb35211 379 return true;
jamesheavey 14:87052bb35211 380 } else {
jamesheavey 14:87052bb35211 381 return false;
jamesheavey 14:87052bb35211 382 }
jamesheavey 14:87052bb35211 383 }
jamesheavey 14:87052bb35211 384
jamesheavey 14:87052bb35211 385 char junction_logic()
jamesheavey 14:87052bb35211 386 {
jamesheavey 14:87052bb35211 387 bool straight = false;
jamesheavey 7:7fefd782532d 388 bool left = false;
jamesheavey 6:c10b367747a0 389 bool right = false;
jamesheavey 9:952586accbf9 390 bool goal = false;
jamesheavey 7:7fefd782532d 391
jamesheavey 20:5cf6a378801d 392 if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
jamesheavey 20:5cf6a378801d 393 while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
jamesheavey 13:bd271266e161 394 robot.forward(speed);
jamesheavey 9:952586accbf9 395 robot.scan();
jamesheavey 20:5cf6a378801d 396 if ( sensor[0] > SENS_THRESH ) { left = true; }
jamesheavey 20:5cf6a378801d 397 if ( sensor[4] > SENS_THRESH ) { right = true; }
jamesheavey 13:bd271266e161 398 }
jamesheavey 7:7fefd782532d 399
jamesheavey 20:5cf6a378801d 400 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 15:6c461501d12d 401 wait(0.05); // maybe change or replace w something better
jamesheavey 9:952586accbf9 402 robot.scan();
jamesheavey 20:5cf6a378801d 403 if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
jamesheavey 13:bd271266e161 404 goal = true;
jamesheavey 12:d80399686f32 405 }
jamesheavey 13:bd271266e161 406 }
jamesheavey 14:87052bb35211 407
jamesheavey 14:87052bb35211 408 robot.scan();
jamesheavey 14:87052bb35211 409
jamesheavey 20:5cf6a378801d 410 if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
jamesheavey 14:87052bb35211 411 straight = true;
jamesheavey 14:87052bb35211 412 }
jamesheavey 14:87052bb35211 413
jamesheavey 20:5cf6a378801d 414 } else if (sensor[1] < SENS_THRESH && sensor[2] < SENS_THRESH && sensor[3] < SENS_THRESH) {
jamesheavey 9:952586accbf9 415 return 'B';
jamesheavey 6:c10b367747a0 416 }
jamesheavey 7:7fefd782532d 417
jamesheavey 9:952586accbf9 418 if (goal) {
jamesheavey 9:952586accbf9 419 return 'G';
jamesheavey 9:952586accbf9 420 } else if (left) {
jamesheavey 9:952586accbf9 421 return 'L';
jamesheavey 14:87052bb35211 422 } else if (straight) {
jamesheavey 14:87052bb35211 423 return 'S';
jamesheavey 9:952586accbf9 424 } else if (right) {
jamesheavey 9:952586accbf9 425 return 'R';
jamesheavey 6:c10b367747a0 426 } else {
jamesheavey 9:952586accbf9 427 return 'S';
jamesheavey 9:952586accbf9 428 }
jamesheavey 6:c10b367747a0 429 }
jamesheavey 6:c10b367747a0 430
jamesheavey 0:df5216b20861 431
jamesheavey 14:87052bb35211 432 void turn_select( char turn )
jamesheavey 1:79219d0a33c8 433 {
jamesheavey 1:79219d0a33c8 434 switch(turn) {
jamesheavey 5:ae417756235a 435 case 'G':
jamesheavey 5:ae417756235a 436 goal();
jamesheavey 1:79219d0a33c8 437 case 'L':
jamesheavey 1:79219d0a33c8 438 left();
jamesheavey 5:ae417756235a 439 break;
jamesheavey 1:79219d0a33c8 440 case 'S':
jamesheavey 1:79219d0a33c8 441 break;
jamesheavey 1:79219d0a33c8 442 case 'R':
jamesheavey 1:79219d0a33c8 443 right();
jamesheavey 5:ae417756235a 444 break;
jamesheavey 1:79219d0a33c8 445 case 'B':
jamesheavey 1:79219d0a33c8 446 back();
jamesheavey 5:ae417756235a 447 break;
jamesheavey 1:79219d0a33c8 448 }
jamesheavey 1:79219d0a33c8 449 }
jamesheavey 1:79219d0a33c8 450
jamesheavey 0:df5216b20861 451 void left()
jamesheavey 0:df5216b20861 452 {
jamesheavey 1:79219d0a33c8 453 leds = 0b1100;
jamesheavey 3:a5e06482462e 454
jamesheavey 20:5cf6a378801d 455 while (sensor[0] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 456
jamesheavey 20:5cf6a378801d 457 robot.spin_left(0.2);
jamesheavey 3:a5e06482462e 458 wait(0.1);
jamesheavey 3:a5e06482462e 459
jamesheavey 20:5cf6a378801d 460 while (sensor[1] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 461
jamesheavey 20:5cf6a378801d 462 while (sensor[1] > SENS_THRESH) { robot.scan(); }
jamesheavey 1:79219d0a33c8 463 }
jamesheavey 1:79219d0a33c8 464
jamesheavey 1:79219d0a33c8 465 void right()
jamesheavey 1:79219d0a33c8 466 {
jamesheavey 1:79219d0a33c8 467 leds = 0b0011;
jamesheavey 5:ae417756235a 468
jamesheavey 20:5cf6a378801d 469 while (sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 470
jamesheavey 20:5cf6a378801d 471 robot.spin_right(TURN_SPEED);
jamesheavey 3:a5e06482462e 472 wait(0.1);
jamesheavey 3:a5e06482462e 473
jamesheavey 20:5cf6a378801d 474 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 475
jamesheavey 20:5cf6a378801d 476 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 477 }
jamesheavey 0:df5216b20861 478
jamesheavey 0:df5216b20861 479 void back()
jamesheavey 0:df5216b20861 480 {
jamesheavey 1:79219d0a33c8 481 leds = 0b1111;
jamesheavey 5:ae417756235a 482 robot.reverse(speed);
jamesheavey 5:ae417756235a 483 wait(0.1);
jamesheavey 20:5cf6a378801d 484 robot.spin_right(TURN_SPEED);
jamesheavey 3:a5e06482462e 485
jamesheavey 20:5cf6a378801d 486 while (sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 3:a5e06482462e 487
jamesheavey 20:5cf6a378801d 488 while (sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 0:df5216b20861 489 }
jamesheavey 1:79219d0a33c8 490
jamesheavey 2:940e46e21353 491 void simplify()
jamesheavey 1:79219d0a33c8 492 {
jamesheavey 2:940e46e21353 493 // check if the last one was a 'B'
jamesheavey 2:940e46e21353 494 // if it was, iterate over the last three turns and check the total angle change
jamesheavey 2:940e46e21353 495 // replace the three turns with the new single turn
jamesheavey 1:79219d0a33c8 496
jamesheavey 10:691c02b352cb 497 if( path[path_length-2] == 'B' && path_length >= 3) {
jamesheavey 10:691c02b352cb 498 int angle_change = 0;
jamesheavey 2:940e46e21353 499
jamesheavey 10:691c02b352cb 500 for (int i = 1; i <= 3; i++) {
jamesheavey 10:691c02b352cb 501 if (path[path_length - i] == 'L') { angle_change += 270; }
jamesheavey 10:691c02b352cb 502 else if (path[path_length - i] == 'R') { angle_change += 90; }
jamesheavey 10:691c02b352cb 503 else if (path[path_length - i] == 'B') { angle_change += 180; }
jamesheavey 4:38c29dbc5953 504 }
jamesheavey 4:38c29dbc5953 505
jamesheavey 4:38c29dbc5953 506 angle_change = angle_change % 360;
jamesheavey 4:38c29dbc5953 507
jamesheavey 4:38c29dbc5953 508 if (angle_change == 0) { path[path_length - 3] = 'S'; }
jamesheavey 4:38c29dbc5953 509 else if (angle_change == 90) { path[path_length - 3] = 'R'; }
jamesheavey 4:38c29dbc5953 510 else if (angle_change == 180) { path[path_length - 3] = 'B'; }
jamesheavey 4:38c29dbc5953 511 else if (angle_change == 270) { path[path_length - 3] = 'L'; }
jamesheavey 4:38c29dbc5953 512
jamesheavey 4:38c29dbc5953 513 for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; } // clear the other turns
jamesheavey 4:38c29dbc5953 514
jamesheavey 4:38c29dbc5953 515 path_length -= 2;
jamesheavey 2:940e46e21353 516 }
jamesheavey 5:ae417756235a 517 }
jamesheavey 5:ae417756235a 518
jamesheavey 10:691c02b352cb 519 void goal()
jamesheavey 10:691c02b352cb 520 {
jamesheavey 10:691c02b352cb 521 invert_path();
jamesheavey 11:c3299aca7d8f 522
jamesheavey 11:c3299aca7d8f 523 leds = 0b0000;
jamesheavey 10:691c02b352cb 524
jamesheavey 10:691c02b352cb 525 robot.lcd_clear();
jamesheavey 16:96c7dc8a1119 526 robot.lcd_print(inv_path,100);
jamesheavey 10:691c02b352cb 527
jamesheavey 11:c3299aca7d8f 528 while(1) {
jamesheavey 15:6c461501d12d 529 int pointer = 0;
jamesheavey 15:6c461501d12d 530
jamesheavey 15:6c461501d12d 531 robot.stop();
jamesheavey 15:6c461501d12d 532
jamesheavey 15:6c461501d12d 533 leds = 0b1001;
jamesheavey 15:6c461501d12d 534 wait(0.2);
jamesheavey 15:6c461501d12d 535 leds = 0b0110;
jamesheavey 15:6c461501d12d 536 wait(0.2);
jamesheavey 15:6c461501d12d 537
jamesheavey 15:6c461501d12d 538 robot.reverse(speed);
jamesheavey 20:5cf6a378801d 539 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 16:96c7dc8a1119 540 wait(0.05);
jamesheavey 15:6c461501d12d 541
jamesheavey 20:5cf6a378801d 542 robot.spin_right(TURN_SPEED);
jamesheavey 20:5cf6a378801d 543 while(sensor[2] > SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 544 while(sensor[3] < SENS_THRESH) { robot.scan(); }
jamesheavey 20:5cf6a378801d 545 while(sensor[3] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 546
jamesheavey 15:6c461501d12d 547 robot.stop();
jamesheavey 15:6c461501d12d 548
jamesheavey 15:6c461501d12d 549 while(pointer <= path_length) {
jamesheavey 15:6c461501d12d 550 follow_line();
jamesheavey 15:6c461501d12d 551
jamesheavey 15:6c461501d12d 552 if ( junction_detect() ) { // if junction found
jamesheavey 16:96c7dc8a1119 553 char na = junction_logic(); // aids turing fluidity (char not necessary therefore could clean up a bit)
jamesheavey 15:6c461501d12d 554 turn_select(inv_path[pointer]);
jamesheavey 15:6c461501d12d 555 if(inv_path[pointer] == 'S') { // make this better
jamesheavey 15:6c461501d12d 556 robot.forward(speed);
jamesheavey 15:6c461501d12d 557 leds = 0b1010;
jamesheavey 20:5cf6a378801d 558 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 15:6c461501d12d 559 }
jamesheavey 15:6c461501d12d 560 pointer++;
jamesheavey 15:6c461501d12d 561 }
jamesheavey 15:6c461501d12d 562 }
jamesheavey 15:6c461501d12d 563
jamesheavey 15:6c461501d12d 564 back();
jamesheavey 12:d80399686f32 565
jamesheavey 11:c3299aca7d8f 566 robot.stop();
jamesheavey 20:5cf6a378801d 567 robot.lcd_goto_xy(0,0);
jamesheavey 20:5cf6a378801d 568 robot.lcd_print(" ENTER ", 10);
jamesheavey 20:5cf6a378801d 569 robot.lcd_goto_xy(0,1);
jamesheavey 20:5cf6a378801d 570 robot.lcd_print("=restart", 10);
jamesheavey 20:5cf6a378801d 571
jamesheavey 14:87052bb35211 572 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 573
jamesheavey 20:5cf6a378801d 574 robot.lcd_clear();
jamesheavey 20:5cf6a378801d 575 robot.lcd_print(path,100);
jamesheavey 20:5cf6a378801d 576
jamesheavey 15:6c461501d12d 577 pointer = 0;
jamesheavey 15:6c461501d12d 578
jamesheavey 11:c3299aca7d8f 579 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 580 wait(0.2);
jamesheavey 11:c3299aca7d8f 581 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 582 wait(0.2);
jamesheavey 11:c3299aca7d8f 583 leds = 0b1001;
jamesheavey 11:c3299aca7d8f 584 wait(0.2);
jamesheavey 11:c3299aca7d8f 585 leds = 0b0110;
jamesheavey 11:c3299aca7d8f 586 wait(0.2);
jamesheavey 11:c3299aca7d8f 587
jamesheavey 11:c3299aca7d8f 588 while(pointer <= path_length) {
jamesheavey 11:c3299aca7d8f 589 follow_line();
jamesheavey 11:c3299aca7d8f 590
jamesheavey 14:87052bb35211 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 14:87052bb35211 593 turn_select(path[pointer]);
jamesheavey 11:c3299aca7d8f 594 if(path[pointer] == 'S') { // make this better
jamesheavey 11:c3299aca7d8f 595 robot.forward(speed);
jamesheavey 11:c3299aca7d8f 596 leds = 0b1010;
jamesheavey 20:5cf6a378801d 597 while(sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) { robot.scan(); }
jamesheavey 11:c3299aca7d8f 598 }
jamesheavey 11:c3299aca7d8f 599 pointer++;
jamesheavey 10:691c02b352cb 600 }
jamesheavey 10:691c02b352cb 601 }
jamesheavey 5:ae417756235a 602 }
jamesheavey 5:ae417756235a 603 }
jamesheavey 5:ae417756235a 604
jamesheavey 10:691c02b352cb 605 void invert_path()
jamesheavey 5:ae417756235a 606 {
jamesheavey 10:691c02b352cb 607 // only call once then can use infinitely
jamesheavey 16:96c7dc8a1119 608 for( int i = 0; i < path_length; i++ ){
jamesheavey 16:96c7dc8a1119 609 if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
jamesheavey 16:96c7dc8a1119 610 else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
jamesheavey 16:96c7dc8a1119 611 else { inv_path[i] = path[path_length-1-i]; }
jamesheavey 10:691c02b352cb 612 }
jamesheavey 5:ae417756235a 613 }