Arduino: Brazo robot con servos ( Robot arm ) Parte 2

Buenas a todos!!!
Como ya publique hace un tiempo, tenia este brazo robot de 4 ejes hecho con una impresora 3D, en la primera entrada no publique código por un problema que tuve con el PC, pero esta vez va completo.
Bueno todo empezó cuando buscando algo, vaya usted a saber que... encontré un viejo gamepad USB, (a decir verdad encontré varios) y se me ocurrió la idea de usar los thumbsticks o joys para controlar los movimientos del brazo.

Después de desmontar el mando en este modelo vienen los dos joys montados en una misma plaquita, con la ayuda de un multímetro identificamos alimentación y las salidas de los potenciómetros además de los dos pulsadores que tiene cada joy, en mi caso solo tuve que seguir las pistas y hacer una pequeña modificación con la resistencia que venia de casa en el joypad. 

Solo nos hace falta:

• 1 Arduino (Uno o Nano)
• 5 Servos
• 2 joysticks

Una vez tengamos todas las piezas el montaje es el siguiente: 

Los joystick controlan los movimientos y los pulsadores que llevan incorporados se utilizan para abrir y cerrar la pinza.
Lo único que habría que retocar serian los máximos (maxSerXpos) y mínimos (minSerXpos) de cada servo además de el punto medio donde quedaran los ejes una vez soltemos los joysticks. Se configura justo después del comentario "//estos son los valores para los joysticks en reposo".

Y bueno en principio esto es todo, os dejo el enlace para que lo podáis descargar y probar en vuestro arduino.

Descargar Ejemplo para brazo robot 4 ejes y joypad