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@17:043ed1d0196f, 2017-05-20 (annotated)

Committer:
kolanery
Date:
Sat May 20 21:34:06 2017 +0000
Revision:
17:043ed1d0196f
Parent:
16:c26d8e007df5
Child:
18:85196734207e
Child:
19:931f8257fb74
change front wall thres checking

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