1 // license info at bottom of file
7 digitalWrite(pin
, HIGH
);}
10 digitalWrite(pin
, LOW
);}
12 void set_motor(int speed_pin
,
26 // since speed has been set positive, no need to check if speed < -255.
29 analogWrite(speed_pin
, speed
);}
31 void go(int left_motor_speed
, int right_motor_speed
){
32 set_motor(left_motor_speed_pin
,
33 left_motor_forward_pin
,
34 left_motor_backward_pin
,
36 set_motor(right_motor_speed_pin
,
37 right_motor_forward_pin
,
38 right_motor_backward_pin
,
41 int ping(int trigger
, int echo
){
46 // turn on the trigger and leave it on long enough for the
47 // sonar sensor to notice
49 delayMicroseconds(10);
51 ping_time
= pulseIn(echo
, HIGH
);
54 // sonar needs some time to recover before pinging again,
55 // so make sure it gets enough sleep right here. 50 milliseconds
62 pinMode(right_motor_speed_pin
, OUTPUT
);
63 pinMode(right_motor_forward_pin
, OUTPUT
);
64 pinMode(right_motor_backward_pin
, OUTPUT
);
66 pinMode(right_echo_pin
, INPUT
);
67 pinMode(right_trigger_pin
, OUTPUT
);
69 pinMode(left_motor_speed_pin
, OUTPUT
);
70 pinMode(left_motor_forward_pin
, OUTPUT
);
71 pinMode(left_motor_backward_pin
, OUTPUT
);
73 pinMode(left_echo_pin
, INPUT
);
74 pinMode(left_trigger_pin
, OUTPUT
);
76 off(left_motor_speed_pin
);
77 off(left_motor_forward_pin
);
78 off(left_motor_backward_pin
);
79 off(left_trigger_pin
);
81 off(right_motor_speed_pin
);
82 off(right_motor_forward_pin
);
83 off(right_motor_backward_pin
);
84 off(right_trigger_pin
); }
90 // you'll need to adjust these based on your sonar sensor's behavior
91 int desired_right_ping_time
= 800;
92 int desired_left_ping_time
= 800;
94 unsigned int actual_left_ping_time
= ping(left_trigger_pin
, left_echo_pin
);
95 unsigned int actual_right_ping_time
= ping(right_trigger_pin
, right_echo_pin
);
97 /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
98 /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
100 /* analogWrite(left_led_pin, left_led_value); */
101 /* analogWrite(right_led_pin, right_led_value); */
103 /* Serial.print("left led value = "); */
104 /* Serial.print(left_led_value); */
105 /* Serial.print(", right led value = "); */
106 /* Serial.print(right_led_value); */
108 left_speed
= actual_left_ping_time
- desired_left_ping_time
;
109 right_speed
= actual_right_ping_time
- desired_right_ping_time
;
111 Serial
.print(", left: ping = ");
112 Serial
.print(actual_left_ping_time
);
113 Serial
.print(" speed = ");
114 Serial
.print(left_speed
);
115 Serial
.print(" right: ping = ");
116 Serial
.print(actual_right_ping_time
);
117 Serial
.print(" speed = ");
118 Serial
.print(right_speed
);
121 go(left_speed
, right_speed
); }
124 This program is free software: you can redistribute it and/or modify
125 it under the terms of the GNU Affero General Public License as
126 published by the Free Software Foundation, either version 3 of the
127 License, or (at your option) any later version.
129 This program is distributed in the hope that it will be useful,
130 but WITHOUT ANY WARRANTY; without even the implied warranty of
131 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
132 GNU Affero General Public License for more details.
134 You should have received a copy of the GNU Affero General Public License
135 along with this program. If not, see <http://www.gnu.org/licenses/>.