--- /dev/null
+
+/*
+ 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 <http://www.gnu.org/licenses/>.
+ */
+
+#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(){
+ 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.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);}}
+
+void follow() {
+ Serial.println("following!");
+ delay(100); }
+
+void loop() {
+ digitalWrite(LED_BUILTIN, digitalRead(button_pin)); }
+
+/*
+enum Behavior {STAY_ON_TABLE, FOLLOW};
+Behavior behavior = STAY_ON_TABLE;
+
+enum Button_state {UP, DOWN};
+Button_state prior_button_state = UP;
+
+enum Button_state get_button_state() {
+ return
+ (digitalRead(button_pin) == LOW)
+ ? DOWN
+ : UP; }
+void loop_asdf() {
+ static Button_state button_state = getButtonState();
+
+ // if button was just pressed
+ if (prior_button_state == UP && 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; }
+ off(LED_BUILTIN); }
+
+ switch(behavior) {
+ case STAY_ON_TABLE: stay_on_table(); break;
+ case FOLLOW: follow(); break; }
+
+ prior_button_state = button_state; }
+*/