From: daniel watson Date: Mon, 25 Aug 2014 02:54:06 +0000 (-0700) Subject: follow while on table X-Git-Url: http://challenge-bot.com/repos/?p=challenge-bot;a=commitdiff_plain;h=5eff75dab1e11aeea0629dd1b1fca454b3ba260e 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 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 . + */ + +// 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);}}