+++ /dev/null
-/*
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU Affero General Public License as
- published by the Free Software Foundation, either version 3 of the
- License, or (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU Affero General Public License for more details.
-
- You should have received a copy of the GNU Affero General Public License
- along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-// use pin 13's LED to indicate intended travel direction.
-// on == forward, off == backward
-int led = 13;
-
-int leftMotorEnable = 10;
-int leftMotorA = 9;
-int leftMotorB = 8;
-
-int rightMotorEnable = 3;
-int rightMotorA = 4;
-int rightMotorB = 5;
-
-void setupMotor(int motorEnable, int motorA, int motorB){
- pinMode(motorEnable, OUTPUT);
- pinMode(motorA, OUTPUT);
- pinMode(motorB, OUTPUT);
-
- digitalWrite(motorEnable, LOW);
- digitalWrite(motorA, LOW);
- digitalWrite(motorB, LOW);
-}
-
-// the setup routine runs once when you press reset:
-void setup() {
- // initialize the digital pin as an output.
- setupMotor(leftMotorEnable, leftMotorA, leftMotorB);
- setupMotor(rightMotorEnable, rightMotorA, rightMotorB);
-
- pinMode(led, OUTPUT);
-}
-
-void motorsRun(int left, int right, int msDelay) {
- // Set left motor direction:
- if (left > 0) {
- // Set left motor to go forward:
- digitalWrite(leftMotorA, HIGH);
- digitalWrite(leftMotorB, LOW);
- } else {
- // Set left motor to go backward:
- digitalWrite(leftMotorA, LOW);
- digitalWrite(leftMotorB, HIGH);
- left = -left; // Make left a positive value:
- }
-
- analogWrite(rightMotorEnable, left); // Start motor in right direction
- // Set left motor direction:
- if (right > 0) {
- // Set right motor to go forward:
- digitalWrite(rightMotorA, HIGH);
- digitalWrite(rightMotorB, LOW);
- } else {
- // Set right motor to go backward:
- digitalWrite(rightMotorA, LOW);
- digitalWrite(rightMotorB, HIGH);
- right = -right; // Make right a positive value:
- }
- analogWrite(leftMotorEnable, left); // Start motor in right direction
-
- delay(msDelay); // Wait the specified amount of time
-
- // Stop both motors:
- analogWrite(leftMotorEnable, 0);
- analogWrite(rightMotorEnable, 0);
-}
-
-// the loop routine runs over and over again forever:
-void loop() {
- digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level)
- motorsRun(100, 100, 2000);// Run the robot forward
- digitalWrite(led, LOW); // turn the LED off by making the voltage LOW
- motorsRun(-100, -100, 2000); // Run the robot backward
-}