/*
 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/>.
 */
/*
  Motor.
  Runs both motors back and forth.

  This example code is in the public domain.
 */

// Pin 13 has an LED connected on most Arduino boards.
// give it a name:
int led = 13;
int motor1_enable = 10;
int motor1a = 9;
int motor1b = 8;
int motor2_enable = 3;
int motor2a = 4;
int motor2b = 5;

// the setup routine runs once when you press reset:
void setup() {
  // initialize the digital pin as an output.
  pinMode(led, OUTPUT);
  pinMode(motor1_enable, OUTPUT);
  pinMode(motor1a, OUTPUT);
  pinMode(motor1b, OUTPUT);
  pinMode(motor2_enable, OUTPUT);
  pinMode(motor2a, OUTPUT);
  pinMode(motor2b, OUTPUT);

  digitalWrite(led, HIGH);
  digitalWrite(motor1_enable, LOW);
  digitalWrite(motor1a, LOW);
  digitalWrite(motor1b, LOW);
  digitalWrite(motor2_enable, LOW);
  digitalWrite(motor2a, LOW);
  digitalWrite(motor2b, LOW);
}

void motorsRun(int left, int right, int ms_delay) {
  // Set left motor direction:
  if (left > 0) {
    // Set left motor to go forward:
    digitalWrite(motor1a, HIGH);
    digitalWrite(motor1b, LOW);
  } else {
    // Set left motor to go backward:
    digitalWrite(motor1a, LOW);
    digitalWrite(motor1b, HIGH);
    left = -left;                   // Make left a positive value:
  }

  analogWrite(motor2_enable, left); // Start motor in right direction
  // Set left motor direction:
  if (right > 0) {
    // Set right motor to go forward:
    digitalWrite(motor2a, HIGH);
    digitalWrite(motor2b, LOW);
  } else {
    // Set right motor to go backward:
    digitalWrite(motor2a, LOW);
    digitalWrite(motor2b, HIGH);
    right = -right;                 // Make right a positive value:
  }
  analogWrite(motor1_enable, left); // Start motor in right direction

  delay(ms_delay);                  // Wait the specified amount of time

  // Stop both motors:
  analogWrite(motor1_enable, 0);
  analogWrite(motor2_enable, 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
}