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/>.
19 digitalWrite(pin
, HIGH
);}
22 digitalWrite(pin
, LOW
);}
24 void set_motor(int speed_pin
,
38 // since speed has been set positive, no need to check if speed < -255.
41 analogWrite(speed_pin
, speed
);}
43 void go(int left_motor_speed
, int right_motor_speed
){
44 set_motor(left_motor_speed_pin
,
45 left_motor_forward_pin
,
46 left_motor_backward_pin
,
48 set_motor(right_motor_speed_pin
,
49 right_motor_forward_pin
,
50 right_motor_backward_pin
,
53 int ping(int trigger
, int echo
){
58 // turn on the trigger and leave it on long enough for the
59 // sonar sensor to notice
61 delayMicroseconds(10);
63 ping_time
= pulseIn(echo
, HIGH
);
66 // sonar needs some time to recover before pinging again,
67 // so make sure it gets enough sleep right here. 50 milliseconds
71 void backup(int backup_time
){
75 void turn_around(int turn_around_time
){
77 delay(turn_around_time
);}
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
);}
110 int forward_speed
= 250;
113 // adjust this number as necessary for your robot.
114 // it represents how far the table is from your sonar sensor.
115 // larger values mean larger distance. default is 800
116 int right_max_ping_time_over_table
= 800;
117 int left_max_ping_time_over_table
= 800;
118 int backup_time
= 1000;
119 // the exact amount of time for turning around might need
120 // twerking for your robot. the default value is 3200
121 int turn_around_time
= 3200;
123 int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
124 int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
126 /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
127 /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
129 /* Serial.print("left led value = "); */
130 /* Serial.print(left_led_value); */
131 /* Serial.print(", right led value = "); */
132 /* Serial.print(right_led_value); */
134 /* analogWrite(left_led_pin, left_led_value); */
135 /* analogWrite(right_led_pin, right_led_value); */
137 Serial
.print("left ping = ");
138 Serial
.print(actual_left_ping_time
);
139 Serial
.print("\tright_ping = ");
140 Serial
.println(actual_right_ping_time
);
142 // if the left sonar senses a table, keep driving left side forward,
143 // otherwise, stop left side
144 if(actual_left_ping_time
< left_max_ping_time_over_table
){
145 left_speed
= forward_speed
;}
147 left_speed
= stop_speed
;}
148 // if the right sonar senses a table, keep driving right side forward,
149 // otherwise, stop right side
150 if(actual_right_ping_time
< right_max_ping_time_over_table
){
151 right_speed
= forward_speed
;}
153 right_speed
= stop_speed
;}
155 // if both sonars detect being off the table, backup and turn around
156 // otherwise, go the correct speed for each wheel
157 if(actual_left_ping_time
>= left_max_ping_time_over_table
158 && actual_right_ping_time
>= right_max_ping_time_over_table
){
159 Serial
.println("backing up");
161 Serial
.println("turning around");
162 turn_around(turn_around_time
);}
164 Serial
.println("going");
165 go(left_speed
, right_speed
);}}