Fala galera!

Estou montando um robo com dois motores DC, vou controlar eles com um controle de PSx e estou quebrando muito a cabeça pra isso = /

Usei como base estes dois sites:

http://automatobr.blogspot.com.br/2012/10/automacao-no-parque-20102...

http://www.billporter.info/2010/06/05/playstation-2-controller-ardu...

Baixei a biblioteca PS2X e usei o programa que o cara uso nesse blog e conectei o controle como ele mostra no blog (imagino que tenha conectado certo):

#include <Servo.h>
#include <PS2X_lib.h>

PS2X ps2x; // cria a classe para controle do PS2

Servo shoulder;
Servo elbow;

int shoulderPos = 180;
int elbowPos = 30;

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

void setup()

{

Serial.begin(57600);
shoulder.attach(5);
elbow.attach(6);

error = ps2x.config_gamepad(13,11,10,12, true, true);

//GamePad(clock, command, attention, data, Pressures?, Rumble?)

if(error == 0)

{
Serial.println("Controle encontrado e configurado");
Serial.println("Os botões L1 e L2, R1 e L2 acionarão os motores.");
}
else if(error == 1)
Serial.println("Controle não encontrado.");
else if(error == 2)
Serial.println("Controle encontrado, mas não aceita comandos.");
else if(error == 3)
Serial.println("Controle não aceita modo pressão.");

type = ps2x.readType();

switch(type)
{

case 0:
Serial.println("Tipo desconhecido");
break;

case 1:
Serial.println("Controle DualShock");
break;

case 2:
Serial.println("Controle GuitarHero");
break;
}
}

void loop()
{
if(error == 1)
return;
ps2x.read_gamepad(false, vibrate);
if (ps2x.NewButtonState())
{
if(ps2x.Button(PSB_L1))
{
Serial.println("L1 pressionado");
elbowPos=elbowPos+5;
if (elbowPos > 60)
elbowPos=60;
elbow.write(elbowPos);
} if(ps2x.Button(PSB_L2))
{
Serial.println("L2 pressionado");
elbowPos=elbowPos-5;
if (elbowPos < 30)
elbowPos=30;
elbow.write(elbowPos);
}
if(ps2x.Button(PSB_R1))
{
Serial.println("R1 pressionado");
shoulderPos=shoulderPos+10;
if (shoulderPos > 180)
shoulderPos=180;
shoulder.write(shoulderPos);
}
if(ps2x.Button(PSB_R2))
{
Serial.println("R2 pressionado");
shoulderPos=shoulderPos-10;
if (shoulderPos < 0)
shoulderPos=0;
shoulder.write(shoulderPos); }
}

delay(50);
}

Copilo sem problema para o arduino.

Agora estou com a seguinte dúvida, não consegui entender como faço para configurar a saída, para quando eu apertar o botão ele acionar o a saída que eu quero. E como posso saber se o controle esta funcionando.

Fico grato se alguem puder me ajudar, isso faz parte do meu tcc e é muito importante. Estou começando a mexer com arduino agora (uso o arduino mega).