73ad8845 |
1 | |
2 | /* |
3 | This program is free software: you can redistribute it and/or modify |
4 | it under the terms of the GNU Affero General Public License as |
5 | published by the Free Software Foundation, either version 3 of the |
6 | License, or (at your option) any later version. |
7 | |
8 | This program is distributed in the hope that it will be useful, |
9 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
11 | GNU Affero General Public License for more details. |
12 | |
13 | You should have received a copy of the GNU Affero General Public License |
14 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
15 | */ |
16 | |
9e6034d5 |
17 | #include "bot_with_leds_map.h" |
73ad8845 |
18 | |
19 | void on(int pin){ |
20 | digitalWrite(pin, HIGH);} |
21 | |
22 | void off(int pin){ |
23 | digitalWrite(pin, LOW);} |
24 | |
25 | void set_motor(int speed_pin, |
26 | int forward_pin, |
27 | int backward_pin, |
28 | int speed){ |
29 | if(speed > 0){ |
30 | off(backward_pin); |
31 | on(forward_pin);} |
32 | else if(speed < 0){ |
33 | off(forward_pin); |
34 | on(backward_pin); |
35 | speed = -speed;} |
36 | else{ // speed is 0 |
37 | off(forward_pin); |
38 | off(backward_pin);} |
39 | // since speed has been set positive, no need to check if speed < -255. |
40 | if(speed > 255){ |
41 | speed = 255;} |
42 | analogWrite(speed_pin, speed);} |
43 | |
c8a4b2d2 |
44 | void go(const int left_motor_speed, const int right_motor_speed){ |
73ad8845 |
45 | set_motor(left_motor_speed_pin, |
46 | left_motor_forward_pin, |
47 | left_motor_backward_pin, |
48 | left_motor_speed); |
49 | set_motor(right_motor_speed_pin, |
50 | right_motor_forward_pin, |
51 | right_motor_backward_pin, |
52 | right_motor_speed);} |
53 | |
54 | int ping(int trigger, int echo){ |
55 | int ping_time = 0; |
56 | // turn off trigger |
57 | off(trigger); |
58 | delayMicroseconds(2); |
59 | // turn on the trigger and leave it on long enough for the |
60 | // sonar sensor to notice |
61 | on(trigger); |
62 | delayMicroseconds(10); |
63 | off(trigger); |
64 | ping_time = pulseIn(echo, HIGH); |
65 | if(ping_time <= 0){ |
66 | ping_time = 3000;} |
67 | // sonar needs some time to recover before pinging again, |
68 | // so make sure it gets enough sleep right here. 50 milliseconds |
69 | delay(50); |
70 | return ping_time;} |
71 | |
73ad8845 |
72 | void setup(){ |
73 | Serial.begin(9600); |
74 | |
75 | pinMode(LED_BUILTIN, OUTPUT); |
76 | |
77 | pinMode(button_pin, INPUT_PULLUP); |
78 | |
79 | pinMode(right_motor_speed_pin, OUTPUT); |
80 | pinMode(right_motor_forward_pin, OUTPUT); |
81 | pinMode(right_motor_backward_pin, OUTPUT); |
82 | |
83 | pinMode(right_echo_pin, INPUT); |
84 | pinMode(right_trigger_pin, OUTPUT); |
85 | |
86 | pinMode(left_motor_speed_pin, OUTPUT); |
87 | pinMode(left_motor_forward_pin, OUTPUT); |
88 | pinMode(left_motor_backward_pin, OUTPUT); |
89 | |
90 | pinMode(left_echo_pin, INPUT); |
91 | pinMode(left_trigger_pin, OUTPUT); |
92 | |
93 | off(left_motor_speed_pin); |
94 | off(left_motor_forward_pin); |
95 | off(left_motor_backward_pin); |
96 | off(left_trigger_pin); |
97 | |
98 | off(right_motor_speed_pin); |
99 | off(right_motor_forward_pin); |
100 | off(right_motor_backward_pin); |
101 | off(right_trigger_pin);} |
102 | |
dad0fdb2 |
103 | enum class Stay_on_table_state { |
104 | going, start_backing, backing, start_turning, turning}; |
105 | static Stay_on_table_state stay_on_table_state = |
106 | Stay_on_table_state::going; |
73ad8845 |
107 | |
dad0fdb2 |
108 | void going() { |
109 | Serial.print("going "); |
73ad8845 |
110 | int forward_speed = 250; |
111 | int stop_speed = 0; |
112 | |
dad0fdb2 |
113 | int left_speed; |
114 | int right_speed; |
115 | |
73ad8845 |
116 | // adjust this number as necessary for your robot. |
117 | // it represents how far the table is from your sonar sensor. |
118 | // larger values mean larger distance. default is 800 |
dad0fdb2 |
119 | const int right_max_ping_time_over_table = 800; |
120 | const int left_max_ping_time_over_table = 800; |
121 | |
122 | const int left_ping_time = |
123 | ping(left_trigger_pin, left_echo_pin); |
124 | const int right_ping_time = |
125 | ping(right_trigger_pin, right_echo_pin); |
126 | |
127 | if (left_ping_time <= left_max_ping_time_over_table |
128 | || right_ping_time <= right_max_ping_time_over_table) { |
129 | if(left_ping_time <= left_max_ping_time_over_table) { |
130 | left_speed = forward_speed; } |
131 | else { |
132 | left_speed = stop_speed; } |
133 | if(right_ping_time <= right_max_ping_time_over_table) { |
134 | right_speed = forward_speed; } |
135 | else { |
136 | right_speed = stop_speed; } } |
137 | else { // both ping times were above max acceptable ping time |
138 | left_speed = right_speed = 0; |
139 | stay_on_table_state = Stay_on_table_state::start_backing; } |
140 | |
141 | go(left_speed, right_speed); } |
142 | |
143 | void backing(unsigned long start_backing) { |
144 | Serial.print("backing "); |
145 | static const unsigned int allowed_backup_duration = 500; |
146 | unsigned long now = millis(); |
147 | unsigned long backup_duration = now - start_backing; |
148 | |
149 | go(-250, -250); |
150 | |
151 | if(backup_duration > allowed_backup_duration) { |
152 | stay_on_table_state = Stay_on_table_state::start_turning; } } |
153 | |
154 | void turning(unsigned long start_turning_time) { |
155 | Serial.print("turning "); |
73ad8845 |
156 | // the exact amount of time for turning around might need |
157 | // twerking for your robot. the default value is 3200 |
dad0fdb2 |
158 | static const unsigned int allowed_turning_duration = 2500; |
159 | unsigned long now = millis(); |
160 | unsigned long turning_duration = now - start_turning_time; |
161 | |
162 | go(-250, 250); |
163 | |
164 | if(turning_duration > allowed_turning_duration) { |
165 | stay_on_table_state = Stay_on_table_state::going; } } |
166 | |
167 | void stay_on_table(){ |
168 | Serial.print("stay on table "); |
169 | static unsigned long start_backing_time = 0; |
170 | static unsigned long start_turning_time = 0; |
171 | |
73ad8845 |
172 | |
173 | int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); |
174 | int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); |
175 | |
dad0fdb2 |
176 | switch(stay_on_table_state) { |
177 | case Stay_on_table_state::going: going(); break; |
178 | case Stay_on_table_state::start_backing: |
179 | Serial.print("start backing "); |
180 | start_backing_time = millis(); |
181 | stay_on_table_state = Stay_on_table_state::backing; |
182 | case Stay_on_table_state::backing: |
183 | backing(start_backing_time); |
184 | break; |
185 | case Stay_on_table_state::start_turning: |
186 | Serial.print("start turning "); |
187 | start_turning_time = millis(); |
188 | stay_on_table_state = Stay_on_table_state::turning; |
189 | case Stay_on_table_state::turning: |
190 | turning(start_turning_time); |
191 | break; } } |
73ad8845 |
192 | |
193 | void follow() { |
dad0fdb2 |
194 | int left_speed; |
195 | int right_speed; |
196 | |
197 | // you'll need to adjust these based on your sonar sensor's behavior |
198 | int desired_right_ping_time = 800; |
199 | int desired_left_ping_time = 800; |
200 | |
694a1c36 |
201 | unsigned int left_ping_time = ping(left_trigger_pin, left_echo_pin); |
202 | unsigned int right_ping_time = ping(right_trigger_pin, right_echo_pin); |
dad0fdb2 |
203 | |
694a1c36 |
204 | left_speed = left_ping_time - desired_left_ping_time; |
205 | right_speed = right_ping_time - desired_right_ping_time; |
dad0fdb2 |
206 | |
207 | Serial.print(", left: ping = "); |
694a1c36 |
208 | Serial.print(left_ping_time); |
dad0fdb2 |
209 | Serial.print(" speed = "); |
210 | Serial.print(left_speed); |
211 | Serial.print(" right: ping = "); |
694a1c36 |
212 | Serial.print(right_ping_time); |
dad0fdb2 |
213 | Serial.print(" speed = "); |
214 | Serial.print(right_speed); |
215 | |
216 | go(left_speed, right_speed); } |
73ad8845 |
217 | |
3cb3ea0b |
218 | enum class Behavior {stay_on_table, follow}; |
219 | Behavior behavior = Behavior::stay_on_table; |
73ad8845 |
220 | |
3cb3ea0b |
221 | enum class Button_state {up, down}; |
73ad8845 |
222 | |
3cb3ea0b |
223 | Button_state prior_button_state = Button_state::up; |
73ad8845 |
224 | |
3cb3ea0b |
225 | void loop() { |
226 | static int count = 0; |
227 | Serial.print(count); |
228 | Serial.print(" "); |
229 | count++; |
230 | |
231 | Button_state button_state = |
232 | (digitalRead(button_pin) == HIGH) |
233 | ? Button_state::up |
234 | : Button_state::down; |
235 | switch(button_state) { |
236 | case Button_state::up: Serial.print("up "); break; |
237 | case Button_state::down: Serial.print("down "); break; } |
73ad8845 |
238 | |
239 | // if button was just pressed |
3cb3ea0b |
240 | if (prior_button_state == Button_state::up |
241 | && button_state == Button_state::down) { |
73ad8845 |
242 | // indicate button press recognized |
243 | on(LED_BUILTIN); |
244 | // turn off motors, to allow robot to be set down |
245 | go(0, 0); |
246 | delay(1000); |
247 | switch(behavior) { |
3cb3ea0b |
248 | case Behavior::stay_on_table: behavior = Behavior::follow; break; |
249 | case Behavior::follow: behavior = Behavior::stay_on_table; break; } |
73ad8845 |
250 | off(LED_BUILTIN); } |
251 | |
252 | switch(behavior) { |
3cb3ea0b |
253 | case Behavior::stay_on_table: stay_on_table(); break; |
254 | case Behavior:: follow: follow(); break; } |
255 | |
256 | prior_button_state = button_state; |
73ad8845 |
257 | |
3cb3ea0b |
258 | Serial.println(); } |