 Servo servo_tronco;
 Servo servo_trompa;
 Servo servo_pinza;
+Servo servos[] = { servo_base, servo_tronco, servo_trompa, servo_pinza };
+const int servos_quantity = 4;
+int current_pair_selection = 0;
 
 /* ***************************************************************************************
  *                                         SETUP                                         *
  * ***************************************************************************************/
 void init_buttons() {
-  for (int i; i < buttons_quantity; i++) {
+  for (int i = 0; i < buttons_quantity; i++) {
    pinMode(buttons[i], INPUT_PULLUP);
   }
 }

#include <Servo.h>

/* ***************************************************************************************
 *                                      PINES BOTONERA                                   *
 * ***************************************************************************************/
int up_button = 2;
int down_button = 4;
int left_button = 5;
int right_button = 3;
int start_button = 6;
int select_button = 7;
int analog_button = 8;
int x_axis = A0;
int y_axis = A1;
int buttons[] = {up_button, down_button, left_button, right_button, start_button, select_button, analog_button};
int buttons_quantity = 7;

/* ***************************************************************************************
 *                                     MAPEO A FUNCIONES                                 *

  Serial.print("Y = "),Serial.print(map(analogRead(yAxisId), 0, 1023, -1, 1));Serial.print("\n");
}

void update_servo(int axis_id, Servo* servo) {
  int val = analogRead(axis_id);
  val = map(val, 0, 1023, -100, 100);
  int final_val = val / 10;
  servo->write(servo->read() + final_val);
}

void update_servos_pair() {
  int base_servo_index = current_pair_selection * 2;
  update_servo(x_axis, &servos[base_servo_index]);
  update_servo(y_axis, &servos[base_servo_index + 1]);
}

void read_servos_pair_selection() {
  int selection = digitalRead(boton_cambios);
  if (selection == LOW) {
    current_pair_selection = (current_pair_selection + 2) % servos_quantity;
    if (DEBUG) {
      Serial.print("Par de servos: "),Serial.print(current_pair_selection),Serial.print("\n");
    }
  }
}

void loop() {
  update_servos_pair();
  read_servos_pair_selection();
  delay(150);
}


