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 // right side LED, white wire
17 int right_led_pin
= 3;
19 // define which pins are connected to which components
21 int right_motor_forward_pin
= 2;
23 int right_motor_backward_pin
= 4;
25 int right_motor_speed_pin
= 5;
28 int right_echo_pin
= 6;
30 int right_trigger_pin
= 7;
33 int left_motor_forward_pin
= 8;
35 int left_motor_backward_pin
= 13;
37 int left_motor_speed_pin
= 10;
40 int left_echo_pin
= 11;
42 int left_trigger_pin
= 12;
44 // left side LED, blue wire
48 digitalWrite(pin
, HIGH
);}
51 digitalWrite(pin
, LOW
);}
53 void set_motor(int speed_pin
,
67 // since speed has been set positive, no need to check if speed < -255.
70 analogWrite(speed_pin
, speed
);}
72 void go(int left_motor_speed
, int right_motor_speed
){
73 set_motor(left_motor_speed_pin
,
74 left_motor_forward_pin
,
75 left_motor_backward_pin
,
77 set_motor(right_motor_speed_pin
,
78 right_motor_forward_pin
,
79 right_motor_backward_pin
,
82 int ping(int trigger
, int echo
){
87 // turn on the trigger and leave it on long enough for the
88 // sonar sensor to notice
90 delayMicroseconds(10);
92 ping_time
= pulseIn(echo
, HIGH
);
95 // sonar needs some time to recover before pinging again,
96 // so make sure it gets enough sleep right here. 50 milliseconds
103 pinMode(right_motor_speed_pin
, OUTPUT
);
104 pinMode(right_motor_forward_pin
, OUTPUT
);
105 pinMode(right_motor_backward_pin
, OUTPUT
);
107 pinMode(right_echo_pin
, INPUT
);
108 pinMode(right_trigger_pin
, OUTPUT
);
110 pinMode(left_motor_speed_pin
, OUTPUT
);
111 pinMode(left_motor_forward_pin
, OUTPUT
);
112 pinMode(left_motor_backward_pin
, OUTPUT
);
114 pinMode(left_echo_pin
, INPUT
);
115 pinMode(left_trigger_pin
, OUTPUT
);
117 off(left_motor_speed_pin
);
118 off(left_motor_forward_pin
);
119 off(left_motor_backward_pin
);
120 off(left_trigger_pin
);
122 off(right_motor_speed_pin
);
123 off(right_motor_forward_pin
);
124 off(right_motor_backward_pin
);
125 off(right_trigger_pin
); }
131 // you'll need to adjust these based on your sonar sensor's behavior
132 int desired_right_ping_time
= 800;
133 int desired_left_ping_time
= 800;
135 unsigned int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
136 unsigned int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
138 int left_led_value
= (int)(actual_left_ping_time
/ 16.0);
139 int right_led_value
= (int)(actual_right_ping_time
/ 16.0);
141 Serial
.print("left led value = ");
142 Serial
.print(left_led_value
);
143 Serial
.print(", right led value = ");
144 Serial
.print(right_led_value
);
146 analogWrite(left_led_pin
, left_led_value
);
147 analogWrite(right_led_pin
, right_led_value
);
149 left_speed
= actual_left_ping_time
- desired_left_ping_time
;
150 right_speed
= actual_right_ping_time
- desired_right_ping_time
;
152 Serial
.print(", left: ping = ");
153 Serial
.print(actual_left_ping_time
);
154 Serial
.print(" speed = ");
155 Serial
.print(left_speed
);
156 Serial
.print(" right: ping = ");
157 Serial
.print(actual_right_ping_time
);
158 Serial
.print(" speed = ");
159 Serial
.print(right_speed
);
162 go(left_speed
, right_speed
); }