R/C Tank Wireless

#include <PS2X_lib.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

#define PS2_DAT        9
#define PS2_CMD        11
#define PS2_SEL        10
#define PS2_CLK        12
#define pressures   false
#define rumble      false

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *portSideMotor = AFMS.getMotor(3);
Adafruit_DCMotor *starboardSideMotor = AFMS.getMotor(4);

PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;

void setup(){

  Serial.begin(57600);

  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
  pinMode(9, INPUT_PULLUP);
  Serial.println("tank motors");
  AFMS.begin();
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

//  Serial.print(ps2x.Analog(1), HEX);

  type = ps2x.readType();
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
    case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
}

void loop() {

  if(error == 1) //skip loop if no controller found
    return;

  if(type == 2) //Guitar Hero Controller
    return;

  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");

    if(ps2x.ButtonPressed(PSB_PAD_UP)) {
      Serial.println("TRIGGERED");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
      portSideMotor->setSpeed(220);
      starboardSideMotor->setSpeed(200);
      portSideMotor->run(FORWARD);
      starboardSideMotor->run(FORWARD);
    }
    if(ps2x.ButtonReleased(PSB_PAD_UP)){
      portSideMotor->setSpeed(0);
      starboardSideMotor->setSpeed(0);
    }
    if(ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
      Serial.println("TRIGGERED");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
      portSideMotor->setSpeed(105);
      starboardSideMotor->setSpeed(100);
      portSideMotor->run(FORWARD);
      starboardSideMotor->run(BACKWARD);
    }
    if(ps2x.ButtonReleased(PSB_PAD_RIGHT)){
      portSideMotor->setSpeed(0);
      starboardSideMotor->setSpeed(0);
    }
    if(ps2x.ButtonPressed(PSB_PAD_LEFT)) {
      Serial.println("TRIGGERED");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
      portSideMotor->setSpeed(105);
      starboardSideMotor->setSpeed
      (100);
      portSideMotor->run(BACKWARD);
      starboardSideMotor->run(FORWARD);
    }
    if(ps2x.ButtonReleased(PSB_PAD_LEFT)){
      portSideMotor->setSpeed(0);
      starboardSideMotor->setSpeed(0);
    }
    if(ps2x.ButtonPressed(PSB_PAD_DOWN)){
      Serial.println("I DON'T FEEL SAFE");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
      portSideMotor->setSpeed(220);
      starboardSideMotor->setSpeed(200);
      portSideMotor->run(BACKWARD);
      starboardSideMotor->run(BACKWARD);
    }
    if(ps2x.ButtonReleased(PSB_PAD_DOWN)){
      portSideMotor->setSpeed(0);
      starboardSideMotor->setSpeed(0);
    }
    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
      if(ps2x.Button(PSB_L3))
        Serial.println("L3 pressed");
      if(ps2x.Button(PSB_R3))
        Serial.println("R3 pressed");
      if(ps2x.Button(PSB_L2))
        Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
        Serial.println("R2 pressed");
      if(ps2x.Button(PSB_TRIANGLE))
        Serial.println("Triangle pressed");
    }

    if(ps2x.ButtonPressed(PSB_CIRCLE))               //will be TRUE if button was JUST pressed
      Serial.println("Circle just pressed");
    if(ps2x.NewButtonState(PSB_CROSS))               //will be TRUE if button was JUST pressed OR released
      Serial.println("X just changed");
    if(ps2x.ButtonReleased(PSB_SQUARE))              //will be TRUE if button was JUST released
      Serial.println("Square just released");

    if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC);
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC);
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC);
    }
  }
  delay(50);
}