X-Git-Url: http://challenge-bot.com/repos/?p=challenge-bot;a=blobdiff_plain;f=build-stages%2Fi_chooser%2Fi_chooser.ino;h=d43591fa56618d9ad9cdbefd2f98eb03bc5a3de6;hp=b90574a3e776c82dc7c6d1e800b5531cc6c8dc14;hb=f9290e379cc65d97e76bc5650066bdbe27ee4883;hpb=3cb3ea0ba5ea81fb679adac1c9acffe53cedeeb5 diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino index b90574a..d43591f 100644 --- a/build-stages/i_chooser/i_chooser.ino +++ b/build-stages/i_chooser/i_chooser.ino @@ -14,7 +14,7 @@ along with this program. If not, see . */ -#include "bot_map.h" +#include "bot_with_leds_map.h" void on(int pin){ digitalWrite(pin, HIGH);} @@ -41,7 +41,7 @@ void set_motor(int speed_pin, speed = 255;} analogWrite(speed_pin, speed);} -void go(int left_motor_speed, int right_motor_speed){ +void go(const int left_motor_speed, const int right_motor_speed){ set_motor(left_motor_speed_pin, left_motor_forward_pin, left_motor_backward_pin, @@ -69,14 +69,6 @@ int ping(int trigger, int echo){ delay(50); return ping_time;} -void backup(int backup_time){ - go(-250, -250); - delay(backup_time);} - -void turn_around(int turn_around_time){ - go(-250, 250); - delay(turn_around_time);} - void setup(){ Serial.begin(9600); @@ -84,6 +76,9 @@ void setup(){ pinMode(button_pin, INPUT_PULLUP); + pinMode(left_led_pin, OUTPUT); + pinMode(right_led_pin, OUTPUT); + pinMode(right_motor_speed_pin, OUTPUT); pinMode(right_motor_forward_pin, OUTPUT); pinMode(right_motor_backward_pin, OUTPUT); @@ -108,71 +103,103 @@ void setup(){ off(right_motor_backward_pin); off(right_trigger_pin);} -void stay_on_table(){ - Serial.print("stay on table"); - int left_speed; - int right_speed; +enum class Stay_on_table_state { + going, start_backing, backing, start_turning, turning }; +static Stay_on_table_state stay_on_table_state = + Stay_on_table_state::going; +void going(const unsigned int left_ping_time, + const unsigned int right_ping_time) { + Serial.print("going "); int forward_speed = 250; int stop_speed = 0; + int left_speed; + int right_speed; + // adjust this number as necessary for your robot. // it represents how far the table is from your sonar sensor. // larger values mean larger distance. default is 800 - int right_max_ping_time_over_table = 800; - int left_max_ping_time_over_table = 800; - int backup_time = 500; + const int right_max_ping_time_over_table = 800; + const int left_max_ping_time_over_table = 800; + + if (left_ping_time <= left_max_ping_time_over_table + || right_ping_time <= right_max_ping_time_over_table) { + if(left_ping_time <= left_max_ping_time_over_table) { + left_speed = forward_speed; } + else { + left_speed = stop_speed; } + if(right_ping_time <= right_max_ping_time_over_table) { + right_speed = forward_speed; } + else { + right_speed = stop_speed; } } + else { // both ping times were above max acceptable ping time + left_speed = right_speed = 0; + stay_on_table_state = Stay_on_table_state::start_backing; } + + go(left_speed, right_speed); } + +void backing(unsigned long start_backing) { + Serial.print("backing "); + static const unsigned int allowed_backup_duration = 500; + unsigned long now = millis(); + unsigned long backup_duration = now - start_backing; + + go(-250, -250); + + if(backup_duration > allowed_backup_duration) { + stay_on_table_state = Stay_on_table_state::start_turning; } } + +void turning(unsigned long start_turning_time) { + Serial.print("turning "); // the exact amount of time for turning around might need // twerking for your robot. the default value is 3200 - int turn_around_time = 2500; - - int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); - int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); - - /* int left_led_value = (int)(actual_left_ping_time / 16.0); */ - /* int right_led_value = (int)(actual_right_ping_time / 16.0); */ - - /* Serial.print("left led value = "); */ - /* Serial.print(left_led_value); */ - /* Serial.print(", right led value = "); */ - /* Serial.print(right_led_value); */ - - /* analogWrite(left_led_pin, left_led_value); */ - /* analogWrite(right_led_pin, right_led_value); */ - - /* Serial.print("left ping = "); */ - /* Serial.print(actual_left_ping_time); */ - /* Serial.print("\tright_ping = "); */ - /* Serial.print(actual_right_ping_time); */ - - // if the left sonar senses a table, keep driving left side forward, - // otherwise, stop left side - if(actual_left_ping_time < left_max_ping_time_over_table){ - left_speed = forward_speed;} - else{ - left_speed = stop_speed;} - // if the right sonar senses a table, keep driving right side forward, - // otherwise, stop right side - if(actual_right_ping_time < right_max_ping_time_over_table){ - right_speed = forward_speed;} - else{ - right_speed = stop_speed;} - - // if both sonars detect being off the table, backup and turn around - // otherwise, go the correct speed for each wheel - if(actual_left_ping_time >= left_max_ping_time_over_table - && actual_right_ping_time >= right_max_ping_time_over_table){ - /* Serial.print("backing up"); */ - backup(backup_time); - /* Serial.print("turning around"); */ - turn_around(turn_around_time);} - else{ - /* Serial.print("going"); */ - go(left_speed, right_speed);}} - -void follow() { - Serial.print("following!"); - delay(100); } + static const unsigned int allowed_turning_duration = 2500; + unsigned long now = millis(); + unsigned long turning_duration = now - start_turning_time; + + go(-250, 250); + + if(turning_duration > allowed_turning_duration) { + stay_on_table_state = Stay_on_table_state::going; } } + +void stay_on_table(const unsigned int left_ping_time, + const unsigned int right_ping_time){ + Serial.print("stay on table "); + static unsigned long start_backing_time = 0; + static unsigned long start_turning_time = 0; + + + switch(stay_on_table_state) { + case Stay_on_table_state::going: + going(left_ping_time, right_ping_time); + break; + case Stay_on_table_state::start_backing: + Serial.print("start backing "); + start_backing_time = millis(); + stay_on_table_state = Stay_on_table_state::backing; + case Stay_on_table_state::backing: + backing(start_backing_time); + break; + case Stay_on_table_state::start_turning: + Serial.print("start turning "); + start_turning_time = millis(); + stay_on_table_state = Stay_on_table_state::turning; + case Stay_on_table_state::turning: + turning(start_turning_time); + break; } } + +void follow(const unsigned int left_ping_time, + const unsigned int right_ping_time) { + // you'll need to adjust these based on your sonar sensor's behavior + const unsigned int desired_right_ping_time = 800; + const unsigned int desired_left_ping_time = 800; + + const int + left_speed = left_ping_time - desired_left_ping_time, + right_speed = right_ping_time - desired_right_ping_time; + + go(left_speed, right_speed); } enum class Behavior {stay_on_table, follow}; Behavior behavior = Behavior::stay_on_table; @@ -187,6 +214,15 @@ void loop() { Serial.print(" "); count++; + const unsigned int + left_ping_time = ping(left_trigger_pin, left_echo_pin), + right_ping_time = ping(right_trigger_pin, right_echo_pin); + const unsigned int + left_led_value = map(left_ping_time, 0, 3000, 0, 255), + right_led_value = map(right_ping_time, 0, 3000, 0, 255); + analogWrite(left_led_pin, left_led_value); + analogWrite(right_led_pin, right_led_value); + Button_state button_state = (digitalRead(button_pin) == HIGH) ? Button_state::up @@ -209,8 +245,12 @@ void loop() { off(LED_BUILTIN); } switch(behavior) { - case Behavior::stay_on_table: stay_on_table(); break; - case Behavior:: follow: follow(); break; } + case Behavior::stay_on_table: + stay_on_table(left_ping_time, right_ping_time); + break; + case Behavior:: follow: + follow(left_ping_time, right_ping_time); + break; } prior_button_state = button_state;