a77b88c5 |
1 | /* |
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. |
6 | |
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. |
11 | |
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/>. |
14 | */ |
15 | // This program implements table top challenge level 1 -- |
16 | // The robot stays on the table without falling off. |
17 | |
18 | // This example code is in the public domain. |
19 | |
20 | // Set *debug* to 1 to enable debugging statements: |
21 | int debug = 1; |
22 | |
23 | // Set *stop_at_edge* to 1 to have robot stop at the edge; |
24 | // otherwise, it will back away from edge: |
25 | int stop_at_edge = 0; |
26 | |
27 | // Set *table_top_threshold* to a value that captures the |
28 | // tabletop distance for *your* robot: |
29 | int table_top_threshold = 600; |
30 | |
31 | // Set *sonar_timeout* to a value a 200-300 uSec longer |
32 | // that your table top threshold: |
33 | |
34 | int sonar_timeout = 1600; |
35 | |
36 | // Set *sonar_set_point* to a value that specifies the approximate |
37 | // follow distance: |
38 | |
39 | int sonar_follow_distance = 800; |
40 | |
41 | // Pin definitions: |
42 | int led = 13; |
43 | int left_motor_enable = 3; |
44 | int left_motor_a = 4; |
45 | int left_motor_b = 5; |
46 | int right_motor_enable = 10; |
47 | int right_motor_a = 9; |
48 | int right_motor_b = 8; |
49 | int right_sonar_trigger = 7; |
50 | int right_sonar_echo = 6; |
51 | int left_sonar_trigger = 12; |
52 | int left_sonar_echo = 11; |
53 | |
54 | // the setup routine runs once when you press reset: |
55 | void setup() { |
56 | // If debugging is enabled, |
57 | if (debug) { |
58 | // 115200 is the fastest "standard" baud rate for debugging. |
59 | // Be sure to set the baud Tools=>Serial Monitor to 115200: |
60 | Serial.begin(115200); |
61 | } |
62 | |
63 | // Set LED output pin: |
64 | pinMode(led, OUTPUT); |
65 | |
66 | // Set H-bridge control pins: |
67 | pinMode(left_motor_enable, OUTPUT); |
68 | pinMode(left_motor_a, OUTPUT); |
69 | pinMode(left_motor_b, OUTPUT); |
70 | pinMode(right_motor_enable, OUTPUT); |
71 | pinMode(right_motor_a, OUTPUT); |
72 | pinMode(right_motor_b, OUTPUT); |
73 | |
74 | // Set control pins for both left and right sonars: |
75 | pinMode(left_sonar_trigger, OUTPUT); |
76 | pinMode(left_sonar_echo, INPUT); |
77 | pinMode(right_sonar_trigger, OUTPUT); |
78 | pinMode(right_sonar_echo, INPUT); |
79 | |
80 | // Set all the pin values to predefined values: |
81 | digitalWrite(led, HIGH); |
82 | digitalWrite(left_motor_enable, LOW); |
83 | digitalWrite(left_motor_a, LOW); |
84 | digitalWrite(left_motor_b, LOW); |
85 | digitalWrite(right_motor_enable, LOW); |
86 | digitalWrite(right_motor_a, LOW); |
87 | digitalWrite(right_motor_b, LOW); |
88 | digitalWrite(left_sonar_trigger, LOW); |
89 | digitalWrite(right_sonar_trigger, LOW); |
90 | digitalWrite(left_sonar_trigger, LOW); |
91 | digitalWrite(right_sonar_trigger, LOW); |
92 | } |
93 | |
94 | void motorsRun(int left, int right, int ms_delay) { |
95 | // Set left motor direction: |
96 | if (left > 0) { |
97 | // Set left motor to go forward: |
98 | digitalWrite(left_motor_a, HIGH); |
99 | digitalWrite(left_motor_b, LOW); |
100 | } else { |
101 | // Set left motor to go backward: |
102 | digitalWrite(left_motor_a, LOW); |
103 | digitalWrite(left_motor_b, HIGH); |
104 | left = -left; // Make left a positive value: |
105 | } |
106 | // Make sure we cap the speed at 255: |
107 | if (left > 255) { |
108 | left = 255; |
109 | } |
110 | analogWrite(left_motor_enable, left); // Start motor in right direction |
111 | |
112 | // Set right motor direction: |
113 | if (right > 0) { |
114 | // Set right motor to go forward: |
115 | digitalWrite(right_motor_a, HIGH); |
116 | digitalWrite(right_motor_b, LOW); |
117 | } else { |
118 | // Set right motor to go backward: |
119 | digitalWrite(right_motor_a, LOW); |
120 | digitalWrite(right_motor_b, HIGH); |
121 | right = -right; // Make right a positive value: |
122 | } |
123 | // Make sure we cap the speed at 255: |
124 | if (right > 255) { |
125 | right = 255; |
126 | } |
127 | analogWrite(right_motor_enable, right); // Start motor in right direction |
128 | |
129 | delay(ms_delay); // Wait the specified amount of time |
130 | } |
131 | |
132 | // This routine will trigger a HC-SR04 sonar that has its |
133 | // trigger pin attached to *sonar_trigger* and its echo |
134 | // pin attached to *sonar_echo*. If the echo response |
135 | // takes longer than *timeout* microseconds, *timeout* is |
136 | // returned. |
137 | int ping(int sonar_trigger, int sonar_echo, int timeout) { |
138 | int result = timeout; |
139 | if (!digitalRead(sonar_echo)) { |
140 | // *sonar_echo* is LOW, so we can safely trigger a 12uS |
141 | // trigger pulse: |
142 | digitalWrite(sonar_trigger, HIGH); |
143 | delayMicroseconds(12); |
144 | digitalWrite(sonar_trigger, LOW); |
145 | |
146 | // Time the resulting echo pulse. If the resulting echo pulse |
147 | // is longer than *timeout*, *pulseIn* returns 0. |
148 | result = pulseIn(sonar_echo, HIGH, timeout); |
149 | |
150 | if (result == 0) { |
151 | // We timed out, so return *timeout*: |
152 | result = timeout; |
153 | } |
154 | } // else *sonar_echo* is still high from the previous trigger; |
155 | // return *timeout* when this occurs. |
156 | return result; |
157 | } |
158 | |
159 | // the loop routine runs over and over again forever: |
160 | void loop() { |
161 | // Get the left and right sonar values: |
162 | int left_ping = ping(left_sonar_trigger, left_sonar_echo, sonar_timeout); |
163 | int right_ping = ping(right_sonar_trigger, right_sonar_echo, sonar_timeout); |
164 | |
165 | // Set the left and right motor speed: |
166 | int left_speed = left_ping - sonar_follow_distance; |
167 | int right_speed = right_ping - sonar_follow_distance; |
168 | |
169 | // Debugging code: |
170 | if (debug) { |
171 | // Print some debugging information: |
172 | Serial.print("lp="); |
173 | Serial.print(left_ping); |
174 | Serial.print(" rp="); |
175 | Serial.print(right_ping); |
176 | Serial.print(" ls="); |
177 | Serial.print(left_speed); |
178 | Serial.print(" rs="); |
179 | Serial.println(right_speed); |
180 | |
181 | // Delay a little so that the serial port does not get |
182 | // swamped with debugging information: |
183 | delay(100); |
184 | } |
185 | |
186 | // Set the motor speeds: |
187 | motorsRun(left_speed, right_speed, 0); |
188 | } |