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.
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.
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/>.
16 // define which pins are connected to which components
17 int right_motor_forward_pin
= 3;
18 int right_motor_backward_pin
= 4;
19 int right_motor_speed_pin
= 5;
21 int follow_right_echo_pin
= 6;
22 int follow_right_trigger_pin
= 7;
24 int left_motor_forward_pin
= 8;
25 int left_motor_backward_pin
= 9;
26 int left_motor_speed_pin
= 10;
28 int follow_left_echo_pin
= 11;
29 int follow_left_trigger_pin
= 12;
31 int table_right_echo_pin
= 13;
32 int table_right_trigger_pin
= 2;
34 // you'll need to adjust these based on your sonar sensor's behavior
35 int desired_right_follow_ping_time
= 800;
36 int desired_left_follow_ping_time
= 800;
38 int right_max_ping_time_over_table
= 500;
41 digitalWrite(pin
, HIGH
);}
44 digitalWrite(pin
, LOW
);}
46 void set_motor(int speed_pin
,
60 // since speed has been set positive, no need to check if speed < -255.
63 analogWrite(speed_pin
, speed
);}
65 void go(int left_motor_speed
, int right_motor_speed
){
66 set_motor(left_motor_speed_pin
,
67 left_motor_forward_pin
,
68 left_motor_backward_pin
,
70 set_motor(right_motor_speed_pin
,
71 right_motor_forward_pin
,
72 right_motor_backward_pin
,
75 int ping(int trigger
, int echo
){
80 // turn on the trigger and leave it on long enough for the
81 // sonar sensor to notice
83 delayMicroseconds(10);
85 ping_time
= pulseIn(echo
, HIGH
);
88 // sonar needs some time to recover before pinging again,
89 // so make sure it gets enough sleep right here. 50 milliseconds
93 void backup(int backup_time
){
97 void turn_around(int turn_around_time
){
99 delay(turn_around_time
);}
104 pinMode(right_motor_speed_pin
, OUTPUT
);
105 pinMode(right_motor_forward_pin
, OUTPUT
);
106 pinMode(right_motor_backward_pin
, OUTPUT
);
108 pinMode(follow_right_echo_pin
, INPUT
);
109 pinMode(follow_right_trigger_pin
, OUTPUT
);
111 pinMode(table_right_echo_pin
, INPUT
);
112 pinMode(table_right_trigger_pin
, OUTPUT
);
114 pinMode(left_motor_speed_pin
, OUTPUT
);
115 pinMode(left_motor_forward_pin
, OUTPUT
);
116 pinMode(left_motor_backward_pin
, OUTPUT
);
118 pinMode(follow_left_echo_pin
, INPUT
);
119 pinMode(follow_left_trigger_pin
, OUTPUT
);
121 off(left_motor_speed_pin
);
122 off(left_motor_forward_pin
);
123 off(left_motor_backward_pin
);
124 off(follow_left_trigger_pin
);
126 off(right_motor_speed_pin
);
127 off(right_motor_forward_pin
);
128 off(right_motor_backward_pin
);
129 off(follow_right_trigger_pin
);}
132 int wall_right_min_ping_time
= desired_right_follow_ping_time
- 50;
133 int wall_right_max_ping_time
= desired_right_follow_ping_time
+ 50;
134 int wall_left_min_ping_time
= desired_left_follow_ping_time
- 50;
135 int wall_left_max_ping_time
= desired_left_follow_ping_time
+ 50;
136 int left_follow_speed
;
137 int right_follow_speed
;
139 unsigned int actual_left_ping_time
=
140 ping(follow_left_trigger_pin
, follow_left_echo_pin
);
141 unsigned int actual_right_ping_time
=
142 ping(follow_right_trigger_pin
, follow_right_echo_pin
);
144 unsigned int actual_right_table_ping_time
=
145 ping(table_right_trigger_pin
, table_right_echo_pin
);
147 left_follow_speed
= actual_left_ping_time
- desired_left_follow_ping_time
;
148 right_follow_speed
= actual_right_ping_time
- desired_right_follow_ping_time
;
150 Serial
.print("left: ping = ");
151 Serial
.print(actual_left_ping_time
);
152 Serial
.print(" speed = ");
153 Serial
.print(left_follow_speed
);
154 Serial
.print(" right: ping = ");
155 Serial
.print(actual_right_ping_time
);
156 Serial
.print(" speed = ");
157 Serial
.println(right_follow_speed
);
160 actual_right_table_ping_time
< right_max_ping_time_over_table
;
161 // if the actual ping time is in between the min and max ping time,
162 // then the robot is at the desired distance from the wall.
163 boolean left_follow_sonar_desired_distance
=
164 (wall_left_min_ping_time
< actual_left_ping_time
165 && actual_left_ping_time
< wall_left_max_ping_time
);
166 boolean right_follow_sonar_desired_distance
=
167 (wall_right_min_ping_time
< actual_right_ping_time
168 && actual_right_ping_time
< wall_right_max_ping_time
);
169 // if the left and right pings show the desired distance,
170 // the robot is facing the wall
171 boolean facing_wall
=
172 left_follow_sonar_desired_distance
&& right_follow_sonar_desired_distance
;
178 else{ // not facing wall
179 go(left_follow_speed
, right_follow_speed
);}}
180 else{ // not over table