/*

*/

#include <Arduino.h>

//the right motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13;            //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor

//the left motor will be controlled by the motor B pins on the motor driver
const int PWMB = 8;           //speed control pin on the motor driver for the left motor
const int BIN2 = 9;            //control pin 2 on the motor driver for the left motor
const int BIN1 = 10;            //control pin 1 on the motor driver for the left motor

int buttonPin = 7;             //Emergency stop button to turn the robot off suddenly

const int driveTime = 20;      //this is the number of milliseconds that it takes the robot to drive 1 inch
                               //it is set so that if you tell the robot to drive forward 25 units, the robot drives about 25 inches

const int turnTime = 15;        //this is the number of milliseconds that it takes to turn the robot 90 degree
                               //it is set so that if you tell the robot to turn right 90 units, the robot turns about 90 degrees

                               //Note: these numbers will vary a little bit based on how you mount your motors, the friction of the
                               //surface that your driving on, and fluctuations in the power to the motors.
                               //You can change the driveTime and turnTime to make them more accurate

String botDirection;           //the direction that the robot will drive in (this change which direction the two motors spin in)
String distance="40";               //the distance to travel in each direction

/********************************************************************************/
void setup(){
    pinMode(buttonPin, INPUT_PULLUP);   //set this as a input to sense whether the button is pushed

    //set the motor control pins as outputs
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(PWMA, OUTPUT);

    pinMode(BIN1, OUTPUT);
    pinMode(BIN2, OUTPUT);
    pinMode(PWMB, OUTPUT);

    Serial.begin(9600);           //begin serial communication with the computer

}

/********************************************************************************/
void loop(){

    if (digitalRead(buttonPin)==HIGH){

    }  else {
        goForwards();
        delay(1000);
        turnLeft();
        delay(1000);
        goBackwards();
        delay(1000);
        turnRight();
        delay(1000);}
     else{
      while(1){

      }
     }



}

/********************************************************************************/
void rightMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}
/********************************************************************************/
void leftMotor(int motorSpeed)                        //function for driving the left motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(BIN1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}
/********************************************************************************/
void turnLeft(){
    rightMotor(255);                                //drive the right wheel forward
    leftMotor(-200);                                //drive the left wheel forward
    delay(turnTime * distance.toInt());             //drive the motors long enough turn the entered distance
    rightMotor(0);                                  //turn the right motor off
    leftMotor(0);                                   //turn the left motor off
}
/********************************************************************************/
void turnRight(){
    rightMotor(-200);                               //drive the right wheel forward
    leftMotor(255);                                 //drive the left wheel forward
    delay(turnTime * distance.toInt());             //drive the motors long enough turn the entered distance
    rightMotor(0);                                  //turn the right motor off
    leftMotor(0);                                   //turn the left motor off
}
/********************************************************************************/
void goForwards(){
    rightMotor(200);                                //drive the right wheel forward
    leftMotor(200);                                 //drive the left wheel forward
    delay(driveTime * distance.toInt());            //drive the motors long enough travel the entered distance
    rightMotor(0);                                  //turn the right motor off
    leftMotor(0);                                   //turn the left motor off
}
/********************************************************************************/
void goBackwards(){
    rightMotor(-200);                               //drive the right wheel forward
    leftMotor(-200);                                //drive the left wheel forward
    delay(driveTime * distance.toInt());            //drive the motors long enough travel the entered distance
    rightMotor(0);                                  //turn the right motor off
    leftMotor(0);                                   //turn the left motor off
}
}