2 This program is free software: you can redistribute it and/or modify
3 it under the terms of the GNU Affero General Public License as
4 published by the Free Software Foundation, either version 3 of the
5 License, or (at your option) any later version.
7 This program is distributed in the hope that it will be useful,
8 but WITHOUT ANY WARRANTY; without even the implied warranty of
9 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 GNU Affero General Public License for more details.
12 You should have received a copy of the GNU Affero General Public License
13 along with this program. If not, see <http://www.gnu.org/licenses/>.
15 // This program implements table top challenge level ###?### --
16 // The robot follows a flat vertical surface at a distance of a couple inches.
18 // Set *debug* to 1 to enable debugging statements:
21 // Set *stop_at_edge* to 1 to have robot stop at the edge;
22 // otherwise, it will back away from edge:
25 // Set *table_top_threshold* to a value that captures the
26 // tabletop distance for *your* robot:
27 int table_top_threshold
= 600;
29 // Set *sonar_timeout* to a value a 200-300 uSec longer
30 // that your table top threshold:
32 int sonar_timeout
= 1600;
34 // Set *sonar_set_point* to a value that specifies the approximate
37 int sonar_follow_distance
= 800;
41 int left_motor_enable
= 3;
44 int right_motor_enable
= 10;
45 int right_motor_a
= 9;
46 int right_motor_b
= 8;
47 int right_sonar_trigger
= 7;
48 int right_sonar_echo
= 6;
49 int left_sonar_trigger
= 12;
50 int left_sonar_echo
= 11;
52 // the setup routine runs once when you press reset:
54 // If debugging is enabled,
56 // 115200 is the fastest "standard" baud rate for debugging.
57 // Be sure to set the baud Tools=>Serial Monitor to 115200:
61 // Set LED output pin:
64 // Set H-bridge control pins:
65 pinMode(left_motor_enable
, OUTPUT
);
66 pinMode(left_motor_a
, OUTPUT
);
67 pinMode(left_motor_b
, OUTPUT
);
68 pinMode(right_motor_enable
, OUTPUT
);
69 pinMode(right_motor_a
, OUTPUT
);
70 pinMode(right_motor_b
, OUTPUT
);
72 // Set control pins for both left and right sonars:
73 pinMode(left_sonar_trigger
, OUTPUT
);
74 pinMode(left_sonar_echo
, INPUT
);
75 pinMode(right_sonar_trigger
, OUTPUT
);
76 pinMode(right_sonar_echo
, INPUT
);
78 // Set all the pin values to predefined values:
79 digitalWrite(led
, HIGH
);
80 digitalWrite(left_motor_enable
, LOW
);
81 digitalWrite(left_motor_a
, LOW
);
82 digitalWrite(left_motor_b
, LOW
);
83 digitalWrite(right_motor_enable
, LOW
);
84 digitalWrite(right_motor_a
, LOW
);
85 digitalWrite(right_motor_b
, LOW
);
86 digitalWrite(left_sonar_trigger
, LOW
);
87 digitalWrite(right_sonar_trigger
, LOW
);
88 digitalWrite(left_sonar_trigger
, LOW
);
89 digitalWrite(right_sonar_trigger
, LOW
);
92 void motors_run(int left
, int right
, int ms_delay
) {
93 // Set left motor direction:
95 // Set left motor to go forward:
96 digitalWrite(left_motor_a
, HIGH
);
97 digitalWrite(left_motor_b
, LOW
);
99 // Set left motor to go backward:
100 digitalWrite(left_motor_a
, LOW
);
101 digitalWrite(left_motor_b
, HIGH
);
102 left
= -left
; // Make left a positive value:
104 // Make sure we cap the speed at 255:
108 analogWrite(left_motor_enable
, left
); // Start motor in right direction
110 // Set right motor direction:
112 // Set right motor to go forward:
113 digitalWrite(right_motor_a
, HIGH
);
114 digitalWrite(right_motor_b
, LOW
);
116 // Set right motor to go backward:
117 digitalWrite(right_motor_a
, LOW
);
118 digitalWrite(right_motor_b
, HIGH
);
119 right
= -right
; // Make right a positive value:
121 // Make sure we cap the speed at 255:
125 analogWrite(right_motor_enable
, right
); // Start motor in right direction
127 delay(ms_delay
); // Wait the specified amount of time
130 // This routine will trigger a HC-SR04 sonar that has its
131 // trigger pin attached to *sonar_trigger* and its echo
132 // pin attached to *sonar_echo*. If the echo response
133 // takes longer than *timeout* microseconds, *timeout* is
135 int ping(int sonar_trigger
, int sonar_echo
, int timeout
) {
136 int result
= timeout
;
137 if (!digitalRead(sonar_echo
)) {
138 // *sonar_echo* is LOW, so we can safely trigger a 12uS
140 digitalWrite(sonar_trigger
, HIGH
);
141 delayMicroseconds(12);
142 digitalWrite(sonar_trigger
, LOW
);
144 // Time the resulting echo pulse. If the resulting echo pulse
145 // is longer than *timeout*, *pulseIn* returns 0.
146 result
= pulseIn(sonar_echo
, HIGH
, timeout
);
149 // We timed out, so return *timeout*:
153 // else *sonar_echo* is still high from the previous trigger;
154 // return *timeout* when this occurs.
158 // the loop routine runs over and over again forever:
160 // Get the left and right sonar values:
161 int left_ping
= ping(left_sonar_trigger
, left_sonar_echo
, sonar_timeout
);
162 int right_ping
= ping(right_sonar_trigger
, right_sonar_echo
, sonar_timeout
);
164 // Set the left and right motor speed:
165 int left_speed
= left_ping
- sonar_follow_distance
;
166 int right_speed
= right_ping
- sonar_follow_distance
;
170 // Print some debugging information:
172 Serial
.print(left_ping
);
173 Serial
.print(" rp=");
174 Serial
.print(right_ping
);
175 Serial
.print(" ls=");
176 Serial
.print(left_speed
);
177 Serial
.print(" rs=");
178 Serial
.println(right_speed
);
180 // Delay a little so that the serial port does not get
181 // swamped with debugging information:
185 // Set the motor speeds:
186 motors_run(left_speed
, right_speed
, 0);