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@26:191ec0e78774, 2017-05-21 (annotated)

Committer:
kolanery
Date:
Sun May 21 08:10:29 2017 +0000
Revision:
26:191ec0e78774
Parent:
25:7155cb993870
Child:
27:b980fce784ea
Update baseline version for UCSD competition.

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 16:c26d8e007df5 12 // CONTROL CONSTANTS
kolanery 16:c26d8e007df5 13 const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
kolanery 16:c26d8e007df5 14 const int START_POS = 0, END_POS = 0;
kolanery 16:c26d8e007df5 15 const int CONTROL = 1;
kolanery 26:191ec0e78774 16 Timer flood_fill_timer;
kolanery 26:191ec0e78774 17 Timer state_timeout_timer;
kolanery 2:619b02232144 18
kolanery 16:c26d8e007df5 19 void check_battery () {
kolanery 15:151e59899221 20 if (battery.read() < 0.70f){
kolanery 4:73510c7fa316 21 led_1 = 1;
kolanery 4:73510c7fa316 22 led_2 = 1;
kolanery 4:73510c7fa316 23 led_3 = 1;
kolanery 4:73510c7fa316 24 led_4 = 1;
kolanery 15:151e59899221 25 }
kolanery 0:cb667de3a336 26 }
kolanery 0:cb667de3a336 27
kolanery 16:c26d8e007df5 28 void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
kolanery 16:c26d8e007df5 29 led_1 = _led_1;
kolanery 16:c26d8e007df5 30 led_2 = _led_2;
kolanery 16:c26d8e007df5 31 led_3 = _led_3;
kolanery 16:c26d8e007df5 32 led_4 = _led_4;
kolanery 16:c26d8e007df5 33 }
kolanery 16:c26d8e007df5 34
kolanery 0:cb667de3a336 35 int main() {
kolanery 4:73510c7fa316 36 DriveControl * driver = new DriveControl (START_POS, END_POS);
kolanery 14:a646667ac9ea 37 driver->resetEncoders();
kolanery 16:c26d8e007df5 38 int state, count = 0;
kolanery 14:a646667ac9ea 39 bool finished_traverse_one_cell = true;
kolanery 26:191ec0e78774 40 Mouse *my_mouse = new Mouse();
kolanery 26:191ec0e78774 41 flood_fill_timer.start();
kolanery 26:191ec0e78774 42 while(flood_fill_timer.read() < 50) {
kolanery 26:191ec0e78774 43 check_battery();
kolanery 26:191ec0e78774 44 wait_ms(1);
kolanery 26:191ec0e78774 45 if (finished_traverse_one_cell == true) {
kolanery 26:191ec0e78774 46 if (state_timeout_timer.read() > 10) {
kolanery 26:191ec0e78774 47 driver->stop();
kolanery 26:191ec0e78774 48 wait(0.5);
kolanery 26:191ec0e78774 49 driver->reverse();
kolanery 26:191ec0e78774 50 wait(0.5);
kolanery 26:191ec0e78774 51 driver->stop();
kolanery 26:191ec0e78774 52 driver->resetEncoders();
kolanery 26:191ec0e78774 53 state_timeout_timer.reset();
kolanery 26:191ec0e78774 54 state_timeout_timer.start();
kolanery 26:191ec0e78774 55 }
kolanery 26:191ec0e78774 56 state = my_mouse->solve_maze();
kolanery 26:191ec0e78774 57 if(state == RIGHT){
kolanery 26:191ec0e78774 58 flash_led(1,1,0,0);
kolanery 26:191ec0e78774 59 wait(0.25);
kolanery 26:191ec0e78774 60 driver->resetEncoders();
kolanery 26:191ec0e78774 61 }
kolanery 26:191ec0e78774 62 else if(state == LEFT){
kolanery 26:191ec0e78774 63 flash_led(1,0,1,0);
kolanery 26:191ec0e78774 64 wait(0.25);
kolanery 26:191ec0e78774 65 driver->resetEncoders();
kolanery 26:191ec0e78774 66 }
kolanery 26:191ec0e78774 67 else if(state == UTURN){
kolanery 26:191ec0e78774 68 flash_led(1,1,1,0);
kolanery 26:191ec0e78774 69 wait(0.25);
kolanery 26:191ec0e78774 70 driver->resetEncoders();
kolanery 26:191ec0e78774 71 count = 0;
kolanery 26:191ec0e78774 72 }
kolanery 26:191ec0e78774 73 finished_traverse_one_cell = false;
kolanery 26:191ec0e78774 74 }
kolanery 26:191ec0e78774 75 state_timeout_timer.start();
kolanery 26:191ec0e78774 76 if (state == STRAIGHT) {
kolanery 26:191ec0e78774 77 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 26:191ec0e78774 78 driver->drive_forward();
kolanery 26:191ec0e78774 79 flash_led(1, 0, 0, 0);
kolanery 26:191ec0e78774 80 }
kolanery 26:191ec0e78774 81 else {
kolanery 26:191ec0e78774 82 driver->clear_pid();
kolanery 26:191ec0e78774 83 driver->stop();
kolanery 26:191ec0e78774 84 driver->resetEncoders();
kolanery 26:191ec0e78774 85 flash_led (0, 0, 0, 0);
kolanery 26:191ec0e78774 86 finished_traverse_one_cell = true;
kolanery 26:191ec0e78774 87 continue;
kolanery 26:191ec0e78774 88 }
kolanery 26:191ec0e78774 89 }
kolanery 26:191ec0e78774 90 if (state == RIGHT) {
kolanery 26:191ec0e78774 91 if (!driver->should_finish_turn_right()) {
kolanery 26:191ec0e78774 92 driver->turn_right();
kolanery 26:191ec0e78774 93 flash_led (0, 1, 0, 0);
kolanery 26:191ec0e78774 94 }
kolanery 26:191ec0e78774 95 else {
kolanery 26:191ec0e78774 96 driver->stop();
kolanery 26:191ec0e78774 97 flash_led (0, 0, 0, 0);
kolanery 26:191ec0e78774 98 //finished_traverse_one_cell = true;
kolanery 26:191ec0e78774 99 state = STRAIGHT;
kolanery 26:191ec0e78774 100 driver->resetEncoders();
kolanery 26:191ec0e78774 101 driver->clear_pid();
kolanery 26:191ec0e78774 102 // wait(0.25);
kolanery 26:191ec0e78774 103 continue;
kolanery 26:191ec0e78774 104 }
kolanery 26:191ec0e78774 105 }
kolanery 26:191ec0e78774 106 if (state == LEFT) {
kolanery 26:191ec0e78774 107 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 108 driver->turn_left();
kolanery 26:191ec0e78774 109 flash_led (0, 0, 1, 0);
kolanery 26:191ec0e78774 110 }
kolanery 26:191ec0e78774 111 else {
kolanery 26:191ec0e78774 112 driver->stop();
kolanery 26:191ec0e78774 113 flash_led (0, 0, 0, 0);
kolanery 26:191ec0e78774 114 state = STRAIGHT;
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
kolanery 26:191ec0e78774 121 if (state == UTURN){
kolanery 26:191ec0e78774 122 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 123 driver->turn_left();
kolanery 26:191ec0e78774 124 flash_led (0, 1, 1, 0);
kolanery 26:191ec0e78774 125 }
kolanery 26:191ec0e78774 126 else {
kolanery 26:191ec0e78774 127 driver->stop();
kolanery 26:191ec0e78774 128 driver->resetEncoders();
kolanery 26:191ec0e78774 129 flash_led (0, 0, 0, 0);
kolanery 26:191ec0e78774 130 count == 1 ? finished_traverse_one_cell = true: count++;
kolanery 26:191ec0e78774 131 continue;
kolanery 26:191ec0e78774 132 }
kolanery 26:191ec0e78774 133 }
kolanery 26:191ec0e78774 134 }
kolanery 26:191ec0e78774 135 flood_fill_timer.stop();
kolanery 26:191ec0e78774 136 state = count = 0;
kolanery 26:191ec0e78774 137 finished_traverse_one_cell = true;
ryan_whr 25:7155cb993870 138
kolanery 26:191ec0e78774 139 flash_led(1,1,1,1);
kolanery 16:c26d8e007df5 140 while(CONTROL) {
kolanery 26:191ec0e78774 141 check_battery();
kolanery 26:191ec0e78774 142 wait_ms(1);
ryan_whr 22:1a02d66218e4 143 if (finished_traverse_one_cell == true) {
kolanery 26:191ec0e78774 144 if (!driver->has_right_wall() && !driver->has_left_wall()) {
ryan_whr 22:1a02d66218e4 145 wait(1);
kolanery 26:191ec0e78774 146 if ((rand() % 3) == 0) {
ryan_whr 22:1a02d66218e4 147 state = LEFT;
ryan_whr 22:1a02d66218e4 148 driver->resetEncoders();
ryan_whr 22:1a02d66218e4 149 }
ryan_whr 22:1a02d66218e4 150 else {
ryan_whr 22:1a02d66218e4 151 state = RIGHT;
ryan_whr 22:1a02d66218e4 152 driver->resetEncoders();
ryan_whr 22:1a02d66218e4 153 }
ryan_whr 22:1a02d66218e4 154
ryan_whr 22:1a02d66218e4 155 }
ryan_whr 25:7155cb993870 156 else if (!driver->has_right_wall()) {
ryan_whr 22:1a02d66218e4 157 wait(0.25);
kolanery 16:c26d8e007df5 158 state = RIGHT;
kolanery 19:931f8257fb74 159 driver->resetEncoders();
kolanery 15:151e59899221 160 }
kolanery 15:151e59899221 161 else if (!driver->has_left_wall()) {
ryan_whr 22:1a02d66218e4 162 wait(0.25);
kolanery 15:151e59899221 163 state = LEFT;
kolanery 19:931f8257fb74 164 driver->resetEncoders();
kolanery 15:151e59899221 165 }
kolanery 16:c26d8e007df5 166 else if(!driver->has_front_wall()){
kolanery 15:151e59899221 167 state = STRAIGHT;
kolanery 15:151e59899221 168 driver->resetEncoders();
kolanery 14:a646667ac9ea 169 }
kolanery 16:c26d8e007df5 170 else{
ryan_whr 22:1a02d66218e4 171 wait(0.25);
kolanery 16:c26d8e007df5 172 state = UTURN;
kolanery 16:c26d8e007df5 173 driver->resetEncoders();
kolanery 16:c26d8e007df5 174 count = 0;
kolanery 16:c26d8e007df5 175 }
kolanery 16:c26d8e007df5 176 finished_traverse_one_cell = false;
kolanery 7:7215adbae3da 177 }
kolanery 16:c26d8e007df5 178
kolanery 14:a646667ac9ea 179 if (state == STRAIGHT) {
kolanery 17:043ed1d0196f 180 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 16:c26d8e007df5 181 driver->drive_forward();
kolanery 16:c26d8e007df5 182 flash_led(1, 0, 0, 0);
kolanery 16:c26d8e007df5 183 }
kolanery 16:c26d8e007df5 184 else {
kolanery 16:c26d8e007df5 185 driver->clear_pid();
kolanery 16:c26d8e007df5 186 driver->stop();
kolanery 16:c26d8e007df5 187 driver->resetEncoders();
kolanery 16:c26d8e007df5 188 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 189 finished_traverse_one_cell = true;
kolanery 16:c26d8e007df5 190 continue;
kolanery 14:a646667ac9ea 191 }
kolanery 7:7215adbae3da 192 }
kolanery 14:a646667ac9ea 193 if (state == RIGHT) {
kolanery 14:a646667ac9ea 194 if (!driver->should_finish_turn_right()) {
kolanery 14:a646667ac9ea 195 driver->turn_right();
kolanery 16:c26d8e007df5 196 flash_led (0, 1, 0, 0);
kolanery 14:a646667ac9ea 197 }
kolanery 14:a646667ac9ea 198 else {
kolanery 14:a646667ac9ea 199 driver->stop();
kolanery 16:c26d8e007df5 200 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 201 //finished_traverse_one_cell = true;
kolanery 16:c26d8e007df5 202 state = STRAIGHT;
kolanery 16:c26d8e007df5 203 driver->resetEncoders();
kolanery 16:c26d8e007df5 204 driver->clear_pid();
ryan_whr 22:1a02d66218e4 205 // wait(0.25);
kolanery 14:a646667ac9ea 206 continue;
kolanery 14:a646667ac9ea 207 }
kolanery 14:a646667ac9ea 208 }
kolanery 14:a646667ac9ea 209 if (state == LEFT) {
kolanery 14:a646667ac9ea 210 if (!driver->should_finish_turn_left()) {
kolanery 14:a646667ac9ea 211 driver->turn_left();
kolanery 16:c26d8e007df5 212 flash_led (0, 0, 1, 0);
kolanery 16:c26d8e007df5 213 }
kolanery 16:c26d8e007df5 214 else {
kolanery 16:c26d8e007df5 215 driver->stop();
kolanery 16:c26d8e007df5 216 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 217 state = STRAIGHT;
kolanery 16:c26d8e007df5 218 driver->resetEncoders();
kolanery 16:c26d8e007df5 219 driver->clear_pid();
kolanery 16:c26d8e007df5 220 continue;
kolanery 16:c26d8e007df5 221 }
kolanery 16:c26d8e007df5 222 }
kolanery 16:c26d8e007df5 223
kolanery 16:c26d8e007df5 224 if (state == UTURN){
kolanery 16:c26d8e007df5 225 if (!driver->should_finish_turn_left()) {
kolanery 16:c26d8e007df5 226 driver->turn_left();
kolanery 16:c26d8e007df5 227 flash_led (0, 1, 1, 0);
kolanery 14:a646667ac9ea 228 }
kolanery 14:a646667ac9ea 229 else {
kolanery 14:a646667ac9ea 230 driver->stop();
kolanery 16:c26d8e007df5 231 driver->resetEncoders();
kolanery 16:c26d8e007df5 232 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 233 count == 1 ? finished_traverse_one_cell = true: count++;
kolanery 16:c26d8e007df5 234 continue;
kolanery 16:c26d8e007df5 235 }
kolanery 20:b18eed69ee32 236 }
kolanery 16:c26d8e007df5 237 }
kolanery 26:191ec0e78774 238 }