/* Modified from Adafruit's "DCMotorTest" by Hans Huth
 * Starter sketch to use with Isaiah's hacked tank

For use with the Adafruit Motor Shield v2 
---->	http://www.adafruit.com/products/1438
*/

#include <Wire.h>
#include <Adafruit_MotorShield.h>

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 

// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *leftMotor = AFMS.getMotor(1); // left motor on terminal 1
// You can also make another motor on port M2
Adafruit_DCMotor *rightMotor = AFMS.getMotor(3); // right motor on terminal 3

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");

  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz

}

void loop() {
  uint8_t i;

//***** Isaiah - lets fist just run both motors forward and backwards

// First, we need to set the speed of the motors.
// You can change the speed of the motor here

// Set the speed to start, from 0 (off) to 255 (max speed)
  leftMotor->setSpeed(150);
  rightMotor->setSpeed(150);

  // Isaih - I've noticed these motors work best if you keep the speed 
  // at greater than 150. I also notice they have a little
  // trouble going backwards-- just keep that in mind when programming

// Make both motors run forward at the same time 
leftMotor->run(FORWARD);
rightMotor->run(FORWARD);
delay(3000); // this lets them run for three seconds

// RELEASE THE MOTORS
leftMotor->run(RELEASE);
rightMotor->run(RELEASE);

// Make both motors run backward at the same time 
leftMotor->run(BACKWARD);
rightMotor->run(BACKWARD);
delay(3000); // this lets them run for three seconds

// RELEASE THE MOTORS - take your foot of the gas ;)
leftMotor->run(RELEASE);
rightMotor->run(RELEASE);

// Next, let's try something a little more complicated.
// This next block slowly accelerates and decelerates the left motor

// Isaiah - change the number in "i<150" and "i=150" to something
// else between 100 and 255 to see what happens.

  leftMotor->run(FORWARD);
  for (i=0; i<255; i++) {
    leftMotor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    leftMotor->setSpeed(i);  
    delay(10);
  }
  leftMotor->run(RELEASE);
  
// This block slowly accelerate and decelerates right motor

// Isaiah - change the number in "i<100" from 100 to something
// else between 100 and 255 to see what happens

  rightMotor->run(FORWARD);
  for (i=0; i<255; i++) {
    rightMotor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    rightMotor->setSpeed(i);  
    delay(10);
  }
  rightMotor->run(RELEASE);

// Let's delay for one second before returning to the top of the loop
// and doing it all over again. 

// Isaiah - change the delay to see 
// what happens

  delay(1000);
  
}