TEMA:

#include "PS2X_lib.h"  //for v1.6
#include "Servo.h"

PS2X ps2x; // create PS2 Controller Class

Servo UpLServo;  //Create servo object representing up left ESC
Servo UpRServo;  //Create servo object representing up right ESC
Servo HorizLServo;  //Create servo object representing horiz left ESC
Servo HorizRServo;  //Create servo object representing horiz right ESC
Servo CamServo;  //Create servo object representing camera pitch

const int led1 = 30;

volatile boolean LEDHdlts;

int ForwardVal = 0;   // valor leído en palo PS2 derecho arriba/abajo
int YawLeftVal = 0;   // valor leído en palo PS2 derecho derecha/izquierda
int UpVal = 0;        // valor leído en palo PS2 izquierdo arriba/abajo
int RollLeftVal = 0;  // valor leído en palo PS2 izquierdo derecha/izquierda
int CamPitch = 90;

int upLraw = 0;  // Variables para transportar el valor actual de los datos a los ESC
int upRraw = 0;
int HLraw = 0;
int HRraw = 0;

int auxMillis = 0;
int intervaloEncendido = 1000;

void setup() {

  Serial.begin(9600);
  Serial.flush(); // Depura la informacion del puerto Serie

  // Configuracion de los pines y ajustes:  GamePad (clock, command, attention, data, Pressures?, Rumble?) verifica el error
  ps2x.config_gamepad(22, 23, 24, 25, true, true);

  pinMode(led1, OUTPUT); // Establece el pin del LED como salida

  UpLServo.attach(32);// attaches the Up left Servo to pin 5
  UpRServo.attach(34);// attaches the Up Right Servo to pin 6
  HorizLServo.attach(36);// attaches the Horizontal Left Servo to pin 7
  HorizRServo.attach(38);// attaches the Horizontal Right Servo to pin 8
  CamServo.attach(40);

}

void loop() {

  /*
    Debe leer el Gamepad para obtener nuevos valores
    Lea Gamepad y establezca valores de vibración
    ps2x.read_gamepad (motor pequeño encendido / apagado, mayor potencia del motor de 0 -22)
    Si no habilita la vibración, use ps2x.read_gamepad();  sin valores
    Deberias llamar por lo menos una vez a esto
  */

  ps2x.read_gamepad();   // Lee el controlador

  if (ps2x.Button(PSB_START)) {       // Sera verdadero cuando se presione el botón
    Serial.println(F("\n Botón Start esta siendo presionado"));
  }

  if (ps2x.Button(PSB_SELECT)) {
    Serial.println(F("\n Botón Select esta siendo presionado"));
  }

  if ( millis() - auxMillis > intervaloEncendido) {

    if (ps2x.ButtonPressed(PSB_PAD_UP)) {      // Sera verdad cuando se presione el botón, 1 vez ON / 1 vez OF
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Botón izquierda arriba presionado:"));
    }

    if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Botón izquierda abajo presinado:"));
    }

    if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Botón izquierda derecha presionado:"));
    }

    if (ps2x.ButtonPressed(PSB_PAD_LEFT)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Botón Izquierda Izquierda presionado:"));
    }

    if (ps2x.ButtonPressed(PSB_GREEN)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Triangulo presionado"));
    }

    if (ps2x.ButtonPressed(PSB_RED)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Circulo presionado"));
    }

    if (ps2x.ButtonPressed(PSB_PINK)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n Cuadrado presinado"));
    }

    if (ps2x.ButtonPressed(PSB_BLUE)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n X presionado"));
    }

    if (ps2x.ButtonPressed(PSB_L1)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n L1 presionado"));
    }

    if (ps2x.ButtonPressed(PSB_L2)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n L2 presionado"));
    }

    if (ps2x.ButtonPressed(PSB_L3)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n L3 presionado"));
    }

    if (ps2x.Button(PSB_R1)) {  // Será verdad mientras se mantenga pulsado el boton
      CamPitch = CamPitch + 1;
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n R1 presionado"));
    }

    if (ps2x.Button(PSB_R2)) {
      CamPitch = CamPitch - 1;
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n R2 presionado"));
    }

    if (ps2x.ButtonPressed(PSB_R3)) {
      LEDHdlts = !LEDHdlts;
      Serial.println(F("\n R3 presionado"));
    }

    ForwardVal = ps2x.Analog(PSS_RY);
    YawLeftVal = ps2x.Analog(PSS_RX);
    UpVal = ps2x.Analog(PSS_LY);
    RollLeftVal = ps2x.Analog(PSS_LX);

    upLraw = (128 - UpVal) + (128 - RollLeftVal) / 2; // esto será hasta un valor de 192
    upRraw = (128 - UpVal) - (128 - RollLeftVal) / 2; // esto será hasta un valor de 192
    HLraw = (128 - ForwardVal) + (128 - YawLeftVal); // esto será hasta un valor de 256
    HRraw = (128 - ForwardVal) - (128 - YawLeftVal); // esto será hasta un valor de 256

    // Scala los valores para que sean adecuados para los ESC y los servos
    upLraw = map(upLraw, -193, 193, 0, 179);
    upRraw = map(upRraw, -193, 198, 0, 179);
    HLraw = map(HLraw, -256, 256, 0, 179);
    HRraw = map(HRraw, -256, 256, 0, 179);

    // escribe el valor 0 en los servos

    UpLServo.write(upLraw);
    UpRServo.write(upRraw);
    HorizLServo.write(HLraw);
    HorizRServo.write(HRraw);

    CamPitch = constrain(CamPitch, 30, 149); //keep CamPitch within the 30 - 149 boundary
    CamServo.write(CamPitch);  //Move the camera servo

    digitalWrite(led1, LEDHdlts); // Enciende el Led Amarillo

  }
}