X-Git-Url: http://challenge-bot.com/repos/?p=challenge-bot;a=blobdiff_plain;f=build-stages%2Fh_stay_on_table%2Fh_stay_on_table.ino;fp=build-stages%2Fh_stay_on_table%2Fh_stay_on_table.ino;h=73f9e9b61ccaf1b3d0e76df1b319e6efdbbba46a;hp=0000000000000000000000000000000000000000;hb=88c50c0cfc5e067de79c958ba8f834f5049cb723;hpb=790037ce816d4a01a78666fd597a71f1f51bb190 diff --git a/build-stages/h_stay_on_table/h_stay_on_table.ino b/build-stages/h_stay_on_table/h_stay_on_table.ino new file mode 100644 index 0000000..73f9e9b --- /dev/null +++ b/build-stages/h_stay_on_table/h_stay_on_table.ino @@ -0,0 +1,166 @@ +/* + 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; // 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);}}