/*
 * RC Tank by Michael C. @ Bluestamp Engineering NYC
 * Based on code from the Adafruit Motor Shield Library and code for reading the PS2 controller by Bill Porter
 *
 * v0.1:
 * Created file. 
 * Included basic movement using directional controller buttons.
 * v0.2:
 * Removed unnecessary code.
 * Added some code to print to the Serial Monitor for button presses.
 * Added the ability to toggle the device's speed between '255' and '0' by pressing the start button. 
 */
#include <Wire.h> //Library for I2C communication
#include <Adafruit_MotorShield.h> // Library for Motor Shield
#include "utility/Adafruit_MS_PWMServoDriver.h" // Library for PWM control
#include <PS2X_lib.h> // Library for PS2 Controller
#define PS2_DAT        13  // Defining pin for "data" cable    
#define PS2_CMD        11  // Defining pin for "command" cable
#define PS2_SEL        12  // Defining pin for "attention" cable
#define PS2_CLK        10  // Defining pin for "clock" cable
#define pressures   false // Setting mode for the "analog reading of push buttons" to false
#define rumble      false // Setting mode for the "motor rumbling" (in the controller) to false
PS2X ps2x; // Creating the PS2 Controller Object
int error = 0; // Declaring an integer for identifying different types of errors
byte type = 0; // Declaring a byte for identifying different types of controllers
byte vibrate = 0; // Declaring a byte for controller vibration speed
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); // Creating the motor shield object with the default I2C address
Adafruit_DCMotor *motorOne = AFMS.getMotor(1); // Creating a motor object at port M1
Adafruit_DCMotor *motorTwo = AFMS.getMotor(2); // Creating a motor object at port M2
boolean isRunning = false; //Declaring this boolean so that the motors' speed can be toggled once it is ready to be used
void setup() {
  // put your setup code here, to run once:
  Serial.begin(57600); // Initializes the serial connection at 57600 bits per second, used here for debugging with a computer
  AFMS.begin(); // Starting the shield at the default frequency of 1.6KHz
  delay(1000); //1 second delay added to give wireless ps2 module some time to startup before configuring it
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble); //Setting up pins and settings and checking for error
  /*
   * errors:
   * 0 - No error
   * 1 - No controller found
   * 2 - Controller found but not accepting commands
   * 3 - Controller refusing to enter pressures mode, may not support it.
   */
  Serial.println("Error: ");
  Serial.println(error);// Printing out the result of the error check 
  type = ps2x.readType(); // Reading the type of the controller
  /*
   * types:
   * 0 - Unknown
   * 1 - Dualshock
   * 2 - GuitarHero
   * 3 - Wireless **SONY** DualShock
   */
  Serial.println("Type: "); 
  Serial.println(type); // Printing out the result of the type check
  motorOne->setSpeed(255);
  motorTwo->setSpeed(255);
  // ^ Setting the initial speed of the motors, from 0 (off)to 255 (maximum)
  isRunning = true; // Setting "isRunning" to true since the setup has been completed
}

void loop() {
  
  // put your main code here, to run repeatedly:
    ps2x.read_gamepad(false, vibrate); // Read the controller and set the controller's motor to spin at "vibrate" speed
    if (ps2x.ButtonPressed(PSB_PAD_UP)) { 
        // Executes this code if the given button is pressed
        motorOne->run(FORWARD); // Moves the motor at port M1 forward
        motorTwo->run(FORWARD); // Moves the motor at port M2 forward
        Serial.println("UP"); // Prints out which button is pressed (for debugging purposes)
      }
    if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {
        motorOne->run(BACKWARD);
        motorTwo->run(BACKWARD);
        Serial.println("DOWN");
      }
    if (ps2x.ButtonPressed(PSB_PAD_LEFT)) {
        motorOne->run(FORWARD);
        motorTwo->run(BACKWARD);
        Serial.println("LEFT");
      }
    if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
        motorOne->run(BACKWARD);
        motorTwo->run(FORWARD);
        Serial.println("RIGHT");
      }
    if (ps2x.ButtonReleased(PSB_PAD_UP) || ps2x.ButtonReleased(PSB_PAD_DOWN) || ps2x.ButtonReleased(PSB_PAD_LEFT) || ps2x.ButtonReleased(PSB_PAD_RIGHT)) {
        // Executes this code if ANY of the four directional buttons are released
        motorOne->run(RELEASE); // Stops the motor at port M1
        motorTwo->run(RELEASE); // Stops the motor at port M2
        Serial.println("RELEASE");
      }
    if (ps2x.Button(PSB_START)) {
      // Executes this code if start is pressed  
      if (isRunning){
        // If "isRunning" is set to true, this code is executed
        motorOne->run(RELEASE);
        motorTwo->run(RELEASE);
        motorOne->setSpeed(0); 
        motorTwo->setSpeed(0);
        Serial.println("STOPPED");
      } else {
        // If "isRunning" is set to false, this code is executed
        motorOne->setSpeed(255);
        motorTwo->setSpeed(255);
        Serial.println("STARTED");
      }
      isRunning = !isRunning; // Toggles the value of "isRunning" from true to false or false to true
    }
    delay(50); // 50 millisecond delay for controller input to be read and for motors to fully change state
}