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(int left_motor_speed
, 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(right_motor_speed_pin
, OUTPUT
);
80 pinMode(right_motor_forward_pin
, OUTPUT
);
81 pinMode(right_motor_backward_pin
, OUTPUT
);
83 pinMode(right_echo_pin
, INPUT
);
84 pinMode(right_trigger_pin
, OUTPUT
);
86 pinMode(left_motor_speed_pin
, OUTPUT
);
87 pinMode(left_motor_forward_pin
, OUTPUT
);
88 pinMode(left_motor_backward_pin
, OUTPUT
);
90 pinMode(left_echo_pin
, INPUT
);
91 pinMode(left_trigger_pin
, OUTPUT
);
93 off(left_motor_speed_pin
);
94 off(left_motor_forward_pin
);
95 off(left_motor_backward_pin
);
96 off(left_trigger_pin
);
98 off(right_motor_speed_pin
);
99 off(right_motor_forward_pin
);
100 off(right_motor_backward_pin
);
101 off(right_trigger_pin
);}
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
;
109 Serial
.print("going ");
110 int forward_speed
= 250;
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
119 const int right_max_ping_time_over_table
= 800;
120 const int left_max_ping_time_over_table
= 800;
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
);
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
; }
132 left_speed
= stop_speed
; }
133 if(right_ping_time
<= right_max_ping_time_over_table
) {
134 right_speed
= forward_speed
; }
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
; }
141 go(left_speed
, right_speed
); }
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
;
151 if(backup_duration
> allowed_backup_duration
) {
152 stay_on_table_state
= Stay_on_table_state::start_turning
; } }
154 void turning(unsigned long start_turning_time
) {
155 Serial
.print("turning ");
156 // the exact amount of time for turning around might need
157 // twerking for your robot. the default value is 3200
158 static const unsigned int allowed_turning_duration
= 2500;
159 unsigned long now
= millis();
160 unsigned long turning_duration
= now
- start_turning_time
;
164 if(turning_duration
> allowed_turning_duration
) {
165 stay_on_table_state
= Stay_on_table_state::going
; } }
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;
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
);
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
);
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
);
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;
201 unsigned int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
202 unsigned int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
204 left_speed
= actual_left_ping_time
- desired_left_ping_time
;
205 right_speed
= actual_right_ping_time
- desired_right_ping_time
;
207 Serial
.print(", left: ping = ");
208 Serial
.print(actual_left_ping_time
);
209 Serial
.print(" speed = ");
210 Serial
.print(left_speed
);
211 Serial
.print(" right: ping = ");
212 Serial
.print(actual_right_ping_time
);
213 Serial
.print(" speed = ");
214 Serial
.print(right_speed
);
216 go(left_speed
, right_speed
); }
218 enum class Behavior
{stay_on_table
, follow
};
219 Behavior behavior
= Behavior::stay_on_table
;
221 enum class Button_state
{up
, down
};
223 Button_state prior_button_state
= Button_state::up
;
226 static int count
= 0;
231 Button_state button_state
=
232 (digitalRead(button_pin
) == HIGH
)
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; }
239 // if button was just pressed
240 if (prior_button_state
== Button_state::up
241 && button_state
== Button_state::down
) {
242 // indicate button press recognized
244 // turn off motors, to allow robot to be set down
248 case Behavior::stay_on_table
: behavior
= Behavior::follow
; break;
249 case Behavior::follow
: behavior
= Behavior::stay_on_table
; break; }
253 case Behavior::stay_on_table
: stay_on_table(); break;
254 case Behavior:: follow
: follow(); break; }
256 prior_button_state
= button_state
;