follow while on table

diff --git a/build-stages/i_follow_on_table/i_follow_on_table.ino b/build-stages/i_follow_on_table/i_follow_on_table.ino
new file mode 100644 (file)
index 0000000..1330633
--- /dev/null
+++ b/build-stages/i_follow_on_table/i_follow_on_table.ino
@@ -0,0 +1,182 @@
+/*
+ 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/>.
+ */
+
+// define which pins are connected to which components
+int right_motor_speed_pin = 3;
+int right_motor_forward_pin = 4;
+int right_motor_backward_pin = 5;
+
+int follow_right_echo_pin = 6;
+int follow_right_trigger_pin = 7;
+
+int left_motor_speed_pin = 8;
+int left_motor_forward_pin = 9;
+int left_motor_backward_pin = 10;
+
+int follow_left_echo_pin = 11;
+int follow_left_trigger_pin = 12;
+
+int table_right_echo_pin = 13;
+int table_right_trigger_pin = 2;
+
+// you'll need to adjust these based on your sonar sensor's behavior
+int desired_right_follow_ping_time = 800;
+int desired_left_follow_ping_time = 800;
+
+int right_max_ping_time_over_table = 500;
+
+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(right_motor_speed_pin, OUTPUT);
+  pinMode(right_motor_forward_pin, OUTPUT);
+  pinMode(right_motor_backward_pin, OUTPUT);
+
+  pinMode(follow_right_echo_pin, INPUT);
+  pinMode(follow_right_trigger_pin, OUTPUT);
+
+  pinMode(table_right_echo_pin, INPUT);
+  pinMode(table_right_trigger_pin, OUTPUT);
+
+  pinMode(left_motor_speed_pin, OUTPUT);
+  pinMode(left_motor_forward_pin, OUTPUT);
+  pinMode(left_motor_backward_pin, OUTPUT);
+
+  pinMode(follow_left_echo_pin, INPUT);
+  pinMode(follow_left_trigger_pin, OUTPUT);
+
+  off(left_motor_speed_pin);
+  off(left_motor_forward_pin);
+  off(left_motor_backward_pin);
+  off(follow_left_trigger_pin);
+
+  off(right_motor_speed_pin);
+  off(right_motor_forward_pin);
+  off(right_motor_backward_pin);
+  off(follow_right_trigger_pin);}
+
+void loop(){
+  int wall_right_min_ping_time = desired_right_follow_ping_time - 50;
+  int wall_right_max_ping_time = desired_right_follow_ping_time + 50;
+  int wall_left_min_ping_time = desired_left_follow_ping_time - 50;
+  int wall_left_max_ping_time = desired_left_follow_ping_time + 50;
+  int left_follow_speed;
+  int right_follow_speed;
+
+  unsigned int actual_left_ping_time =
+    ping(follow_left_trigger_pin, follow_left_echo_pin);
+  unsigned int actual_right_ping_time =
+    ping(follow_right_trigger_pin, follow_right_echo_pin);
+
+  unsigned int actual_right_table_ping_time =
+    ping(table_right_trigger_pin, table_right_echo_pin);
+
+  left_follow_speed = actual_left_ping_time - desired_left_follow_ping_time;
+  right_follow_speed = actual_right_ping_time - desired_right_follow_ping_time;
+
+  Serial.print("left: ping = ");
+  Serial.print(actual_left_ping_time);
+  Serial.print(" speed = ");
+  Serial.print(left_follow_speed);
+  Serial.print(" right: ping = ");
+  Serial.print(actual_right_ping_time);
+  Serial.print(" speed = ");
+  Serial.println(right_follow_speed);
+
+  boolean over_table =
+    actual_right_table_ping_time < right_max_ping_time_over_table;
+  // if the actual ping time is in between the min and max ping time,
+  //  then the robot is at the desired distance from the wall.
+  boolean left_follow_sonar_desired_distance =
+    (wall_left_min_ping_time < actual_left_ping_time
+     && actual_left_ping_time < wall_left_max_ping_time);
+  boolean right_follow_sonar_desired_distance =
+    (wall_right_min_ping_time < actual_right_ping_time
+     && actual_right_ping_time < wall_right_max_ping_time);
+  // if the left and right pings show the desired distance,
+  //  the robot is facing the wall
+  boolean facing_wall =
+    left_follow_sonar_desired_distance && right_follow_sonar_desired_distance;
+
+  if(over_table){
+    if(facing_wall){
+      backup(1500);
+      turn_around(3000);}
+    else{ // not facing wall
+      go(left_follow_speed, right_follow_speed);}}
+  else{ // not over table
+    backup(1500);
+    turn_around(3000);}}