/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU Affero General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Affero General Public License for more details. You should have received a copy of the GNU Affero General Public License along with this program. If not, see . */ #include "bot_map.h" void on(int pin){ digitalWrite(pin, HIGH);} void off(int pin){ digitalWrite(pin, LOW);} void set_motor(int speed_pin, int forward_pin, int backward_pin, int speed){ if(speed > 0){ off(backward_pin); on(forward_pin);} else if(speed < 0){ off(forward_pin); on(backward_pin); speed = -speed;} else{ // speed is 0 off(forward_pin); off(backward_pin);} // since speed has been set positive, no need to check if speed < -255. if(speed > 255){ speed = 255;} analogWrite(speed_pin, speed);} void go(int left_motor_speed, int right_motor_speed){ set_motor(left_motor_speed_pin, left_motor_forward_pin, left_motor_backward_pin, left_motor_speed); set_motor(right_motor_speed_pin, right_motor_forward_pin, right_motor_backward_pin, right_motor_speed);} int ping(int trigger, int echo){ int ping_time = 0; // turn off trigger off(trigger); delayMicroseconds(2); // turn on the trigger and leave it on long enough for the // sonar sensor to notice on(trigger); delayMicroseconds(10); off(trigger); ping_time = pulseIn(echo, HIGH); if(ping_time <= 0){ ping_time = 3000;} // sonar needs some time to recover before pinging again, // so make sure it gets enough sleep right here. 50 milliseconds 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); pinMode(LED_BUILTIN, OUTPUT); pinMode(button_pin, INPUT_PULLUP); pinMode(right_motor_speed_pin, OUTPUT); pinMode(right_motor_forward_pin, OUTPUT); pinMode(right_motor_backward_pin, OUTPUT); pinMode(right_echo_pin, INPUT); pinMode(right_trigger_pin, OUTPUT); pinMode(left_motor_speed_pin, OUTPUT); pinMode(left_motor_forward_pin, OUTPUT); pinMode(left_motor_backward_pin, OUTPUT); pinMode(left_echo_pin, INPUT); pinMode(left_trigger_pin, OUTPUT); off(left_motor_speed_pin); off(left_motor_forward_pin); off(left_motor_backward_pin); off(left_trigger_pin); off(right_motor_speed_pin); off(right_motor_forward_pin); off(right_motor_backward_pin); off(right_trigger_pin);} void stay_on_table(){ Serial.print("stay on table"); int left_speed; int right_speed; int forward_speed = 250; int stop_speed = 0; // 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; // 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); } 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 == 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 Behavior::stay_on_table: behavior = Behavior::follow; break; case Behavior::follow: behavior = Behavior::stay_on_table; break; } off(LED_BUILTIN); } switch(behavior) { case Behavior::stay_on_table: stay_on_table(); break; case Behavior:: follow: follow(); break; } prior_button_state = button_state; Serial.println(); }