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/>.
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
72 void backup(int backup_time
){
76 void turn_around(int turn_around_time
){
78 delay(turn_around_time
);}
83 pinMode(LED_BUILTIN
, OUTPUT
);
85 pinMode(button_pin
, INPUT_PULLUP
);
87 pinMode(right_motor_speed_pin
, OUTPUT
);
88 pinMode(right_motor_forward_pin
, OUTPUT
);
89 pinMode(right_motor_backward_pin
, OUTPUT
);
91 pinMode(right_echo_pin
, INPUT
);
92 pinMode(right_trigger_pin
, OUTPUT
);
94 pinMode(left_motor_speed_pin
, OUTPUT
);
95 pinMode(left_motor_forward_pin
, OUTPUT
);
96 pinMode(left_motor_backward_pin
, OUTPUT
);
98 pinMode(left_echo_pin
, INPUT
);
99 pinMode(left_trigger_pin
, OUTPUT
);
101 off(left_motor_speed_pin
);
102 off(left_motor_forward_pin
);
103 off(left_motor_backward_pin
);
104 off(left_trigger_pin
);
106 off(right_motor_speed_pin
);
107 off(right_motor_forward_pin
);
108 off(right_motor_backward_pin
);
109 off(right_trigger_pin
);}
111 void stay_on_table(){
115 int forward_speed
= 250;
118 // adjust this number as necessary for your robot.
119 // it represents how far the table is from your sonar sensor.
120 // larger values mean larger distance. default is 800
121 int right_max_ping_time_over_table
= 800;
122 int left_max_ping_time_over_table
= 800;
123 int backup_time
= 500;
124 // the exact amount of time for turning around might need
125 // twerking for your robot. the default value is 3200
126 int turn_around_time
= 2500;
128 int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
129 int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
131 /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
132 /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
134 /* Serial.print("left led value = "); */
135 /* Serial.print(left_led_value); */
136 /* Serial.print(", right led value = "); */
137 /* Serial.print(right_led_value); */
139 /* analogWrite(left_led_pin, left_led_value); */
140 /* analogWrite(right_led_pin, right_led_value); */
142 Serial
.print("left ping = ");
143 Serial
.print(actual_left_ping_time
);
144 Serial
.print("\tright_ping = ");
145 Serial
.println(actual_right_ping_time
);
147 // if the left sonar senses a table, keep driving left side forward,
148 // otherwise, stop left side
149 if(actual_left_ping_time
< left_max_ping_time_over_table
){
150 left_speed
= forward_speed
;}
152 left_speed
= stop_speed
;}
153 // if the right sonar senses a table, keep driving right side forward,
154 // otherwise, stop right side
155 if(actual_right_ping_time
< right_max_ping_time_over_table
){
156 right_speed
= forward_speed
;}
158 right_speed
= stop_speed
;}
160 // if both sonars detect being off the table, backup and turn around
161 // otherwise, go the correct speed for each wheel
162 if(actual_left_ping_time
>= left_max_ping_time_over_table
163 && actual_right_ping_time
>= right_max_ping_time_over_table
){
164 Serial
.println("backing up");
166 Serial
.println("turning around");
167 turn_around(turn_around_time
);}
169 Serial
.println("going");
170 go(left_speed
, right_speed
);}}
173 Serial
.println("following!");
177 digitalWrite(LED_BUILTIN
, digitalRead(button_pin
)); }
180 enum Behavior {STAY_ON_TABLE, FOLLOW};
181 Behavior behavior = STAY_ON_TABLE;
183 enum Button_state {UP, DOWN};
184 Button_state prior_button_state = UP;
186 enum Button_state get_button_state() {
188 (digitalRead(button_pin) == LOW)
192 static Button_state button_state = getButtonState();
194 // if button was just pressed
195 if (prior_button_state == UP && button_state == DOWN) {
196 // indicate button press recognized
198 // turn off motors, to allow robot to be set down
202 case STAY_ON_TABLE: behavior = FOLLOW; break;
203 case FOLLOW: behavior = STAY_ON_TABLE; break; }
207 case STAY_ON_TABLE: stay_on_table(); break;
208 case FOLLOW: follow(); break; }
210 prior_button_state = button_state; }