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@28:600932142201, 2017-05-27 (annotated)

Committer:
kolanery
Date:
Sat May 27 07:21:02 2017 +0000
Revision:
28:600932142201
Parent:
27:b980fce784ea
Child:
29:2224bc8bb49d
Update PID and fixed the algorithm bug.

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
szh66 27:b980fce784ea 13 const unsigned char 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 28:600932142201 16 Mouse * my_mouse;
kolanery 28:600932142201 17 unsigned char state;
kolanery 28:600932142201 18 int move = 0;
kolanery 2:619b02232144 19
kolanery 16:c26d8e007df5 20 void check_battery () {
szh66 27:b980fce784ea 21 // pc.baud(9600);
szh66 27:b980fce784ea 22 // using the serial functions will have an impact on timing.
szh66 27:b980fce784ea 23 // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
szh66 27:b980fce784ea 24 // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
kolanery 15:151e59899221 25 if (battery.read() < 0.70f){
szh66 27:b980fce784ea 26 // flash led
kolanery 4:73510c7fa316 27 led_1 = 1;
kolanery 4:73510c7fa316 28 led_2 = 1;
kolanery 4:73510c7fa316 29 led_3 = 1;
kolanery 4:73510c7fa316 30 led_4 = 1;
kolanery 15:151e59899221 31 }
kolanery 0:cb667de3a336 32 }
kolanery 0:cb667de3a336 33
kolanery 16:c26d8e007df5 34 void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
kolanery 16:c26d8e007df5 35 led_1 = _led_1;
kolanery 16:c26d8e007df5 36 led_2 = _led_2;
kolanery 16:c26d8e007df5 37 led_3 = _led_3;
kolanery 16:c26d8e007df5 38 led_4 = _led_4;
kolanery 16:c26d8e007df5 39 }
kolanery 16:c26d8e007df5 40
kolanery 28:600932142201 41 void print_debug_message(){
kolanery 28:600932142201 42 serial.printf("Next mouse position <%u,%u> \r\n", my_mouse->mouse_x, my_mouse->mouse_y);
kolanery 28:600932142201 43 serial.printf("next state: %u \r\n", state);
kolanery 28:600932142201 44 serial.printf("previous mouse direction <%d> \r\n", my_mouse->get_prev_direction());
kolanery 28:600932142201 45 serial.printf("next cell direction <%d> \r\n", my_mouse->get_next_cell_direction());
kolanery 28:600932142201 46
kolanery 28:600932142201 47 move++;
kolanery 28:600932142201 48 }
kolanery 0:cb667de3a336 49 int main() {
kolanery 4:73510c7fa316 50 DriveControl * driver = new DriveControl (START_POS, END_POS);
kolanery 28:600932142201 51 my_mouse = new Mouse(driver);
kolanery 14:a646667ac9ea 52 driver->resetEncoders();
kolanery 28:600932142201 53
szh66 27:b980fce784ea 54 int count = 0;
kolanery 14:a646667ac9ea 55 bool finished_traverse_one_cell = true;
szh66 27:b980fce784ea 56
szh66 27:b980fce784ea 57
kolanery 28:600932142201 58 wait(2);
szh66 27:b980fce784ea 59 while(CONTROL) {
szh66 27:b980fce784ea 60 check_battery();
szh66 27:b980fce784ea 61 wait_ms(1);
kolanery 28:600932142201 62 if (driver->right_wall_pid_debug()) {
kolanery 28:600932142201 63 flash_led(1,0,1,1);
kolanery 28:600932142201 64 }
szh66 27:b980fce784ea 65
szh66 27:b980fce784ea 66 //wait(2);
szh66 27:b980fce784ea 67 //driver->print_serial_ports();
szh66 27:b980fce784ea 68
kolanery 26:191ec0e78774 69 if (finished_traverse_one_cell == true) {
kolanery 28:600932142201 70
kolanery 26:191ec0e78774 71 state = my_mouse->solve_maze();
kolanery 28:600932142201 72 wait(1);
kolanery 28:600932142201 73 // print_debug_message();
kolanery 28:600932142201 74 // serial.printf("left wall: %d, right wall:%d, front wall: %d \r\n", driver->has_left_wall(), driver->has_right_wall(), driver->has_front_wall());
kolanery 28:600932142201 75 // serial.printf("--- \r\n");
kolanery 28:600932142201 76 // wait(2);
kolanery 26:191ec0e78774 77 if(state == RIGHT){
szh66 27:b980fce784ea 78 flash_led(0,0,1,1);
kolanery 26:191ec0e78774 79 wait(0.25);
kolanery 26:191ec0e78774 80 driver->resetEncoders();
kolanery 26:191ec0e78774 81 }
kolanery 26:191ec0e78774 82 else if(state == LEFT){
szh66 27:b980fce784ea 83 flash_led(1,1,0,0);
kolanery 26:191ec0e78774 84 wait(0.25);
kolanery 26:191ec0e78774 85 driver->resetEncoders();
kolanery 26:191ec0e78774 86 }
kolanery 26:191ec0e78774 87 else if(state == UTURN){
kolanery 26:191ec0e78774 88 flash_led(1,1,1,0);
kolanery 26:191ec0e78774 89 wait(0.25);
kolanery 26:191ec0e78774 90 driver->resetEncoders();
kolanery 26:191ec0e78774 91 count = 0;
szh66 27:b980fce784ea 92 }else{
szh66 27:b980fce784ea 93 //TODO
szh66 27:b980fce784ea 94 //Add error handling
szh66 27:b980fce784ea 95 state = STRAIGHT;
kolanery 28:600932142201 96
szh66 27:b980fce784ea 97 flash_led(1, 0, 0, 0);
kolanery 26:191ec0e78774 98 }
szh66 27:b980fce784ea 99
kolanery 26:191ec0e78774 100 finished_traverse_one_cell = false;
kolanery 26:191ec0e78774 101 }
szh66 27:b980fce784ea 102
kolanery 26:191ec0e78774 103 if (state == STRAIGHT) {
kolanery 26:191ec0e78774 104 if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
kolanery 26:191ec0e78774 105 driver->drive_forward();
szh66 27:b980fce784ea 106
kolanery 26:191ec0e78774 107 }
kolanery 26:191ec0e78774 108 else {
kolanery 26:191ec0e78774 109 driver->clear_pid();
kolanery 26:191ec0e78774 110 driver->stop();
kolanery 28:600932142201 111 wait(0.25);
kolanery 26:191ec0e78774 112 driver->resetEncoders();
kolanery 26:191ec0e78774 113 finished_traverse_one_cell = true;
kolanery 26:191ec0e78774 114 continue;
kolanery 26:191ec0e78774 115 }
kolanery 26:191ec0e78774 116 }
kolanery 26:191ec0e78774 117 if (state == RIGHT) {
kolanery 26:191ec0e78774 118 if (!driver->should_finish_turn_right()) {
kolanery 26:191ec0e78774 119 driver->turn_right();
kolanery 26:191ec0e78774 120 }
kolanery 26:191ec0e78774 121 else {
kolanery 26:191ec0e78774 122 driver->stop();
kolanery 26:191ec0e78774 123 state = STRAIGHT;
kolanery 28:600932142201 124 wait(0.25);
kolanery 26:191ec0e78774 125 driver->resetEncoders();
kolanery 26:191ec0e78774 126 driver->clear_pid();
kolanery 26:191ec0e78774 127 // wait(0.25);
kolanery 26:191ec0e78774 128 continue;
kolanery 26:191ec0e78774 129 }
kolanery 26:191ec0e78774 130 }
kolanery 26:191ec0e78774 131 if (state == LEFT) {
kolanery 26:191ec0e78774 132 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 133 driver->turn_left();
kolanery 26:191ec0e78774 134 }
kolanery 26:191ec0e78774 135 else {
kolanery 26:191ec0e78774 136 driver->stop();
kolanery 26:191ec0e78774 137 state = STRAIGHT;
kolanery 28:600932142201 138 wait(0.25);
kolanery 26:191ec0e78774 139 driver->resetEncoders();
kolanery 26:191ec0e78774 140 driver->clear_pid();
szh66 27:b980fce784ea 141 //wait(0.25);
kolanery 26:191ec0e78774 142 continue;
kolanery 26:191ec0e78774 143 }
kolanery 26:191ec0e78774 144 }
kolanery 26:191ec0e78774 145
kolanery 26:191ec0e78774 146 if (state == UTURN){
kolanery 26:191ec0e78774 147 if (!driver->should_finish_turn_left()) {
kolanery 26:191ec0e78774 148 driver->turn_left();
kolanery 26:191ec0e78774 149 }
kolanery 26:191ec0e78774 150 else {
kolanery 26:191ec0e78774 151 driver->stop();
kolanery 26:191ec0e78774 152 driver->resetEncoders();
szh66 27:b980fce784ea 153 wait(0.25);
szh66 27:b980fce784ea 154 if(count == 1) {
szh66 27:b980fce784ea 155 state = STRAIGHT;
ryan_whr 22:1a02d66218e4 156 }
szh66 27:b980fce784ea 157 count ++;//
kolanery 16:c26d8e007df5 158 continue;
kolanery 16:c26d8e007df5 159 }
kolanery 20:b18eed69ee32 160 }
kolanery 16:c26d8e007df5 161 }
kolanery 26:191ec0e78774 162 }