| 1 | /* |
| 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. |
| 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 | */ |
| 15 | |
| 16 | // use pin 13's LED to indicate intended travel direction. |
| 17 | // on == forward, off == backward |
| 18 | int led = 13; |
| 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 | } |
| 37 | |
| 38 | // the setup routine runs once when you press reset: |
| 39 | void setup() { |
| 40 | // initialize the digital pin as an output. |
| 41 | setupMotor(leftMotorEnable, leftMotorA, leftMotorB); |
| 42 | setupMotor(rightMotorEnable, rightMotorA, rightMotorB); |
| 43 | |
| 44 | pinMode(led, OUTPUT); |
| 45 | } |
| 46 | |
| 47 | void motorsRun(int left, int right, int msDelay) { |
| 48 | // Set left motor direction: |
| 49 | if (left > 0) { |
| 50 | // Set left motor to go forward: |
| 51 | digitalWrite(leftMotorA, HIGH); |
| 52 | digitalWrite(leftMotorB, LOW); |
| 53 | } else { |
| 54 | // Set left motor to go backward: |
| 55 | digitalWrite(leftMotorA, LOW); |
| 56 | digitalWrite(leftMotorB, HIGH); |
| 57 | left = -left; // Make left a positive value: |
| 58 | } |
| 59 | |
| 60 | analogWrite(rightMotorEnable, left); // Start motor in right direction |
| 61 | // Set left motor direction: |
| 62 | if (right > 0) { |
| 63 | // Set right motor to go forward: |
| 64 | digitalWrite(rightMotorA, HIGH); |
| 65 | digitalWrite(rightMotorB, LOW); |
| 66 | } else { |
| 67 | // Set right motor to go backward: |
| 68 | digitalWrite(rightMotorA, LOW); |
| 69 | digitalWrite(rightMotorB, HIGH); |
| 70 | right = -right; // Make right a positive value: |
| 71 | } |
| 72 | analogWrite(leftMotorEnable, left); // Start motor in right direction |
| 73 | |
| 74 | delay(msDelay); // Wait the specified amount of time |
| 75 | |
| 76 | // Stop both motors: |
| 77 | analogWrite(leftMotorEnable, 0); |
| 78 | analogWrite(rightMotorEnable, 0); |
| 79 | } |
| 80 | |
| 81 | // the loop routine runs over and over again forever: |
| 82 | void loop() { |
| 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 | } |