TomYumBoys / Mbed 2 deprecated MM2017

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

main.cpp@31:f7a8a9b82bc1, 2017-05-28 (annotated)

Committer:
kolanery
Date:
Sun May 28 09:54:40 2017 +0000
Revision:
31:f7a8a9b82bc1
Parent:
30:daf286ac049f 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kolanery 0:cb667de3a336 1 #include "drivecontrol.h"
kolanery 4:73510c7fa316 2 #include "io_modules.h"
szh66 6:1bcfda49e146 3 #include "mbed.h"
ryan_whr 25:7155cb993870 4 #include "maze_solver.h"
kolanery 16:c26d8e007df5 5 //// PIN ASSIGNMENTS
szh66 6:1bcfda49e146 6 AnalogIn battery(PA_3);
szh66 6:1bcfda49e146 7 DigitalOut led_1(PB_12);
szh66 6:1bcfda49e146 8 DigitalOut led_2(PB_13);
szh66 6:1bcfda49e146 9 DigitalOut led_3(PB_14);
szh66 6:1bcfda49e146 10 DigitalOut led_4(PB_15);
szh66 6:1bcfda49e146 11 Serial serial (PA_9, PA_10);
kolanery 29:2224bc8bb49d 12
kolanery 29:2224bc8bb49d 13 // Algorithm Switch
kolanery 29:2224bc8bb49d 14 DigitalIn alg_pin (PC_10);
kolanery 29:2224bc8bb49d 15
kolanery 16:c26d8e007df5 16 // CONTROL CONSTANTS
szh66 27:b980fce784ea 17 const unsigned char STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
kolanery 16:c26d8e007df5 18 const int START_POS = 0, END_POS = 0;
kolanery 16:c26d8e007df5 19 const int CONTROL = 1;
kolanery 28:600932142201 20 unsigned char state;
kolanery 28:600932142201 21 int move = 0;
kolanery 2:619b02232144 22
kolanery 16:c26d8e007df5 23 void check_battery () {
szh66 27:b980fce784ea 24 // pc.baud(9600);
szh66 27:b980fce784ea 25 // using the serial functions will have an impact on timing.
szh66 27:b980fce784ea 26 // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
szh66 27:b980fce784ea 27 // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
kolanery 15:151e59899221 28 if (battery.read() < 0.70f){
szh66 27:b980fce784ea 29 // flash led
kolanery 4:73510c7fa316 30 led_1 = 1;
kolanery 4:73510c7fa316 31 led_2 = 1;
kolanery 4:73510c7fa316 32 led_3 = 1;
kolanery 4:73510c7fa316 33 led_4 = 1;
kolanery 15:151e59899221 34 }
kolanery 0:cb667de3a336 35 }
kolanery 0:cb667de3a336 36
kolanery 16:c26d8e007df5 37 void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
kolanery 16:c26d8e007df5 38 led_1 = _led_1;
kolanery 16:c26d8e007df5 39 led_2 = _led_2;
kolanery 16:c26d8e007df5 40 led_3 = _led_3;
kolanery 16:c26d8e007df5 41 led_4 = _led_4;
kolanery 16:c26d8e007df5 42 }
kolanery 16:c26d8e007df5 43
kolanery 30:daf286ac049f 44 //void print_debug_message(){
kolanery 30:daf286ac049f 45 // serial.printf("Next mouse position <%u,%u> \r\n", my_mouse->mouse_x, my_mouse->mouse_y);
kolanery 30:daf286ac049f 46 // serial.printf("next state: %u \r\n", state);
kolanery 30:daf286ac049f 47 // serial.printf("previous mouse direction <%d> \r\n", my_mouse->get_prev_direction());
kolanery 30:daf286ac049f 48 // serial.printf("next cell direction <%d> \r\n", my_mouse->get_next_cell_direction());
kolanery 30:daf286ac049f 49 // move++;
kolanery 30:daf286ac049f 50 //}
kolanery 29:2224bc8bb49d 51
kolanery 29:2224bc8bb49d 52 void run_flood_fill_algorithm() {
kolanery 30:daf286ac049f 53 DriveControl * driver = new DriveControl (START_POS, END_POS);
kolanery 30:daf286ac049f 54 Mouse * my_mouse = new Mouse(driver);
kolanery 14:a646667ac9ea 55 driver->resetEncoders();
szh66 27:b980fce784ea 56 int count = 0;
kolanery 14:a646667ac9ea 57 bool finished_traverse_one_cell = true;
kolanery 30:daf286ac049f 58 wait(1);
kolanery 30:daf286ac049f 59 int count_move = 0;
szh66 27:b980fce784ea 60
szh66 27:b980fce784ea 61 while(CONTROL) {
szh66 27:b980fce784ea 62 check_battery();
szh66 27:b980fce784ea 63 wait_ms(1);
kolanery 30:daf286ac049f 64 // if(driver->right_wall_pid_debug()) {
kolanery 30:daf286ac049f 65 // flash_led(0,1,1,0);
kolanery 29:2224bc8bb49d 66 // }
kolanery 30:daf286ac049f 67 // if (my_mouse -> center_found()){
kolanery 30:daf286ac049f 68 // flash_led(1,1,1,1);
kolanery 30:daf286ac049f 69 // //wait(2);
kolanery 30:daf286ac049f 70 // //my_mouse = new Mouse(driver);
kolanery 30:daf286ac049f 71 // }
kolanery 26:191ec0e78774 72 if (finished_traverse_one_cell == true) {
kolanery 31:f7a8a9b82bc1 73 // if (!driver->has_right_wall() && !driver->has_left_wall()){
kolanery 31:f7a8a9b82bc1 74 // flash_led(0,0,0,0);
kolanery 31:f7a8a9b82bc1 75 // }
kolanery 31:f7a8a9b82bc1 76 // else if (!driver->has_left_wall()){
kolanery 31:f7a8a9b82bc1 77 // flash_led(1,0,0,0);
kolanery 31:f7a8a9b82bc1 78 // }
kolanery 31:f7a8a9b82bc1 79 // else if (!driver->has_right_wall()){
kolanery 31:f7a8a9b82bc1 80 // flash_led(0,1,1,1);
kolanery 31:f7a8a9b82bc1 81 // }
kolanery 31:f7a8a9b82bc1 82 // else{
kolanery 31:f7a8a9b82bc1 83 // flash_led(0,0,0,0);
kolanery 31:f7a8a9b82bc1 84 // }
kolanery 31:f7a8a9b82bc1 85
kolanery 31:f7a8a9b82bc1 86 if (my_mouse->center_found()) {
kolanery 31:f7a8a9b82bc1 87 flash_led(1,1,1,1);
kolanery 31:f7a8a9b82bc1 88 }
kolanery 31:f7a8a9b82bc1 89 //count_move++;
kolanery 30:daf286ac049f 90 // if (count_move > 18 && my_mouse->can_reset_mouse) {
kolanery 30:daf286ac049f 91 // my_mouse->clear_stack();
kolanery 30:daf286ac049f 92 // delete my_mouse;
kolanery 30:daf286ac049f 93 // count_move = 0;
kolanery 30:daf286ac049f 94 // flash_led(0,1,1,1);
kolanery 30:daf286ac049f 95 // wait(2);
kolanery 30:daf286ac049f 96 // run_flood_fill_algorithm();
kolanery 30:daf286ac049f 97 // }
kolanery 31:f7a8a9b82bc1 98 // if (count_move > 30) {
kolanery 31:f7a8a9b82bc1 99 // my_mouse->clear_stack();
kolanery 31:f7a8a9b82bc1 100 // count_move = 0;
kolanery 31:f7a8a9b82bc1 101 // flash_led(0,1,1,1);
kolanery 31:f7a8a9b82bc1 102 // wait(2);
kolanery 31:f7a8a9b82bc1 103 // }
kolanery 26:191ec0e78774 104 state = my_mouse->solve_maze();
kolanery 29:2224bc8bb49d 105 wait(0.5);
kolanery 26:191ec0e78774 106 if(state == RIGHT){
kolanery 31:f7a8a9b82bc1 107 //flash_led(0,0,1,1);
kolanery 26:191ec0e78774 108 wait(0.25);
kolanery 26:191ec0e78774 109 driver->resetEncoders();
kolanery 26:191ec0e78774 110 }
kolanery 26:191ec0e78774 111 else if(state == LEFT){
kolanery 31:f7a8a9b82bc1 112 //flash_led(1,1,0,0);
kolanery 26:191ec0e78774 113 wait(0.25);
kolanery 26:191ec0e78774 114 driver->resetEncoders();
kolanery 26:191ec0e78774 115 }
kolanery 26:191ec0e78774 116 else if(state == UTURN){
kolanery 31:f7a8a9b82bc1 117 //flash_led(1,1,1,0);
kolanery 26:191ec0e78774 118 wait(0.25);
kolanery 26:191ec0e78774 119 driver->resetEncoders();
kolanery 26:191ec0e78774 120 count = 0;
szh66 27:b980fce784ea 121 }else{
szh66 27:b980fce784ea 122 state = STRAIGHT;
kolanery 31:f7a8a9b82bc1 123 //flash_led(1, 0, 0, 0);
kolanery 26:191ec0e78774 124 }
szh66 27:b980fce784ea 125
kolanery 26:191ec0e78774 126 finished_traverse_one_cell = false;
kolanery 26:191ec0e78774 127 }
szh66 27:b980fce784ea 128
kolanery 26:191ec0e78774 129 if (state == STRAIGHT) {
kolanery 26:191ec0e78774 130 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 26:191ec0e78774 131 driver->drive_forward();
szh66 27:b980fce784ea 132
kolanery 26:191ec0e78774 133 }
kolanery 26:191ec0e78774 134 else {
kolanery 26:191ec0e78774 135 driver->stop();
kolanery 28:600932142201 136 wait(0.25);
kolanery 26:191ec0e78774 137 driver->resetEncoders();
kolanery 26:191ec0e78774 138 finished_traverse_one_cell = true;
kolanery 26:191ec0e78774 139 continue;
kolanery 26:191ec0e78774 140 }
kolanery 26:191ec0e78774 141 }
kolanery 26:191ec0e78774 142 if (state == RIGHT) {
kolanery 26:191ec0e78774 143 if (!driver->should_finish_turn_right()) {
kolanery 26:191ec0e78774 144 driver->turn_right();
kolanery 26:191ec0e78774 145 }
kolanery 26:191ec0e78774 146 else {
kolanery 26:191ec0e78774 147 driver->stop();
kolanery 26:191ec0e78774 148 state = STRAIGHT;
kolanery 28:600932142201 149 wait(0.25);
kolanery 26:191ec0e78774 150 driver->resetEncoders();
kolanery 26:191ec0e78774 151 driver->clear_pid();
kolanery 26:191ec0e78774 152 continue;
kolanery 26:191ec0e78774 153 }
kolanery 26:191ec0e78774 154 }
kolanery 26:191ec0e78774 155 if (state == LEFT) {
kolanery 26:191ec0e78774 156 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 157 driver->turn_left();
kolanery 26:191ec0e78774 158 }
kolanery 26:191ec0e78774 159 else {
kolanery 26:191ec0e78774 160 driver->stop();
kolanery 26:191ec0e78774 161 state = STRAIGHT;
kolanery 28:600932142201 162 wait(0.25);
kolanery 26:191ec0e78774 163 driver->resetEncoders();
kolanery 26:191ec0e78774 164 driver->clear_pid();
kolanery 26:191ec0e78774 165 continue;
kolanery 26:191ec0e78774 166 }
kolanery 26:191ec0e78774 167 }
kolanery 26:191ec0e78774 168
kolanery 26:191ec0e78774 169 if (state == UTURN){
kolanery 26:191ec0e78774 170 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 171 driver->turn_left();
kolanery 26:191ec0e78774 172 }
kolanery 26:191ec0e78774 173 else {
kolanery 26:191ec0e78774 174 driver->stop();
kolanery 26:191ec0e78774 175 driver->resetEncoders();
szh66 27:b980fce784ea 176 wait(0.25);
szh66 27:b980fce784ea 177 if(count == 1) {
szh66 27:b980fce784ea 178 state = STRAIGHT;
ryan_whr 22:1a02d66218e4 179 }
szh66 27:b980fce784ea 180 count ++;//
kolanery 16:c26d8e007df5 181 continue;
kolanery 16:c26d8e007df5 182 }
kolanery 20:b18eed69ee32 183 }
kolanery 16:c26d8e007df5 184 }
kolanery 29:2224bc8bb49d 185 }
kolanery 29:2224bc8bb49d 186
kolanery 29:2224bc8bb49d 187 void run_heuristic_wall_follower_algorithm() {
kolanery 29:2224bc8bb49d 188 DriveControl * driver = new DriveControl (START_POS, END_POS);
kolanery 30:daf286ac049f 189 driver->set_wall_follower_speed();
kolanery 31:f7a8a9b82bc1 190 driver->set_wall_follower_sensor_thres();
kolanery 29:2224bc8bb49d 191 driver->resetEncoders();
kolanery 29:2224bc8bb49d 192 int state, count = 0;
kolanery 30:daf286ac049f 193 int heuristic = 0;
kolanery 29:2224bc8bb49d 194 bool finished_traverse_one_cell = true;
kolanery 30:daf286ac049f 195 wait(1);
kolanery 29:2224bc8bb49d 196 while(CONTROL) {
kolanery 29:2224bc8bb49d 197 check_battery();
kolanery 29:2224bc8bb49d 198 wait_ms(1);
kolanery 29:2224bc8bb49d 199 if (finished_traverse_one_cell == true) {
kolanery 30:daf286ac049f 200 heuristic ++;
kolanery 30:daf286ac049f 201 if (heuristic % 10 == 0) {
kolanery 30:daf286ac049f 202 if (!driver->has_front_wall()){
kolanery 30:daf286ac049f 203 wait(0.25);
kolanery 30:daf286ac049f 204 state = STRAIGHT;
kolanery 30:daf286ac049f 205 driver->resetEncoders();
kolanery 30:daf286ac049f 206 }
kolanery 30:daf286ac049f 207 }
kolanery 29:2224bc8bb49d 208 if (!driver->has_right_wall() && !driver->has_left_wall()) {
kolanery 29:2224bc8bb49d 209 if ((rand() % 3) == 1) {
kolanery 29:2224bc8bb49d 210 state = LEFT;
kolanery 29:2224bc8bb49d 211 driver->resetEncoders();
kolanery 29:2224bc8bb49d 212 }
kolanery 29:2224bc8bb49d 213 else {
kolanery 29:2224bc8bb49d 214 state = RIGHT;
kolanery 29:2224bc8bb49d 215 driver->resetEncoders();
kolanery 30:daf286ac049f 216 }
kolanery 30:daf286ac049f 217 wait(0.25);
kolanery 29:2224bc8bb49d 218 }
kolanery 29:2224bc8bb49d 219 else if (!driver->has_right_wall()) {
kolanery 29:2224bc8bb49d 220 wait(0.25);
kolanery 29:2224bc8bb49d 221 state = RIGHT;
kolanery 29:2224bc8bb49d 222 driver->resetEncoders();
kolanery 29:2224bc8bb49d 223 }
kolanery 29:2224bc8bb49d 224 else if (!driver->has_left_wall()) {
kolanery 29:2224bc8bb49d 225 wait(0.25);
kolanery 29:2224bc8bb49d 226 state = LEFT;
kolanery 29:2224bc8bb49d 227 driver->resetEncoders();
kolanery 29:2224bc8bb49d 228 }
kolanery 29:2224bc8bb49d 229 else if(!driver->has_front_wall()){
kolanery 29:2224bc8bb49d 230 state = STRAIGHT;
kolanery 29:2224bc8bb49d 231 driver->resetEncoders();
kolanery 29:2224bc8bb49d 232 }
kolanery 29:2224bc8bb49d 233 else{
kolanery 29:2224bc8bb49d 234 wait(0.25);
kolanery 29:2224bc8bb49d 235 state = UTURN;
kolanery 29:2224bc8bb49d 236 driver->resetEncoders();
kolanery 29:2224bc8bb49d 237 count = 0;
kolanery 29:2224bc8bb49d 238 }
kolanery 29:2224bc8bb49d 239 finished_traverse_one_cell = false;
kolanery 29:2224bc8bb49d 240 }
kolanery 29:2224bc8bb49d 241
kolanery 29:2224bc8bb49d 242 if (state == STRAIGHT) {
kolanery 29:2224bc8bb49d 243 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 29:2224bc8bb49d 244 driver->drive_forward();
kolanery 29:2224bc8bb49d 245 flash_led(1, 0, 0, 0);
kolanery 29:2224bc8bb49d 246 }
kolanery 29:2224bc8bb49d 247 else {
kolanery 29:2224bc8bb49d 248 driver->stop();
kolanery 29:2224bc8bb49d 249 driver->resetEncoders();
kolanery 29:2224bc8bb49d 250 flash_led (0, 0, 0, 0);
kolanery 29:2224bc8bb49d 251 finished_traverse_one_cell = true;
kolanery 29:2224bc8bb49d 252 continue;
kolanery 29:2224bc8bb49d 253 }
kolanery 29:2224bc8bb49d 254 }
kolanery 29:2224bc8bb49d 255 if (state == RIGHT) {
kolanery 29:2224bc8bb49d 256 if (!driver->should_finish_turn_right()) {
kolanery 29:2224bc8bb49d 257 driver->turn_right();
kolanery 29:2224bc8bb49d 258 flash_led (0, 1, 0, 0);
kolanery 29:2224bc8bb49d 259 }
kolanery 29:2224bc8bb49d 260 else {
kolanery 29:2224bc8bb49d 261 driver->stop();
kolanery 29:2224bc8bb49d 262 flash_led (0, 0, 0, 0);
kolanery 29:2224bc8bb49d 263 state = STRAIGHT;
kolanery 29:2224bc8bb49d 264 driver->resetEncoders();
kolanery 29:2224bc8bb49d 265 driver->clear_pid();
kolanery 29:2224bc8bb49d 266 continue;
kolanery 29:2224bc8bb49d 267 }
kolanery 29:2224bc8bb49d 268 }
kolanery 29:2224bc8bb49d 269 if (state == LEFT) {
kolanery 29:2224bc8bb49d 270 if (!driver->should_finish_turn_left()) {
kolanery 29:2224bc8bb49d 271 driver->turn_left();
kolanery 29:2224bc8bb49d 272 flash_led (0, 0, 1, 0);
kolanery 29:2224bc8bb49d 273 }
kolanery 29:2224bc8bb49d 274 else {
kolanery 29:2224bc8bb49d 275 driver->stop();
kolanery 29:2224bc8bb49d 276 flash_led (0, 0, 0, 0);
kolanery 29:2224bc8bb49d 277 state = STRAIGHT;
kolanery 29:2224bc8bb49d 278 driver->resetEncoders();
kolanery 29:2224bc8bb49d 279 driver->clear_pid();
kolanery 29:2224bc8bb49d 280 continue;
kolanery 29:2224bc8bb49d 281 }
kolanery 29:2224bc8bb49d 282 }
kolanery 29:2224bc8bb49d 283
kolanery 29:2224bc8bb49d 284 if (state == UTURN){
kolanery 29:2224bc8bb49d 285 if (!driver->should_finish_turn_left()) {
kolanery 29:2224bc8bb49d 286 driver->turn_left();
kolanery 29:2224bc8bb49d 287 flash_led (0, 1, 1, 0);
kolanery 29:2224bc8bb49d 288 }
kolanery 29:2224bc8bb49d 289 else {
kolanery 29:2224bc8bb49d 290 driver->stop();
kolanery 29:2224bc8bb49d 291 driver->resetEncoders();
kolanery 29:2224bc8bb49d 292 flash_led (0, 0, 0, 0);
kolanery 29:2224bc8bb49d 293 if (count == 1) {
kolanery 29:2224bc8bb49d 294 finished_traverse_one_cell = true;
kolanery 29:2224bc8bb49d 295 }
kolanery 29:2224bc8bb49d 296 else {
kolanery 29:2224bc8bb49d 297 state = LEFT;
kolanery 29:2224bc8bb49d 298 wait(0.25);
kolanery 29:2224bc8bb49d 299 count++;
kolanery 29:2224bc8bb49d 300 }
kolanery 29:2224bc8bb49d 301 continue;
kolanery 29:2224bc8bb49d 302 }
kolanery 29:2224bc8bb49d 303 }
kolanery 29:2224bc8bb49d 304 }
kolanery 29:2224bc8bb49d 305 }
kolanery 29:2224bc8bb49d 306
kolanery 29:2224bc8bb49d 307 int main() {
kolanery 29:2224bc8bb49d 308 if (alg_pin) {
kolanery 29:2224bc8bb49d 309 flash_led(1,0,1,0);
kolanery 29:2224bc8bb49d 310 wait(2);
kolanery 29:2224bc8bb49d 311 run_flood_fill_algorithm();
kolanery 29:2224bc8bb49d 312 }
kolanery 29:2224bc8bb49d 313 else {
kolanery 29:2224bc8bb49d 314 flash_led(1,1,1,0);
kolanery 29:2224bc8bb49d 315 wait(2);
kolanery 29:2224bc8bb49d 316 run_heuristic_wall_follower_algorithm();
kolanery 29:2224bc8bb49d 317 }
kolanery 26:191ec0e78774 318 }