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 | |
f9290e37 |
79 | pinMode(left_led_pin, OUTPUT); |
80 | pinMode(right_led_pin, OUTPUT); |
81 | |
73ad8845 |
82 | pinMode(right_motor_speed_pin, OUTPUT); |
83 | pinMode(right_motor_forward_pin, OUTPUT); |
84 | pinMode(right_motor_backward_pin, OUTPUT); |
85 | |
86 | pinMode(right_echo_pin, INPUT); |
87 | pinMode(right_trigger_pin, OUTPUT); |
88 | |
89 | pinMode(left_motor_speed_pin, OUTPUT); |
90 | pinMode(left_motor_forward_pin, OUTPUT); |
91 | pinMode(left_motor_backward_pin, OUTPUT); |
92 | |
93 | pinMode(left_echo_pin, INPUT); |
94 | pinMode(left_trigger_pin, OUTPUT); |
95 | |
96 | off(left_motor_speed_pin); |
97 | off(left_motor_forward_pin); |
98 | off(left_motor_backward_pin); |
99 | off(left_trigger_pin); |
100 | |
101 | off(right_motor_speed_pin); |
102 | off(right_motor_forward_pin); |
103 | off(right_motor_backward_pin); |
104 | off(right_trigger_pin);} |
105 | |
dad0fdb2 |
106 | enum class Stay_on_table_state { |
ca385942 |
107 | going, start_backing, backing, start_turning, turning }; |
dad0fdb2 |
108 | static Stay_on_table_state stay_on_table_state = |
109 | Stay_on_table_state::going; |
73ad8845 |
110 | |
f9290e37 |
111 | void going(const unsigned int left_ping_time, |
112 | const unsigned int right_ping_time) { |
dad0fdb2 |
113 | Serial.print("going "); |
73ad8845 |
114 | int forward_speed = 250; |
115 | int stop_speed = 0; |
116 | |
dad0fdb2 |
117 | int left_speed; |
118 | int right_speed; |
119 | |
73ad8845 |
120 | // adjust this number as necessary for your robot. |
121 | // it represents how far the table is from your sonar sensor. |
122 | // larger values mean larger distance. default is 800 |
dad0fdb2 |
123 | const int right_max_ping_time_over_table = 800; |
124 | const int left_max_ping_time_over_table = 800; |
125 | |
dad0fdb2 |
126 | if (left_ping_time <= left_max_ping_time_over_table |
127 | || right_ping_time <= right_max_ping_time_over_table) { |
128 | if(left_ping_time <= left_max_ping_time_over_table) { |
129 | left_speed = forward_speed; } |
130 | else { |
131 | left_speed = stop_speed; } |
132 | if(right_ping_time <= right_max_ping_time_over_table) { |
133 | right_speed = forward_speed; } |
134 | else { |
135 | right_speed = stop_speed; } } |
136 | else { // both ping times were above max acceptable ping time |
137 | left_speed = right_speed = 0; |
138 | stay_on_table_state = Stay_on_table_state::start_backing; } |
139 | |
140 | go(left_speed, right_speed); } |
141 | |
142 | void backing(unsigned long start_backing) { |
143 | Serial.print("backing "); |
144 | static const unsigned int allowed_backup_duration = 500; |
145 | unsigned long now = millis(); |
146 | unsigned long backup_duration = now - start_backing; |
147 | |
148 | go(-250, -250); |
149 | |
150 | if(backup_duration > allowed_backup_duration) { |
151 | stay_on_table_state = Stay_on_table_state::start_turning; } } |
152 | |
153 | void turning(unsigned long start_turning_time) { |
154 | Serial.print("turning "); |
73ad8845 |
155 | // the exact amount of time for turning around might need |
156 | // twerking for your robot. the default value is 3200 |
dad0fdb2 |
157 | static const unsigned int allowed_turning_duration = 2500; |
158 | unsigned long now = millis(); |
159 | unsigned long turning_duration = now - start_turning_time; |
160 | |
161 | go(-250, 250); |
162 | |
163 | if(turning_duration > allowed_turning_duration) { |
164 | stay_on_table_state = Stay_on_table_state::going; } } |
165 | |
f9290e37 |
166 | void stay_on_table(const unsigned int left_ping_time, |
167 | const unsigned int right_ping_time){ |
dad0fdb2 |
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 | |
dad0fdb2 |
173 | switch(stay_on_table_state) { |
f9290e37 |
174 | case Stay_on_table_state::going: |
175 | going(left_ping_time, right_ping_time); |
176 | break; |
dad0fdb2 |
177 | case Stay_on_table_state::start_backing: |
178 | Serial.print("start backing "); |
179 | start_backing_time = millis(); |
180 | stay_on_table_state = Stay_on_table_state::backing; |
181 | case Stay_on_table_state::backing: |
182 | backing(start_backing_time); |
183 | break; |
184 | case Stay_on_table_state::start_turning: |
185 | Serial.print("start turning "); |
186 | start_turning_time = millis(); |
187 | stay_on_table_state = Stay_on_table_state::turning; |
188 | case Stay_on_table_state::turning: |
189 | turning(start_turning_time); |
190 | break; } } |
73ad8845 |
191 | |
f9290e37 |
192 | void follow(const unsigned int left_ping_time, |
193 | const unsigned int right_ping_time) { |
dad0fdb2 |
194 | // you'll need to adjust these based on your sonar sensor's behavior |
f9290e37 |
195 | const unsigned int desired_right_ping_time = 800; |
196 | const unsigned int desired_left_ping_time = 800; |
dad0fdb2 |
197 | |
f9290e37 |
198 | const int |
199 | left_speed = left_ping_time - desired_left_ping_time, |
200 | right_speed = right_ping_time - desired_right_ping_time; |
dad0fdb2 |
201 | |
202 | go(left_speed, right_speed); } |
73ad8845 |
203 | |
3cb3ea0b |
204 | enum class Behavior {stay_on_table, follow}; |
205 | Behavior behavior = Behavior::stay_on_table; |
73ad8845 |
206 | |
3cb3ea0b |
207 | enum class Button_state {up, down}; |
73ad8845 |
208 | |
3cb3ea0b |
209 | Button_state prior_button_state = Button_state::up; |
73ad8845 |
210 | |
3cb3ea0b |
211 | void loop() { |
212 | static int count = 0; |
213 | Serial.print(count); |
214 | Serial.print(" "); |
215 | count++; |
216 | |
f9290e37 |
217 | const unsigned int |
218 | left_ping_time = ping(left_trigger_pin, left_echo_pin), |
219 | right_ping_time = ping(right_trigger_pin, right_echo_pin); |
220 | const unsigned int |
221 | left_led_value = map(left_ping_time, 0, 3000, 0, 255), |
222 | right_led_value = map(right_ping_time, 0, 3000, 0, 255); |
223 | analogWrite(left_led_pin, left_led_value); |
224 | analogWrite(right_led_pin, right_led_value); |
225 | |
3cb3ea0b |
226 | Button_state button_state = |
227 | (digitalRead(button_pin) == HIGH) |
228 | ? Button_state::up |
229 | : Button_state::down; |
230 | switch(button_state) { |
231 | case Button_state::up: Serial.print("up "); break; |
232 | case Button_state::down: Serial.print("down "); break; } |
73ad8845 |
233 | |
234 | // if button was just pressed |
3cb3ea0b |
235 | if (prior_button_state == Button_state::up |
236 | && button_state == Button_state::down) { |
73ad8845 |
237 | // indicate button press recognized |
238 | on(LED_BUILTIN); |
239 | // turn off motors, to allow robot to be set down |
240 | go(0, 0); |
241 | delay(1000); |
242 | switch(behavior) { |
3cb3ea0b |
243 | case Behavior::stay_on_table: behavior = Behavior::follow; break; |
244 | case Behavior::follow: behavior = Behavior::stay_on_table; break; } |
73ad8845 |
245 | off(LED_BUILTIN); } |
246 | |
247 | switch(behavior) { |
f9290e37 |
248 | case Behavior::stay_on_table: |
249 | stay_on_table(left_ping_time, right_ping_time); |
250 | break; |
251 | case Behavior:: follow: |
252 | follow(left_ping_time, right_ping_time); |
253 | break; } |
3cb3ea0b |
254 | |
255 | prior_button_state = button_state; |
73ad8845 |
256 | |
3cb3ea0b |
257 | Serial.println(); } |