a77b88c5 |
1 | /* |
f29767e8 |
2 | This program is free software: you can redistribute it and/or modify |
a77b88c5 |
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. |
6 | |
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. |
11 | |
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/>. |
14 | */ |
a77b88c5 |
15 | |
c8ce6466 |
16 | // use pin 13's LED to indicate intended travel direction. |
17 | // on == forward, off == backward |
a77b88c5 |
18 | int led = 13; |
c8ce6466 |
19 | |
20 | int leftMotorEnable = 10; |
21 | int leftMotorA = 9; |
22 | int leftMotorB = 8; |
23 | |
24 | int rightMotorEnable = 3; |
25 | int rightMotorA = 4; |
26 | int rightMotorB = 5; |
27 | |
28 | void setupMotor(int motorEnable, int motorA, int motorB){ |
29 | pinMode(motorEnable, OUTPUT); |
30 | pinMode(motorA, OUTPUT); |
31 | pinMode(motorB, OUTPUT); |
32 | |
33 | digitalWrite(motorEnable, LOW); |
34 | digitalWrite(motorA, LOW); |
35 | digitalWrite(motorB, LOW); |
36 | } |
a77b88c5 |
37 | |
38 | // the setup routine runs once when you press reset: |
39 | void setup() { |
40 | // initialize the digital pin as an output. |
c8ce6466 |
41 | setupMotor(leftMotorEnable, leftMotorA, leftMotorB); |
42 | setupMotor(rightMotorEnable, rightMotorA, rightMotorB); |
a77b88c5 |
43 | |
c8ce6466 |
44 | pinMode(led, OUTPUT); |
a77b88c5 |
45 | } |
46 | |
c8ce6466 |
47 | void motorsRun(int left, int right, int msDelay) { |
a77b88c5 |
48 | // Set left motor direction: |
49 | if (left > 0) { |
50 | // Set left motor to go forward: |
c8ce6466 |
51 | digitalWrite(leftMotorA, HIGH); |
52 | digitalWrite(leftMotorB, LOW); |
a77b88c5 |
53 | } else { |
54 | // Set left motor to go backward: |
c8ce6466 |
55 | digitalWrite(leftMotorA, LOW); |
56 | digitalWrite(leftMotorB, HIGH); |
a77b88c5 |
57 | left = -left; // Make left a positive value: |
58 | } |
59 | |
c8ce6466 |
60 | analogWrite(rightMotorEnable, left); // Start motor in right direction |
a77b88c5 |
61 | // Set left motor direction: |
62 | if (right > 0) { |
63 | // Set right motor to go forward: |
c8ce6466 |
64 | digitalWrite(rightMotorA, HIGH); |
65 | digitalWrite(rightMotorB, LOW); |
a77b88c5 |
66 | } else { |
67 | // Set right motor to go backward: |
c8ce6466 |
68 | digitalWrite(rightMotorA, LOW); |
69 | digitalWrite(rightMotorB, HIGH); |
a77b88c5 |
70 | right = -right; // Make right a positive value: |
71 | } |
c8ce6466 |
72 | analogWrite(leftMotorEnable, left); // Start motor in right direction |
a77b88c5 |
73 | |
c8ce6466 |
74 | delay(msDelay); // Wait the specified amount of time |
a77b88c5 |
75 | |
76 | // Stop both motors: |
c8ce6466 |
77 | analogWrite(leftMotorEnable, 0); |
78 | analogWrite(rightMotorEnable, 0); |
a77b88c5 |
79 | } |
80 | |
81 | // the loop routine runs over and over again forever: |
82 | void loop() { |
a77b88c5 |
83 | digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level) |
84 | motorsRun(100, 100, 2000);// Run the robot forward |
85 | digitalWrite(led, LOW); // turn the LED off by making the voltage LOW |
86 | motorsRun(-100, -100, 2000); // Run the robot backward |
87 | } |