-/*
- 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; // orange wire
-int right_motor_forward_pin = 4; // brown
-int right_motor_backward_pin = 5; // purple
-
-int right_echo_pin = 6; // white
-int right_trigger_pin = 7; // blue
-
-int left_motor_speed_pin = 8; // orange
-int left_motor_forward_pin = 9; // brown
-int left_motor_backward_pin = 10; // purple
-
-int left_echo_pin = 11; // white
-int left_trigger_pin = 12; // blue
-
-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);}
- 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 400
- int right_max_ping_time_over_table = 400;
- int left_max_ping_time_over_table = 400;
- 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);}}
projects / challenge-bot / commitdiff