InMoov Robot Head movement with Arduino and joysticks

InMoovHead01.ino

/*
Código para mover servomotores de la cabeza InMoov
*/
#include <Servo.h>

const int pinJoystickOjosX = A1;  
const int pinJoystickOjosY = A0;
const int pinJoystickCuelloX = A3;  
const int pinJoystickCuelloY = A2;
const int pinJoystickBoca = A4;

const int pinServoOjosX= 10;
const int pinServoOjosY = 11;
const int pinServoCuelloX= 12;
const int pinServoCuelloY = 0;
const int pinServoBoca = 9;

const int incrementoServo = 2;

const int posOjosXmax= 76;
const int posOjosXmin= 32;
const int posOjosYmax= 126;
const int posOjosYmin= 50;
const int posCuelloXmax= 135;
const int posCuelloXmin= 40;
const int posCuelloYmax= 120;
const int posCuelloYmin= 60;
const int posBocamax= 122;
const int posBocamin= 76;

Servo servoOjosX;
Servo servoOjosY;
Servo servoCuelloX;
Servo servoCuelloY;
Servo servoBoca;

int posOjosX = 58;        
int posOjosY = 90;
int posCuelloX = 90;        
int posCuelloY = 90;
int posBoca = 95;

int jsOjosX = 0;
int jsOjosY = 0;
int jsCuelloX = 0;
int jsCuelloY = 0;
int jsBoca = 0;

void setup() {
  Serial.begin(9600);

  servoOjosX.attach(pinServoOjosX);
  servoOjosY.attach(pinServoOjosY);
  servoCuelloX.attach(pinServoCuelloX);
  servoBoca.attach(pinServoBoca);
}

void loop() {
  
  jsOjosX = analogRead(pinJoystickOjosX);
  jsOjosY = analogRead(pinJoystickOjosY);
  jsCuelloX = analogRead(pinJoystickCuelloX);
  jsCuelloY = analogRead(pinJoystickCuelloY);
  jsBoca = analogRead(pinJoystickBoca);
    
  //Debido a que el shield esta al reves, se le da vuelta al dato
  //jsOjosX = map(jsOjosX, 0, 1023, 1023, 0);
  //jsOjosY = map(jsOjosY, 0, 1023, 1023, 0);

  if (jsOjosX > 800 & posOjosX < posOjosXmax) { 
    posOjosX = posOjosX + incrementoServo;
  }
  if (jsOjosX < 200 & posOjosX > posOjosXmin) { 
    posOjosX = posOjosX - incrementoServo;
  }
  
  if (jsOjosY > 800 & posOjosY < posOjosYmax) { 
    posOjosY = posOjosY + incrementoServo;
  }
  if (jsOjosY < 200 & posOjosY > posOjosYmin) { 
    posOjosY = posOjosY - incrementoServo;
  }

  if (jsCuelloX > 800 & posCuelloX < posCuelloXmax) { 
    posCuelloX = posCuelloX + incrementoServo;
  }
  if (jsCuelloX < 200 & posCuelloX > posCuelloXmin) { 
    posCuelloX = posCuelloX - incrementoServo;
  }

  if (jsBoca > 800 & posBoca < posBocamax) { 
    posBoca = posBoca + incrementoServo;
  }
  if (jsBoca < 200 & posBoca > posBocamin) { 
    posBoca = posBoca - incrementoServo;
  }

  servoOjosX.write(posOjosX);
  servoOjosY.write(posOjosY);
  servoCuelloX.write(posCuelloX);
  servoBoca.write(posBoca);
    
  Serial.print("OjosX= ");
  Serial.print(posOjosX);
  Serial.print("  OjosY= ");
  Serial.print(posOjosY);
  Serial.print("  CuelloX= ");
  Serial.print(posCuelloX);
  Serial.print("  Boca= ");
  Serial.println(posBoca);

  delay(80);
}

Pictures and info at http://onecuriousrobot.blogspot.com/