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
18 int right_motor_forward_pin
= 3;
20 int right_motor_backward_pin
= 4;
22 int right_motor_speed_pin
= 5;
25 int right_echo_pin
= 6;
27 int right_trigger_pin
= 7;
30 int left_motor_forward_pin
= 8;
32 int left_motor_backward_pin
= 9;
34 int left_motor_speed_pin
= 10;
37 int left_echo_pin
= 11;
39 int left_trigger_pin
= 12;
42 digitalWrite(pin
, HIGH
);}
45 digitalWrite(pin
, LOW
);}
47 void set_motor(int speed_pin
,
61 // since speed has been set positive, no need to check if speed < -255.
64 analogWrite(speed_pin
, speed
);}
66 void go(int left_motor_speed
, int right_motor_speed
){
67 set_motor(left_motor_speed_pin
,
68 left_motor_forward_pin
,
69 left_motor_backward_pin
,
71 set_motor(right_motor_speed_pin
,
72 right_motor_forward_pin
,
73 right_motor_backward_pin
,
76 int ping(int trigger
, int echo
){
81 // turn on the trigger and leave it on long enough for the
82 // sonar sensor to notice
84 delayMicroseconds(10);
86 ping_time
= pulseIn(echo
, HIGH
);
89 // sonar needs some time to recover before pinging again,
90 // so make sure it gets enough sleep right here. 50 milliseconds
94 void backup(int backup_time
){
98 void turn_around(int turn_around_time
){
100 delay(turn_around_time
);}
105 pinMode(right_motor_speed_pin
, OUTPUT
);
106 pinMode(right_motor_forward_pin
, OUTPUT
);
107 pinMode(right_motor_backward_pin
, OUTPUT
);
109 pinMode(right_echo_pin
, INPUT
);
110 pinMode(right_trigger_pin
, OUTPUT
);
112 pinMode(left_motor_speed_pin
, OUTPUT
);
113 pinMode(left_motor_forward_pin
, OUTPUT
);
114 pinMode(left_motor_backward_pin
, OUTPUT
);
116 pinMode(left_echo_pin
, INPUT
);
117 pinMode(left_trigger_pin
, OUTPUT
);
119 off(left_motor_speed_pin
);
120 off(left_motor_forward_pin
);
121 off(left_motor_backward_pin
);
122 off(left_trigger_pin
);
124 off(right_motor_speed_pin
);
125 off(right_motor_forward_pin
);
126 off(right_motor_backward_pin
);
127 off(right_trigger_pin
);}
133 int forward_speed
= 250;
136 // adjust this number as necessary for your robot.
137 // it represents how far the table is from your sonar sensor.
138 // larger values mean larger distance. default is 800
139 int right_max_ping_time_over_table
= 800;
140 int left_max_ping_time_over_table
= 800;
141 int backup_time
= 2000;
142 // the exact amount of time for turning around might need
143 // twerking for your robot. the default value is 3200
144 int turn_around_time
= 3200;
146 int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
147 int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
149 Serial
.print("left ping = ");
150 Serial
.print(actual_left_ping_time
);
151 Serial
.print("\tright_ping = ");
152 Serial
.println(actual_right_ping_time
);
154 // if the left sonar senses a table, keep driving left side forward,
155 // otherwise, stop left side
156 if(actual_left_ping_time
< left_max_ping_time_over_table
){
157 left_speed
= forward_speed
;}
159 left_speed
= stop_speed
;}
160 // if the right sonar senses a table, keep driving right side forward,
161 // otherwise, stop right side
162 if(actual_right_ping_time
< right_max_ping_time_over_table
){
163 right_speed
= forward_speed
;}
165 right_speed
= stop_speed
;}
167 // if both sonars detect being off the table, backup and turn around
168 // otherwise, go the correct speed for each wheel
169 if(actual_left_ping_time
>= left_max_ping_time_over_table
170 && actual_right_ping_time
>= right_max_ping_time_over_table
){
171 Serial
.println("backing up");
173 Serial
.println("turning around");
174 turn_around(turn_around_time
);}
176 Serial
.println("going");
177 go(left_speed
, right_speed
);}}