indicate wire colors in comments

[challenge-bot] / build-stages / h_stay_on_table / h_stay_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
// blue wire
int right_motor_forward_pin = 3;
// orange wire
int right_motor_backward_pin = 4;
// white wire
int right_motor_speed_pin = 5;

// yellow wire
int right_echo_pin = 6;
// blue wire
int right_trigger_pin = 7;

// blue wire
int left_motor_forward_pin = 8;
// orange wire
int left_motor_backward_pin = 9;
// white wire
int left_motor_speed_pin = 10;

// yellow wire
int left_echo_pin = 11;
// blue wire
int left_trigger_pin = 12;

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(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 loop(){
  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 = 2000;
  // the exact amount of time for turning around might need
  // twerking for your robot.  the default value is 3200
  int turn_around_time = 3200;

  int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
  int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);

  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);}}