/*
 * DRV8833_Test_PWM
 * PWM (Pulse Width Modulation) test for the DRV8833 library.
 * The DRV8833 is a dual motor driver carrier made by Pololu.
 * You can find it here: https://www.pololu.com/product/2130
 *
 * Attach the positive wire of a motor to the Aout1 and the negative
 * wire to the Aout2 pins on the DRV8833.
 * Attach the positive wire of a motor to the Bout1 and the negative
 * wire to the Bout2 pins on the DRV8833.
 * Attach the Arduino's ground to the one of the GND pins on the DRV8833.
 * Attach a 9V battery's positive connection to the Vin pin
 * on the DRV8833, and the negative connection to one of the GND pins.
 * 
 * Created March 31, 2015, by Aleksandr J. Spackman.
 */

#include <DRV8833.h>

// Create an instance of the DRV8833:
DRV8833 driver = DRV8833();

// Pin numbers. Replace with your own!
// For this example sketch, these pin numbers MUST be PWM.
// Attach the Arduino's pin numbers below to the
// Ain1, Ain2, Bin1, and Bin2 DRV8833 pins.
const int inputA1 = 5, inputA2 = 6, inputB1 = 9, inputB2 = 10;

// The speed of the motors:
const int motorSpeed = 127; // Half speed (255 / 2).

const int buttonPinA = 8;     // the number of the pushbutton pin
const int buttonPinB = 12;     // the number of the pushbutton pin
int buttonStateA = 0;         // variable for reading the pushbutton status
int buttonStateB = 0;         // variable for reading the pushbutton status

int ir = 0; //analog pin 0
int averageir = 0; //average ir value

void setup() {
  // put your setup code here, to run once:
  
  // Start the serial port:
  Serial.begin(9600);
  
  // Wait for the serial port to connect. Needed for Leonardo.
  while (!Serial);
  
  // Attach the motors to the input pins:
  driver.attachMotorA(inputA1, inputA2);
  driver.attachMotorB(inputB1, inputB2);
  Serial.println("Ready!");

   pinMode(buttonPinA, INPUT_PULLUP);
   pinMode(buttonPinB, INPUT_PULLUP);

}

void loop() {
  // put your main code here, to run repeatedly:
  //buttonState = digitalRead(buttonPin);
  
Serial.print(" ir=");
Serial.print(analogRead(ir));
Serial.print(" A=");
Serial.print(digitalRead(buttonPinA));
Serial.print(" B=");
Serial.println(digitalRead(buttonPinB));
  if(analogRead(ir) > 20){    // if someone gets close
    
    // Put the motors in forward using the speed:
    driver.motorAForward(motorSpeed);
    driver.motorBReverse(motorSpeed);
    Serial.println("Forward:");
    
    // Wait to see the effect:
    delay(3000);
    driver.motorAStop();
    driver.motorBStop(); //stop when you get all the way out
    delay(100);          // pause for a moment
    driver.motorAReverse(motorSpeed * 3 / 4);
    driver.motorBForward(motorSpeed * 3 / 4);
    
    //
    bool aispressed = 0, bispressed = 0;
    
    while(!aispressed || !bispressed){ //
      Serial.print(" ir=");
      Serial.print(analogRead(ir));
      Serial.print(" A=");
      Serial.print(digitalRead(buttonPinA));
      Serial.print(" B=");
      Serial.println(digitalRead(buttonPinB));
      if(!digitalRead(buttonPinA)){
        driver.motorAStop();
        aispressed = 1;
        Serial.print("ButtonA Pressed");
      }
      if(!digitalRead(buttonPinB)){
        driver.motorBStop();
        bispressed = 0;
        Serial.print("ButtonB Pressed");
      }
    }
  }
}