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 int motor1_enable
= 10;
25 int motor2_enable
= 3;
30 int left_trigger
= 12;
32 int right_trigger
= 7;
35 // the setup routine runs once when you press reset:
39 // initialize the digital pin as an output.
40 pinMode(motor1_enable
, OUTPUT
);
41 pinMode(motor1a
, OUTPUT
);
42 pinMode(motor1b
, OUTPUT
);
43 pinMode(motor2_enable
, OUTPUT
);
44 pinMode(motor2a
, OUTPUT
);
45 pinMode(motor2b
, OUTPUT
);
48 pinMode(left_trigger
, OUTPUT
);
49 pinMode(left_echo
, INPUT
);
50 pinMode(right_trigger
, OUTPUT
);
51 pinMode(right_echo
, INPUT
);
53 digitalWrite(motor1_enable
, LOW
);
54 digitalWrite(motor1a
, LOW
);
55 digitalWrite(motor1b
, LOW
);
56 digitalWrite(motor2_enable
, LOW
);
57 digitalWrite(motor2a
, LOW
);
58 digitalWrite(motor2b
, LOW
);
64 digitalWrite (pin
, HIGH
);
70 digitalWrite (pin
, LOW
);
74 ping(int trigger
, int echo
)
78 delayMicroseconds(12);
80 ping_time
= pulseIn (echo
, HIGH
);
86 void motorsRun(int left
, int right
, int ms_delay
) {
87 // Set left motor direction:
89 // Set left motor to go forward:
90 digitalWrite(motor1a
, HIGH
);
91 digitalWrite(motor1b
, LOW
);
93 // Set left motor to go backward:
94 digitalWrite(motor1a
, LOW
);
95 digitalWrite(motor1b
, HIGH
);
96 left
= -left
; // Make left a positive value:
98 analogWrite(motor1_enable
, left
); // Start motor in right direction
100 // Set left motor direction:
102 // Set right motor to go forward:
103 digitalWrite(motor2a
, HIGH
);
104 digitalWrite(motor2b
, LOW
);
106 // Set right motor to go backward:
107 digitalWrite(motor2a
, LOW
);
108 digitalWrite(motor2b
, HIGH
);
109 right
= -right
; // Make right a positive value:
111 analogWrite(motor2_enable
, right
); // Start motor in right direction
113 delay(ms_delay
); // Wait the specified amount of time
116 // the loop routine runs over and over again forever:
123 int left_ping
= ping (left_trigger
, left_echo
);
124 int right_ping
= ping (right_trigger
, right_echo
);
127 Serial.print ("left ping = ");
128 Serial.print (left_ping);
130 Serial.print (" right ping = ");
131 Serial.println (right_ping);
139 if (right_ping
< 400)
144 if (left_speed
== 0 && right_speed
== 0)
147 motorsRun(-250, -250, 2000);
149 motorsRun(-250, 250, 3200);
153 motorsRun(left_speed
, right_speed
, 0);