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.
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.
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/>.
17 #include "bot_with_leds_map.h"
20 digitalWrite(pin
, HIGH
);}
23 digitalWrite(pin
, LOW
);}
25 void set_motor(int speed_pin
,
39 // since speed has been set positive, no need to check if speed < -255.
42 analogWrite(speed_pin
, speed
);}
44 void go(const int left_motor_speed
, const int right_motor_speed
){
45 set_motor(left_motor_speed_pin
,
46 left_motor_forward_pin
,
47 left_motor_backward_pin
,
49 set_motor(right_motor_speed_pin
,
50 right_motor_forward_pin
,
51 right_motor_backward_pin
,
54 int ping(int trigger
, int echo
){
59 // turn on the trigger and leave it on long enough for the
60 // sonar sensor to notice
62 delayMicroseconds(10);
64 ping_time
= pulseIn(echo
, HIGH
);
67 // sonar needs some time to recover before pinging again,
68 // so make sure it gets enough sleep right here. 50 milliseconds
75 pinMode(LED_BUILTIN
, OUTPUT
);
77 pinMode(button_pin
, INPUT_PULLUP
);
79 pinMode(left_led_pin
, OUTPUT
);
80 pinMode(right_led_pin
, OUTPUT
);
82 pinMode(right_motor_speed_pin
, OUTPUT
);
83 pinMode(right_motor_forward_pin
, OUTPUT
);
84 pinMode(right_motor_backward_pin
, OUTPUT
);
86 pinMode(right_echo_pin
, INPUT
);
87 pinMode(right_trigger_pin
, OUTPUT
);
89 pinMode(left_motor_speed_pin
, OUTPUT
);
90 pinMode(left_motor_forward_pin
, OUTPUT
);
91 pinMode(left_motor_backward_pin
, OUTPUT
);
93 pinMode(left_echo_pin
, INPUT
);
94 pinMode(left_trigger_pin
, OUTPUT
);
96 off(left_motor_speed_pin
);
97 off(left_motor_forward_pin
);
98 off(left_motor_backward_pin
);
99 off(left_trigger_pin
);
101 off(right_motor_speed_pin
);
102 off(right_motor_forward_pin
);
103 off(right_motor_backward_pin
);
104 off(right_trigger_pin
);}
106 enum class Stay_on_table_state
{
107 going
, start_backing
, backing
, start_turning
, turning
};
108 static Stay_on_table_state stay_on_table_state
=
109 Stay_on_table_state::going
;
111 void going(const unsigned int left_ping_time
,
112 const unsigned int right_ping_time
) {
113 Serial
.print("going ");
114 int forward_speed
= 250;
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
123 const int right_max_ping_time_over_table
= 800;
124 const int left_max_ping_time_over_table
= 800;
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
; }
131 left_speed
= stop_speed
; }
132 if(right_ping_time
<= right_max_ping_time_over_table
) {
133 right_speed
= forward_speed
; }
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
; }
140 go(left_speed
, right_speed
); }
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
;
150 if(backup_duration
> allowed_backup_duration
) {
151 stay_on_table_state
= Stay_on_table_state::start_turning
; } }
153 void turning(unsigned long start_turning_time
) {
154 Serial
.print("turning ");
155 // the exact amount of time for turning around might need
156 // twerking for your robot. the default value is 3200
157 static const unsigned int allowed_turning_duration
= 2500;
158 unsigned long now
= millis();
159 unsigned long turning_duration
= now
- start_turning_time
;
163 if(turning_duration
> allowed_turning_duration
) {
164 stay_on_table_state
= Stay_on_table_state::going
; } }
166 void stay_on_table(const unsigned int left_ping_time
,
167 const unsigned int right_ping_time
){
168 Serial
.print("stay on table ");
169 static unsigned long start_backing_time
= 0;
170 static unsigned long start_turning_time
= 0;
173 switch(stay_on_table_state
) {
174 case Stay_on_table_state::going
:
175 going(left_ping_time
, right_ping_time
);
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
);
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
);
192 void follow(const unsigned int left_ping_time
,
193 const unsigned int right_ping_time
) {
194 // you'll need to adjust these based on your sonar sensor's behavior
195 const unsigned int desired_right_ping_time
= 800;
196 const unsigned int desired_left_ping_time
= 800;
199 left_speed
= left_ping_time
- desired_left_ping_time
,
200 right_speed
= right_ping_time
- desired_right_ping_time
;
202 go(left_speed
, right_speed
); }
204 enum class Behavior
{stay_on_table
, follow
};
205 Behavior behavior
= Behavior::stay_on_table
;
207 enum class Button_state
{up
, down
};
209 Button_state prior_button_state
= Button_state::up
;
212 static int count
= 0;
218 left_ping_time
= ping(left_trigger_pin
, left_echo_pin
),
219 right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
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
);
226 Button_state button_state
=
227 (digitalRead(button_pin
) == HIGH
)
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; }
234 // if button was just pressed
235 if (prior_button_state
== Button_state::up
236 && button_state
== Button_state::down
) {
237 // indicate button press recognized
239 // turn off motors, to allow robot to be set down
243 case Behavior::stay_on_table
: behavior
= Behavior::follow
; break;
244 case Behavior::follow
: behavior
= Behavior::stay_on_table
; break; }
248 case Behavior::stay_on_table
:
249 stay_on_table(left_ping_time
, right_ping_time
);
251 case Behavior:: follow
:
252 follow(left_ping_time
, right_ping_time
);
255 prior_button_state
= button_state
;