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/>.
17 Runs both motors back and forth.
19 This example code is in the public domain.
22 // Pin 13 has an LED connected on most Arduino boards.
25 int motor1_enable
= 10;
28 int motor2_enable
= 3;
33 int left_trigger
= 12;
35 int right_trigger
= 7;
38 // the setup routine runs once when you press reset:
42 // initialize the digital pin as an output.
44 pinMode(motor1_enable
, OUTPUT
);
45 pinMode(motor1a
, OUTPUT
);
46 pinMode(motor1b
, OUTPUT
);
47 pinMode(motor2_enable
, OUTPUT
);
48 pinMode(motor2a
, OUTPUT
);
49 pinMode(motor2b
, OUTPUT
);
52 pinMode(left_trigger
, OUTPUT
);
53 pinMode(left_echo
, INPUT
);
54 pinMode(right_trigger
, OUTPUT
);
55 pinMode(right_echo
, INPUT
);
57 digitalWrite(led
, HIGH
);
58 digitalWrite(motor1_enable
, LOW
);
59 digitalWrite(motor1a
, LOW
);
60 digitalWrite(motor1b
, LOW
);
61 digitalWrite(motor2_enable
, LOW
);
62 digitalWrite(motor2a
, LOW
);
63 digitalWrite(motor2b
, LOW
);
69 digitalWrite (pin
, HIGH
);
75 digitalWrite (pin
, LOW
);
79 ping(int trigger
, int echo
)
83 delayMicroseconds(12);
85 ping_time
= pulseIn (echo
, HIGH
);
91 void motorsRun(int left
, int right
, int ms_delay
) {
92 // Set left motor direction:
94 // Set left motor to go forward:
95 digitalWrite(motor1a
, HIGH
);
96 digitalWrite(motor1b
, LOW
);
98 // Set left motor to go backward:
99 digitalWrite(motor1a
, LOW
);
100 digitalWrite(motor1b
, HIGH
);
101 left
= -left
; // Make left a positive value:
103 analogWrite(motor1_enable
, left
); // Start motor in right direction
105 // Set left motor direction:
107 // Set right motor to go forward:
108 digitalWrite(motor2a
, HIGH
);
109 digitalWrite(motor2b
, LOW
);
111 // Set right motor to go backward:
112 digitalWrite(motor2a
, LOW
);
113 digitalWrite(motor2b
, HIGH
);
114 right
= -right
; // Make right a positive value:
116 analogWrite(motor2_enable
, right
); // Start motor in right direction
118 delay(ms_delay
); // Wait the specified amount of time
121 // the loop routine runs over and over again forever:
128 int left_ping
= ping (left_trigger
, left_echo
);
129 int right_ping
= ping (right_trigger
, right_echo
);
132 Serial.print ("left ping = ");
133 Serial.print (left_ping);
135 Serial.print (" right ping = ");
136 Serial.println (right_ping);
144 if (right_ping
< 400)
149 if (left_speed
== 0 && right_speed
== 0)
152 motorsRun(-250, -250, 2000);
154 motorsRun(-250, 250, 3200);
158 motorsRun(left_speed
, right_speed
, 0);