along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
-#include "bot_map.h"
+#include "bot_with_leds_map.h"
void on(int pin){
digitalWrite(pin, HIGH);}
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,
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);
off(right_motor_backward_pin);
off(right_trigger_pin);}
-void 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() {
+ 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;
+
+ const int left_ping_time =
+ ping(left_trigger_pin, left_echo_pin);
+ const int right_ping_time =
+ ping(right_trigger_pin, right_echo_pin);
+
+ 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;
+ 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(){
+ Serial.print("stay on table ");
+ static unsigned long start_backing_time = 0;
+ static unsigned long start_turning_time = 0;
+
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.println(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.println("backing up");
- backup(backup_time);
- Serial.println("turning around");
- turn_around(turn_around_time);}
- else{
- Serial.println("going");
- go(left_speed, right_speed);}}
+ switch(stay_on_table_state) {
+ case Stay_on_table_state::going: going(); 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() {
- Serial.println("following!");
- delay(100); }
+ int left_speed;
+ int right_speed;
-void loop() {
- digitalWrite(LED_BUILTIN, digitalRead(button_pin)); }
+ // you'll need to adjust these based on your sonar sensor's behavior
+ int desired_right_ping_time = 800;
+ int desired_left_ping_time = 800;
-/*
-enum Behavior {STAY_ON_TABLE, FOLLOW};
-Behavior behavior = STAY_ON_TABLE;
+ unsigned int left_ping_time = ping(left_trigger_pin, left_echo_pin);
+ unsigned int right_ping_time = ping(right_trigger_pin, right_echo_pin);
-enum Button_state {UP, DOWN};
-Button_state prior_button_state = UP;
+ left_speed = left_ping_time - desired_left_ping_time;
+ right_speed = right_ping_time - desired_right_ping_time;
-enum Button_state get_button_state() {
- return
- (digitalRead(button_pin) == LOW)
- ? DOWN
- : UP; }
-void loop_asdf() {
- static Button_state button_state = getButtonState();
+ Serial.print(", left: ping = ");
+ Serial.print(left_ping_time);
+ Serial.print(" speed = ");
+ Serial.print(left_speed);
+ Serial.print(" right: ping = ");
+ Serial.print(right_ping_time);
+ Serial.print(" speed = ");
+ Serial.print(right_speed);
+
+ go(left_speed, right_speed); }
+
+enum class Behavior {stay_on_table, follow};
+Behavior behavior = Behavior::stay_on_table;
+
+enum class Button_state {up, down};
+
+Button_state prior_button_state = Button_state::up;
+
+void loop() {
+ static int count = 0;
+ Serial.print(count);
+ Serial.print(" ");
+ count++;
+
+ Button_state button_state =
+ (digitalRead(button_pin) == HIGH)
+ ? Button_state::up
+ : Button_state::down;
+ switch(button_state) {
+ case Button_state::up: Serial.print("up "); break;
+ case Button_state::down: Serial.print("down "); break; }
// if button was just pressed
- if (prior_button_state == UP && button_state == DOWN) {
+ if (prior_button_state == Button_state::up
+ && button_state == Button_state::down) {
// indicate button press recognized
on(LED_BUILTIN);
// turn off motors, to allow robot to be set down
go(0, 0);
delay(1000);
switch(behavior) {
- case STAY_ON_TABLE: behavior = FOLLOW; break;
- case FOLLOW: behavior = STAY_ON_TABLE; break; }
+ case Behavior::stay_on_table: behavior = Behavior::follow; break;
+ case Behavior::follow: behavior = Behavior::stay_on_table; break; }
off(LED_BUILTIN); }
switch(behavior) {
- case STAY_ON_TABLE: stay_on_table(); break;
- case FOLLOW: follow(); break; }
+ case Behavior::stay_on_table: stay_on_table(); break;
+ case Behavior:: follow: follow(); break; }
+
+ prior_button_state = button_state;
- prior_button_state = button_state; }
-*/
+ Serial.println(); }
projects / challenge-bot / blobdiff