put left motor speed pin on 10 for pwm

[challenge-bot] / build-stages / i_follow_on_table / i_follow_on_table.ino

/*
 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_forward_pin = 3;
int right_motor_backward_pin = 4;
int right_motor_speed_pin = 5;

int follow_right_echo_pin = 6;
int follow_right_trigger_pin = 7;

int left_motor_forward_pin = 8;
int left_motor_backward_pin = 9;
int left_motor_speed_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);}}