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@25:7155cb993870, 2017-05-21 (annotated)

Committer:
ryan_whr
Date:
Sun May 21 06:34:21 2017 +0000
Revision:
25:7155cb993870
Parent:
24:249505c7c080
Child:
26:191ec0e78774
milestone

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 2:619b02232144 16
kolanery 16:c26d8e007df5 17 void check_battery () {
szh66 6:1bcfda49e146 18 // pc.baud(9600);
kolanery 4:73510c7fa316 19 // using the serial functions will have an impact on timing.
kolanery 15:151e59899221 20 // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
kolanery 4:73510c7fa316 21 // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
kolanery 15:151e59899221 22 if (battery.read() < 0.70f){
kolanery 0:cb667de3a336 23 // flash led
kolanery 4:73510c7fa316 24 led_1 = 1;
kolanery 4:73510c7fa316 25 led_2 = 1;
kolanery 4:73510c7fa316 26 led_3 = 1;
kolanery 4:73510c7fa316 27 led_4 = 1;
kolanery 15:151e59899221 28 }
kolanery 0:cb667de3a336 29 }
kolanery 0:cb667de3a336 30
kolanery 16:c26d8e007df5 31 void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
kolanery 16:c26d8e007df5 32 led_1 = _led_1;
kolanery 16:c26d8e007df5 33 led_2 = _led_2;
kolanery 16:c26d8e007df5 34 led_3 = _led_3;
kolanery 16:c26d8e007df5 35 led_4 = _led_4;
kolanery 16:c26d8e007df5 36 }
kolanery 16:c26d8e007df5 37
kolanery 0:cb667de3a336 38 int main() {
kolanery 4:73510c7fa316 39 DriveControl * driver = new DriveControl (START_POS, END_POS);
kolanery 14:a646667ac9ea 40 driver->resetEncoders();
kolanery 16:c26d8e007df5 41 int state, count = 0;
kolanery 14:a646667ac9ea 42 bool finished_traverse_one_cell = true;
ryan_whr 25:7155cb993870 43
ryan_whr 25:7155cb993870 44 Mouse *my_mouse = new Mouse();
ryan_whr 25:7155cb993870 45
kolanery 16:c26d8e007df5 46 //wait(1.5);
kolanery 16:c26d8e007df5 47 while(CONTROL) {
kolanery 16:c26d8e007df5 48 check_battery();
kolanery 15:151e59899221 49 wait_ms(1);
kolanery 16:c26d8e007df5 50
kolanery 16:c26d8e007df5 51 //wait(2);
kolanery 16:c26d8e007df5 52 //driver->print_serial_ports();
ryan_whr 22:1a02d66218e4 53
ryan_whr 22:1a02d66218e4 54
kolanery 16:c26d8e007df5 55
kolanery 16:c26d8e007df5 56
ryan_whr 22:1a02d66218e4 57 if (finished_traverse_one_cell == true) {
ryan_whr 22:1a02d66218e4 58
ryan_whr 23:953651133b29 59 /*if (!driver->has_right_wall() && !driver->has_left_wall()) {
ryan_whr 22:1a02d66218e4 60 wait(1);
ryan_whr 22:1a02d66218e4 61 if ((rand() % 4) == 0) {
ryan_whr 22:1a02d66218e4 62 state = LEFT;
ryan_whr 22:1a02d66218e4 63 driver->resetEncoders();
ryan_whr 22:1a02d66218e4 64 }
ryan_whr 22:1a02d66218e4 65 else {
ryan_whr 22:1a02d66218e4 66 state = RIGHT;
ryan_whr 22:1a02d66218e4 67 driver->resetEncoders();
ryan_whr 22:1a02d66218e4 68 }
ryan_whr 22:1a02d66218e4 69
ryan_whr 22:1a02d66218e4 70 }
ryan_whr 25:7155cb993870 71 else if (!driver->has_right_wall()) {
ryan_whr 22:1a02d66218e4 72 wait(0.25);
kolanery 16:c26d8e007df5 73 state = RIGHT;
kolanery 19:931f8257fb74 74 driver->resetEncoders();
kolanery 15:151e59899221 75 }
kolanery 15:151e59899221 76 else if (!driver->has_left_wall()) {
ryan_whr 22:1a02d66218e4 77 wait(0.25);
kolanery 15:151e59899221 78 state = LEFT;
kolanery 19:931f8257fb74 79 driver->resetEncoders();
kolanery 15:151e59899221 80 }
kolanery 16:c26d8e007df5 81 else if(!driver->has_front_wall()){
kolanery 15:151e59899221 82 state = STRAIGHT;
kolanery 15:151e59899221 83 driver->resetEncoders();
kolanery 14:a646667ac9ea 84 }
kolanery 16:c26d8e007df5 85 else{
ryan_whr 22:1a02d66218e4 86 wait(0.25);
kolanery 16:c26d8e007df5 87 state = UTURN;
kolanery 16:c26d8e007df5 88 driver->resetEncoders();
kolanery 16:c26d8e007df5 89 count = 0;
kolanery 16:c26d8e007df5 90 }
ryan_whr 25:7155cb993870 91 */
ryan_whr 25:7155cb993870 92
ryan_whr 25:7155cb993870 93 state = my_mouse->solve_maze();
ryan_whr 25:7155cb993870 94 if(state == RIGHT){
ryan_whr 25:7155cb993870 95 flash_led(1,1,0,0);
ryan_whr 25:7155cb993870 96 wait(0.25);
ryan_whr 25:7155cb993870 97 driver->resetEncoders();
ryan_whr 25:7155cb993870 98 }
ryan_whr 25:7155cb993870 99 else if(state == LEFT){
ryan_whr 25:7155cb993870 100 flash_led(1,0,1,0);
ryan_whr 25:7155cb993870 101 wait(0.25);
ryan_whr 25:7155cb993870 102 driver->resetEncoders();
ryan_whr 25:7155cb993870 103 }
ryan_whr 25:7155cb993870 104 else if(state == UTURN){
ryan_whr 25:7155cb993870 105 flash_led(1,1,1,0);
ryan_whr 25:7155cb993870 106 wait(0.25);
ryan_whr 25:7155cb993870 107 driver->resetEncoders();
ryan_whr 25:7155cb993870 108 count = 0;
ryan_whr 25:7155cb993870 109 }
ryan_whr 25:7155cb993870 110
kolanery 16:c26d8e007df5 111 finished_traverse_one_cell = false;
kolanery 7:7215adbae3da 112 }
kolanery 16:c26d8e007df5 113
kolanery 14:a646667ac9ea 114 if (state == STRAIGHT) {
kolanery 17:043ed1d0196f 115 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 16:c26d8e007df5 116 driver->drive_forward();
kolanery 16:c26d8e007df5 117 flash_led(1, 0, 0, 0);
kolanery 16:c26d8e007df5 118 }
kolanery 16:c26d8e007df5 119 else {
kolanery 16:c26d8e007df5 120 driver->clear_pid();
kolanery 16:c26d8e007df5 121 driver->stop();
kolanery 16:c26d8e007df5 122 driver->resetEncoders();
kolanery 16:c26d8e007df5 123 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 124 finished_traverse_one_cell = true;
kolanery 16:c26d8e007df5 125 continue;
kolanery 14:a646667ac9ea 126 }
kolanery 7:7215adbae3da 127 }
kolanery 14:a646667ac9ea 128 if (state == RIGHT) {
kolanery 14:a646667ac9ea 129 if (!driver->should_finish_turn_right()) {
kolanery 14:a646667ac9ea 130 driver->turn_right();
kolanery 16:c26d8e007df5 131 flash_led (0, 1, 0, 0);
kolanery 14:a646667ac9ea 132 }
kolanery 14:a646667ac9ea 133 else {
kolanery 14:a646667ac9ea 134 driver->stop();
kolanery 16:c26d8e007df5 135 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 136 //finished_traverse_one_cell = true;
kolanery 16:c26d8e007df5 137 state = STRAIGHT;
kolanery 16:c26d8e007df5 138 driver->resetEncoders();
kolanery 16:c26d8e007df5 139 driver->clear_pid();
ryan_whr 22:1a02d66218e4 140 // wait(0.25);
kolanery 14:a646667ac9ea 141 continue;
kolanery 14:a646667ac9ea 142 }
kolanery 14:a646667ac9ea 143 }
kolanery 14:a646667ac9ea 144 if (state == LEFT) {
kolanery 14:a646667ac9ea 145 if (!driver->should_finish_turn_left()) {
kolanery 14:a646667ac9ea 146 driver->turn_left();
kolanery 16:c26d8e007df5 147 flash_led (0, 0, 1, 0);
kolanery 16:c26d8e007df5 148 }
kolanery 16:c26d8e007df5 149 else {
kolanery 16:c26d8e007df5 150 driver->stop();
kolanery 16:c26d8e007df5 151 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 152 //finished_traverse_one_cell = true;
kolanery 16:c26d8e007df5 153 state = STRAIGHT;
kolanery 16:c26d8e007df5 154 driver->resetEncoders();
kolanery 16:c26d8e007df5 155 driver->clear_pid();
ryan_whr 22:1a02d66218e4 156 //wait(0.25);
kolanery 16:c26d8e007df5 157 continue;
kolanery 16:c26d8e007df5 158 }
kolanery 16:c26d8e007df5 159 }
kolanery 16:c26d8e007df5 160
kolanery 16:c26d8e007df5 161 if (state == UTURN){
kolanery 16:c26d8e007df5 162 if (!driver->should_finish_turn_left()) {
kolanery 16:c26d8e007df5 163 driver->turn_left();
kolanery 16:c26d8e007df5 164 flash_led (0, 1, 1, 0);
kolanery 14:a646667ac9ea 165 }
kolanery 14:a646667ac9ea 166 else {
kolanery 14:a646667ac9ea 167 driver->stop();
kolanery 16:c26d8e007df5 168 driver->resetEncoders();
ryan_whr 22:1a02d66218e4 169 //wait(0.25);
kolanery 16:c26d8e007df5 170 flash_led (0, 0, 0, 0);
kolanery 16:c26d8e007df5 171 count == 1 ? finished_traverse_one_cell = true: count++;
kolanery 16:c26d8e007df5 172 continue;
kolanery 16:c26d8e007df5 173 }
kolanery 20:b18eed69ee32 174 }
kolanery 16:c26d8e007df5 175 }
kolanery 16:c26d8e007df5 176 }
kolanery 16:c26d8e007df5 177
kolanery 16:c26d8e007df5 178
kolanery 16:c26d8e007df5 179 /*
kolanery 16:c26d8e007df5 180
kolanery 16:c26d8e007df5 181 if (!driver->has_front_wall()) {
kolanery 16:c26d8e007df5 182 if (!driver->should_finish_drive_forward()) {
kolanery 16:c26d8e007df5 183 driver->drive_forward();
kolanery 16:c26d8e007df5 184 flash_led(0, 1, 1, 0);
kolanery 16:c26d8e007df5 185 }
kolanery 16:c26d8e007df5 186 else {
kolanery 16:c26d8e007df5 187 driver->stop();
kolanery 16:c26d8e007df5 188 wait(1);
kolanery 16:c26d8e007df5 189 flash_led (0,0,0,0);
kolanery 16:c26d8e007df5 190 driver->resetEncoders();
kolanery 14:a646667ac9ea 191 finished_traverse_one_cell = true;
kolanery 14:a646667ac9ea 192 continue;
kolanery 14:a646667ac9ea 193 }
kolanery 16:c26d8e007df5 194 }
kolanery 16:c26d8e007df5 195 else {
kolanery 16:c26d8e007df5 196 driver->stop();
kolanery 16:c26d8e007df5 197 driver->resetEncoders();
kolanery 16:c26d8e007df5 198 flash_led (1, 1, 1, 0);
kolanery 16:c26d8e007df5 199 }
kolanery 16:c26d8e007df5 200
kolanery 16:c26d8e007df5 201
kolanery 16:c26d8e007df5 202
kolanery 16:c26d8e007df5 203
kolanery 16:c26d8e007df5 204 */