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@18:85196734207e, 2017-05-20 (annotated)

Committer:
kolanery
Date:
Sat May 20 22:37:18 2017 +0000
Revision:
18:85196734207e
Parent:
17:043ed1d0196f 
restructure project

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