/*
 * 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. 
 * v0.3:
 * Added functionality to toggle an analog stick mode.
 * Cleaned up older code
 * v0.4:
 * Added code to utilize an ultrasonic sensor
 * v0.5:
 * Added code to utilize an led to indicate information to the user
 * v0.6:
 * Added code to utilize a buzzer for use as a horn.
 * v0.7:
 * Improved analog stick mode by adding the ability to lower the tank's speed and the ability to move in reverse
 * Cleaned up code formatting
 */
#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 <Servo.h>
#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
#define echoPin 8 // Defining pin for "echo" cable
#define trigPin 9 // Defining pin for "trigger" cable
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
float x = 0; // Declaring a floating point value for the position of the analog stick along the x axis 
float ry = 0;
float fast = 0; // Declaring a floating point value for the speed of the faster motor 
float slow = 0; // Declaring a floating point value for the speed of the slower motor
boolean f = false; // Declaring a boolean for whether or not the tank is moving forward during the analog stick mode
boolean b = false; // Declaring a boolean for whether or not the tank is moving backward during the analog stick mode
boolean isRunning = false; // Declaring this boolean so that the motors' speed can be toggled once it is ready to be used
boolean stickMode = false; // Declaring this boolean so that an anolog stick mode can be toggled on or off
boolean autoMode = false; // Declaring this boolean so that an automatic mode can be toggled on or off
long duration = 0; // Declaring this long integer for storing the time it takes for the returning ultrasonic wave to be detected by the sensor 
long distance = 0; // Declaring this long integer for storing the distance an obstacle is from the sensor 
int i = 0; // Declaring an integer for counting how many times the main loop of code has been completed  
int ai = 0; // Declaring another integer for counting used in the autonomous mode 
boolean ledOn = false; // Declaring this boolean to avoid an error that occurs when the arduino attempts to send a "HIGH" state through the LED's pin when it is already on.
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)
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(3, OUTPUT); 
  pinMode(2, OUTPUT);
  // ^ Configuring digital pins either for input or output
  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_CIRCLE)){
    // Executes the code below if the circle button is pressed.
    digitalWrite(2, HIGH); // Causes the buzzer to play a sound by setting it to "HIGH"
  } else if (ps2x.ButtonReleased(PSB_CIRCLE)){
    // Executes the code below if the circle button is released. 
    digitalWrite(2, LOW); // Stops the buzzer's sound when the circle button is released. 
  }
  if (ps2x.ButtonPressed(PSB_SELECT)) {
    stickMode = !stickMode; // Toggling the value of the boolean "stickMode"
    Serial.println("SELECT");
  }
  if (ps2x.ButtonPressed(PSB_TRIANGLE)) {
    autoMode = !autoMode; // Toggling the value of the boolean "autoMode"
    if (autoMode && !ledOn){
      digitalWrite(3, HIGH); // The LED remains on when the autonomous mode is selected
      ledOn = true;
    } else if (ledOn){
      digitalWrite(3, LOW); // The LED turns off when the autonomous mode is deselected
      ledOn = false;
    }
    motorOne->setSpeed(255);
    motorTwo->setSpeed(255);
    motorOne->run(RELEASE);
    motorTwo->run(RELEASE);
    Serial.println("AUTO");
  }
  if (!autoMode) {
    if (!stickMode) {
      //Executes this code if "stickMode" is set to false
      if (i==0 && !ledOn) {
        digitalWrite(3, HIGH);
        ledOn = true;
        i = 21;
      } else if (i==20 && ledOn) {
        digitalWrite(3, LOW);
        ledOn = false;
      }
      // ^ This code allows for the LED to blink once every second without further delaying the arduino by counting the delay at the bottom of loop()
      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)
      } else if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {
        motorOne->run(BACKWARD);
        motorTwo->run(BACKWARD);
        Serial.println("DOWN");
      } else if (ps2x.ButtonPressed(PSB_PAD_LEFT)) {
        motorOne->run(FORWARD);
        motorTwo->run(BACKWARD);
        Serial.println("LEFT");
      } else if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
        motorOne->run(BACKWARD);
        motorTwo->run(FORWARD);
        Serial.println("RIGHT");
      } else 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");
      } else if (ps2x.ButtonPressed(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
      }
  } else {
    // The code below only executes if "stickMode" is true
    if (i==0 && !ledOn) {
      digitalWrite(3, HIGH);
      i = 21;
      ledOn = true;
    } else if (i==20 && ledOn) {
      digitalWrite(3, LOW);
      ledOn = false;
    } else if (i==19 && !ledOn) {
      digitalWrite(3, HIGH);
      ledOn = true;
    } else if (i==18 && ledOn) {
      digitalWrite(3, LOW);
      ledOn = false;
    }
    // ^ This code allows for the LED to blink twice every second without further delaying the arduino by counting the delay at the bottom of loop()
    if (ps2x.Button(PSB_R2)) {
      if (!f) {
        if(b && fast > 0.0){
          fast = fast - 15;
        } else {
          motorOne->setSpeed(0);
          motorOne->setSpeed(0);
          motorOne->run(FORWARD);
          motorTwo->run(FORWARD);
          b = false;
          f = true;
          // ^ If the device is not set to move forward, the code above will execute
        }
      }
      if (fast < 255 && f){
          fast = fast + 15; // Slowly increases speed instead of moving at the maximum immediately
      }  
    } else if (ps2x.ButtonPressed(PSB_CROSS)) {
      fast = 0;
      slow = 0;
      motorOne->setSpeed(0);
      motorTwo->setSpeed(0);
      // ^ Stops the motors if the "X" button is pressed
    } else if (ps2x.Button(PSB_L2)){
      if (!b) {
        if(f && fast > 0.0){
            fast = fast - 15;
        } else {
            motorOne->setSpeed(0);
            motorOne->setSpeed(0);
            motorOne->run(BACKWARD);
            motorTwo->run(BACKWARD);
            b = true;
            f = false;
            // ^ If the device is not set to move forward, the code above will execute execute
        }
      }
      if (fast < 255 && b){
        fast = fast + 15; // Slowly increases speed instead of moving at the maximum immediately
      }  
    }   
    x = ps2x.Analog(PSS_LX); 
    if (x < 128){
      slow = fast*(x/128);
      Serial.print("X: ");
      Serial.println(x); // Printing the x cooridinate for debugging purposes
      motorOne->setSpeed((uint8_t)fast); // Casting the "fast" float as an 8 bit interger, as "setSpeed" only takes 8 bit interger values
      motorTwo->setSpeed((uint8_t)slow); // Casting the "slow" float as an 8 bit interger, as "setSpeed" only takes 8 bit interger values
    } else if (x > 128) {
      slow = fast*((255 - x)/128);
      motorOne->setSpeed((uint8_t)slow);
      motorTwo->setSpeed((uint8_t)fast); 
      Serial.print("X: ");
      Serial.println(x); // Printing the x coordinate for debugging purposes
    } else {
      motorOne->setSpeed((uint8_t)fast); 
      motorTwo->setSpeed((uint8_t)fast);
    } 
  } 
} else {
    if (i == 0){
      i = 21;
    }
    // ^ Keeps the counter running for when the tank switches back to another mode
    digitalWrite(trigPin, LOW); 
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // ^ Sends out the ultrasonic wave
    duration = pulseIn(echoPin, HIGH); // Measures the amount of time it takes for the ultrasonic wave to return to the sensor
    distance = duration/58.2; // Converts the duration to a distance in centimeters.
    Serial.print("Duration: ");
    Serial.print(duration);
    Serial.print(". Distance: ");
    Serial.print(distance);
    Serial.println(".");
    // ^ Printing out the duration and distance for debugging purposes
    if (distance <= 10 || ai > 0){
      motorOne->run(FORWARD);
      motorTwo->run(BACKWARD);
      if(ai == 0){
          ai = 3;
      } else {
          ai--;
      }
      // ^ Causes the tank to turn if the distance from an obstacle is ten centimeters or less away, for at least 200 milliseconds (at least looping through 4 times)
    } else if (distance > 10 && ai==0) {
      motorOne->run(FORWARD);
      motorTwo->run(FORWARD);
      // ^ Causes the tank to move forward otherwise
    }
  }
  delay(50);// 50 millisecond delay for controller input to be read and for motors to fully change state
  i--; // Decreases the counter by one
}