+++ /dev/null
-/*
- 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/>.
- */
-// This program implements table top challenge level ###?### --
-// The robot follows a flat vertical surface at a distance of a couple inches.
-
-// Set *debug* to 1 to enable debugging statements:
-int debug = 1;
-
-// Set *stop_at_edge* to 1 to have robot stop at the edge;
-// otherwise, it will back away from edge:
-int stop_at_edge = 0;
-
-// Set *table_top_threshold* to a value that captures the
-// tabletop distance for *your* robot:
-int table_top_threshold = 600;
-
-// Set *sonar_timeout* to a value a 200-300 uSec longer
-// that your table top threshold:
-
-int sonar_timeout = 1600;
-
-// Set *sonar_set_point* to a value that specifies the approximate
-// follow distance:
-
-int sonar_follow_distance = 800;
-
-// Pin definitions:
-int led = 13;
-int left_motor_enable = 3;
-int left_motor_a = 4;
-int left_motor_b = 5;
-int right_motor_enable = 10;
-int right_motor_a = 9;
-int right_motor_b = 8;
-int right_sonar_trigger = 7;
-int right_sonar_echo = 6;
-int left_sonar_trigger = 12;
-int left_sonar_echo = 11;
-
-// the setup routine runs once when you press reset:
-void setup() {
- // If debugging is enabled,
- if (debug) {
- // 115200 is the fastest "standard" baud rate for debugging.
- // Be sure to set the baud Tools=>Serial Monitor to 115200:
- Serial.begin(9600);
- }
-
- // Set LED output pin:
- pinMode(led, OUTPUT);
-
- // Set H-bridge control pins:
- pinMode(left_motor_enable, OUTPUT);
- pinMode(left_motor_a, OUTPUT);
- pinMode(left_motor_b, OUTPUT);
- pinMode(right_motor_enable, OUTPUT);
- pinMode(right_motor_a, OUTPUT);
- pinMode(right_motor_b, OUTPUT);
-
- // Set control pins for both left and right sonars:
- pinMode(left_sonar_trigger, OUTPUT);
- pinMode(left_sonar_echo, INPUT);
- pinMode(right_sonar_trigger, OUTPUT);
- pinMode(right_sonar_echo, INPUT);
-
- // Set all the pin values to predefined values:
- digitalWrite(led, HIGH);
- digitalWrite(left_motor_enable, LOW);
- digitalWrite(left_motor_a, LOW);
- digitalWrite(left_motor_b, LOW);
- digitalWrite(right_motor_enable, LOW);
- digitalWrite(right_motor_a, LOW);
- digitalWrite(right_motor_b, LOW);
- digitalWrite(left_sonar_trigger, LOW);
- digitalWrite(right_sonar_trigger, LOW);
- digitalWrite(left_sonar_trigger, LOW);
- digitalWrite(right_sonar_trigger, LOW);
-}
-
-void motors_run(int left, int right, int ms_delay) {
- // Set left motor direction:
- if (left > 0) {
- // Set left motor to go forward:
- digitalWrite(left_motor_a, HIGH);
- digitalWrite(left_motor_b, LOW);
- } else {
- // Set left motor to go backward:
- digitalWrite(left_motor_a, LOW);
- digitalWrite(left_motor_b, HIGH);
- left = -left; // Make left a positive value:
- }
- // Make sure we cap the speed at 255:
- if (left > 255) {
- left = 255;
- }
- analogWrite(left_motor_enable, left); // Start motor in right direction
-
- // Set right motor direction:
- if (right > 0) {
- // Set right motor to go forward:
- digitalWrite(right_motor_a, HIGH);
- digitalWrite(right_motor_b, LOW);
- } else {
- // Set right motor to go backward:
- digitalWrite(right_motor_a, LOW);
- digitalWrite(right_motor_b, HIGH);
- right = -right; // Make right a positive value:
- }
- // Make sure we cap the speed at 255:
- if (right > 255) {
- right = 255;
- }
- analogWrite(right_motor_enable, right); // Start motor in right direction
-
- delay(ms_delay); // Wait the specified amount of time
-}
-
-// This routine will trigger a HC-SR04 sonar that has its
-// trigger pin attached to *sonar_trigger* and its echo
-// pin attached to *sonar_echo*. If the echo response
-// takes longer than *timeout* microseconds, *timeout* is
-// returned.
-int ping(int sonar_trigger, int sonar_echo, int timeout) {
- int result = timeout;
- if (!digitalRead(sonar_echo)) {
- // *sonar_echo* is LOW, so we can safely trigger a 12uS
- // trigger pulse:
- digitalWrite(sonar_trigger, HIGH);
- delayMicroseconds(12);
- digitalWrite(sonar_trigger, LOW);
-
- // Time the resulting echo pulse. If the resulting echo pulse
- // is longer than *timeout*, *pulseIn* returns 0.
- result = pulseIn(sonar_echo, HIGH, timeout);
-
- if (result == 0) {
- // We timed out, so return *timeout*:
- result = timeout;
- }
- }
- // else *sonar_echo* is still high from the previous trigger;
- // return *timeout* when this occurs.
- return result;
-}
-
-// the loop routine runs over and over again forever:
-void loop() {
- // Get the left and right sonar values:
- int left_ping = ping(left_sonar_trigger, left_sonar_echo, sonar_timeout);
- int right_ping = ping(right_sonar_trigger, right_sonar_echo, sonar_timeout);
-
- // Set the left and right motor speed:
- int left_speed = left_ping - sonar_follow_distance;
- int right_speed = right_ping - sonar_follow_distance;
-
- // Debugging code:
- if (debug) {
- // Print some debugging information:
- Serial.print("lp=");
- Serial.print(left_ping);
- Serial.print(" rp=");
- Serial.print(right_ping);
- Serial.print(" ls=");
- Serial.print(left_speed);
- Serial.print(" rs=");
- Serial.println(right_speed);
-
- // Delay a little so that the serial port does not get
- // swamped with debugging information:
- delay(100);
- }
-
- // Set the motor speeds:
- motors_run(left_speed, right_speed, 0);
-}
projects / challenge-bot / blobdiff