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@29:2224bc8bb49d, 2017-05-27 (annotated)

Committer:
kolanery
Date:
Sat May 27 08:07:13 2017 +0000
Revision:
29:2224bc8bb49d
Parent:
28:600932142201
Child:
30:daf286ac049f
Update both algorithms.

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