WALTER: Robot Insecto Fotovoro Alimentado por Arduino

Componentes y suministros

Herramientas y máquinas necesarias

• Soldador (genérico)
• Pistola de pegamento caliente (genérica)

Aplicaciones y servicios en línea

• Arduino IDE

Sobre este proyecto

[Disculpen mi inglés]

Me gusta mucho la popular configuración de insectos Arduino de 2 servos en YouTube. Cuando la vi, siempre recordé lo que los chicos de la robótica BEAM hicieron mucho antes de que esa configuración se volviera favorita. Estas personas, fanáticos de los robots analógicos, lograban una mejor marcha debido al mejor ángulo entre los dos motores (microcore / bicore walker, etc).

Sin embargo, en mi opinión, ninguno de los mencionados se ve más vivo que el VBug1.5 (también conocido como Walkman) creado por el fundador de la robótica BEAM, Mark Tilden. Usa 5 motores, por lo que tiene más maniobrabilidad.

Hacer un robot BEAM simple no es difícil, pero construir algo tan complicado como el VBug1.5 podría ser angustiante para un novato en electrónica como yo. Así que, cuando decidí hacer algo como los bichos de Tilden, tuve que conformarme con la plataforma Arduino, la opción más fácil para no ingenieros (o en mi caso, vergonzosamente, un aspirante a ingeniero).

Como resultado, hice Walter, un robot Arduino de 4 patas con 5 servos. Te preguntarás, si quería hacer un robot bicho que se viera vivo, ¿por qué no usé 8 o 12 servos? Bueno, pensé en lo más simple que podía hacer para obtener la mayor maniobrabilidad posible. Hablo de usar mucho pegamento en lugar de hacer estructuras.

COMPORTAMIENTOS

Como muchos otros robots Arduino, Walter puede evitar obstáculos usando sensores ultrasónicos HC-SR04. Para añadir carácter de bicho, Walter también es un fotovoro, lo que significa que se siente atraído por la luz. Se usan fotodiodos para detectar la luz. Hay valores aleatorios generados en el sketch de Arduino para que Walter decida cuándo quiere detenerse a descansar, y también para cambiar aleatoriamente su velocidad de marcha (3 velocidades).

Cuando empecé, pretendía tener botones táctiles bajo cada una de las patas de Walter para que tuviera sensores de superficie. Pero la batería (un power bank para smartphone) costaba demasiado peso a los servos. Sé que los botones táctiles pesan casi nada, pero irónicamente el peso del robot no es suficiente para poder presionar los botones boca abajo.

Planeo hacer una versión 2 de Walter con servos más grandes e incluir estos botones como sensores de superficie.

Más detalles

El código completo está disponible en la sección de código.

Código

WALTER.ino

/* WALTER - EL FOTOVORO DE 4 PATAS
   Este sketch de Arduino es mi intento de construir un robot cuadrúpedo (4 patas) de 5 servos llamado "WALTER".
   Para usar este sketch, es posible que necesites cambiar algunos valores para tu comodidad o para ajustar a tu propio hardware.
   Busca (Ctrl + F) estas marcas para encontrar fácilmente qué valores podrían necesitar cambios:
   - **** : Estas marcas significan que son las posiciones centrales de los servos y deben calibrarse (la posición de las patas de tu robot cuando está inactivo).
   - *** : Estas marcas significan asignación de pines de Arduino (conexión de sensores y servos a Arduino). Consulta esto al construir el robot.
   - ** : Estas marcas significan que los valores opcionalmente pueden cambiarse según tu gusto (ancho de paso de las patas, cuánto girar al detectar luz/obstáculo, etc). Déjalos como están si no sabes lo que haces.
   Puedes usar este sketch bajo tu propio riesgo... y se proporciona tal cual... uh... ¿qué son esas cosas de derechos de autor que la gente suele poner en el código publicado? El punto es que no quiero ser responsable si algo malo pasa cuando uses este código. ¡Diviértete!
   Yohanes Martedi - 2015 */

#include <Servo.h>

// **** Calibrar ángulo central de los servos (en microsegundos porque usaremos "xx.writeMicroseconds();"). Empezar con 1500.
const int ANGLE_mid_Shaft = 1520;
const int ANGLE_mid_FLeft = 1550;
const int ANGLE_mid_FRight = 1570;
const int ANGLE_mid_BLeft = 1450;
const int ANGLE_mid_BRight = 1450;

const int ANGLE_sweep = 250; // **Establece este valor (en microsegundos) para determinar qué tan ancho barrerán los servos (ancho de paso de las patas). Valor mayor = barrido más amplio.
const int ANGLE_res = 10; // **Establece la resolución de movimiento del servo (en microsegundos) al menos en el ancho de banda muerto por defecto del servo (mayor resolución) o más (menor resolución). Ejemplo: servo SG90 banda muerta 10 microsegundos.

int sweepSPEED; // Variable para determinar qué tan rápido barrerán los servos.
int sweepSPEED_Rand[3] = {4, 6, 8}; // **La velocidad del servo (velocidad de marcha) cambiará aleatoriamente en 3 modos. Establece la velocidad (en milisegundos) para cada modo. Valor menor = más rápido.

const int ANGLE_turnMAX = ANGLE_sweep * 1.5; // **Establece este valor para determinar cuánto máximo girará el bot hacia la luz. Valor mayor = giro mayor.
const int ANGLE_turnNARROW = ANGLE_sweep * 0.25; // **Establece este valor para determinar cuánto máximo girará el bot evitando objetos a sus lados en espacio estrecho. Valor mayor = giro mayor.

const int SONAR_sum = 3; // Cantidad de sonares usados.
const int PHOTO_sum = 4; // Cantidad de fotodiodos usados.

int PIN_trig[SONAR_sum] = {13, 11, 8}; // ***Pines de Arduino conectados a pines trigger de sensores ultrasónicos; {frontal, izquierdo, derecho}.
int PIN_ec[SONAR_sum] = {12, 10, 7}; // ***Pines de Arduino conectados a pines echo de sensores ultrasónicos; {frontal, izquierdo, derecho}.
int PIN_PHOTO[PHOTO_sum] = {2, 3, 1, 0}; // ***Pines de entrada analógica de Arduino conectados a fotodiodos; {frontal izquierdo, frontal derecho, trasero izquierdo, trasero derecho}.

const int distRotate = 25; // **Configura distancia mínima (en cm) entre robot y obstáculo antes de evitarlo girando.
const int distRetreat = 10; // **Configura distancia mínima (en cm) entre robot y obstáculo antes de evitarlo retrocediendo.
const int distTurn = 20; // **Configura distancia mínima (en cm) entre robot y obstáculo antes de evitarlo virando.
const int counter_gait_max = 8; // **Configura cuántos pasos dará el robot para evitar obstáculo (al girar o retroceder).

// **Configura cuánto tiempo descansa y corre el bot (en milisegundos).
const int RUN_time = 25000;
const int REST_time = 3000;

// IDs para sonares:
int SONAR_id;
const int FRONT = 0;
const int LEFT = 1;
const int RIGHT = 2;

// IDs para fotodiodos:
const int FRONT_LEFT = 0;
const int FRONT_RIGHT = 1;
const int BACK_LEFT = 2;
const int BACK_RIGHT = 3;

// Variables para lectura de fotodiodos:
int PHOTO_Front_Left;
int PHOTO_Front_Right;
int PHOTO_Back_Left;
int PHOTO_Back_Right;

const int SONAR_TrigSig = 10; // Duración (en µs) de señal trigger que necesitan los sensores para producir sonido ultrasónico (ya especificado por productos, no cambiar).
const unsigned long SONAR_MaxEc = 50000; // Duración máxima (en µs) de señal echo dada por sensores (ya especificado por productos, no cambiar).
const float SOUND_speed = 0.034; // Velocidad del sonido en aire en cm/µs (ya especificado por la ciencia, se necesitaría a Avatar Aang para doblar el aire si se quiere cambiar este valor).

int distance[SONAR_sum]; // Resultados del cálculo de distancia.

// Declaración de servos:
Servo SERVO_shaft;
Servo SERVO_front_left;
Servo SERVO_front_right;
Servo SERVO_back_left;
Servo SERVO_back_right;

// Variables para ángulos de cada servo:
int ANGLE_shaft = ANGLE_mid_Shaft;
int ANGLE_front_left = ANGLE_mid_FLeft;
int ANGLE_front_right = ANGLE_mid_FRight;
int ANGLE_back_left = ANGLE_mid_BLeft;
int ANGLE_back_right = ANGLE_mid_BRight;

// Manipulación de ángulo para servo central (shaft).
const int ANGLE_max_Shaft = ANGLE_mid_Shaft + ANGLE_sweep;
const int ANGLE_min_Shaft = ANGLE_mid_Shaft - ANGLE_sweep;
int ANGLE_sweep_val;

// Variables para registrar ángulos actuales de cada servo:
int ANGLE_shaft_record;
int ANGLE_front_left_record;
int ANGLE_front_right_record;
int ANGLE_back_left_record;
int ANGLE_back_right_record;

// Variables para corrección de ángulos de servos según detección de luz:
int LIGHT_left;
int LIGHT_right;

// Variables para corrección de ángulos de servos según detección de sonar:
int SONAR_left;
int SONAR_right;

// Banderas, contadores, registros que nunca sé cómo explicar. :(
int ANGLE_prev;
int flag_shaft_reverse;
int flag_transition_rotate;
int flag_transition_start = 1;
int flag_rest = 0;
int flag_RUN_time = 0;
int rotate_random;
int counter_gait;

void setup() {
  // Serial.begin(9600); // Serial... ya sabes, comprobación y depuración...
  SERVO_shaft.attach(2); // ***Configura pin de señal del servo horizontal (shaft) en Arduino.
  SERVO_front_left.attach(4); // ***Configura pin de señal del servo frontal izquierdo en Arduino.
  SERVO_front_right.attach(3); // ***Configura pin de señal del servo frontal derecho en Arduino.
  SERVO_back_left.attach(6); // ***Configura pin de señal del servo trasero izquierdo en Arduino.
  SERVO_back_right.attach(5); // ***Configura pin de señal del servo trasero derecho en Arduino.

  // Poner servos listos en sus ángulos medios.
  SERVO_shaft.writeMicroseconds(ANGLE_mid_Shaft);
  SERVO_front_left.writeMicroseconds(ANGLE_mid_FLeft);
  SERVO_front_right.writeMicroseconds(ANGLE_mid_FRight);
  SERVO_back_left.writeMicroseconds(ANGLE_mid_BLeft);
  SERVO_back_right.writeMicroseconds(ANGLE_mid_BRight);

  // Configurar pines para sonares, tanto pinMode como valor.
  for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) {
    pinMode(PIN_trig[SONAR_id], OUTPUT);
    pinMode(PIN_ec[SONAR_id], INPUT);
    digitalWrite(PIN_trig[SONAR_id], LOW);
  }

  randomSeed(analogRead(5)); // Usar pin analógico 5 para generar valores aleatorios.
  SONAR_READ_ALL(); // Iniciar primera lectura de sonar antes de hacer nada,
  SONAR_READ_ALL(); // y en realidad olvido por qué tengo que llamarlo dos veces la primera vez. Pero créeme, es necesario.
  START(); // Esta función asegura qué patas están levantadas aleatoriamente en el primer paso.
  delay(3000); // **dum.. daa.. dum..
}

void loop() {
  int state = random(0,2);
  if(state == 0) {
    REST();
  } else {
    int randomSPEED = random(0,3);
    sweepSPEED = sweepSPEED_Rand[randomSPEED];
    RUN();
    flag_rest = 0;
  }
}

void REST() {
  if(flag_rest == 0) {
    TRANSITION_GAIT();
    flag_rest = 1;
  }
  delay(REST_time);
}

void RUN() {
  unsigned long TIMER_state = millis();
  while((millis() - TIMER_state) <= RUN_time) {
    if(distance[FRONT] > distRotate) {
      flag_RUN_time = 0;
      while(flag_RUN_time == 0) {
        FORWARD();
      }
    }
    while(distance[FRONT] > distRetreat && distance[FRONT] <= distRotate) {
      while(distance[LEFT] > distance[RIGHT]) {
        ROTATE_LEFT_AVOID();
        break;
      }
      while(distance[LEFT] < distance[RIGHT]) {
        ROTATE_RIGHT_AVOID();
        break;
      }
      while(distance[LEFT] == distance[RIGHT]) {
        ROTATE_RANDOM_AVOID();
        break;
      }
    }
    while(distance[FRONT] <= distRetreat) {
      RETREAT_AVOID();
    }
  }
}

/*________________________________________________________________________________________
########################################## MARCHA ########################################*/

/*=================================== MOVIMIENTO SHAFT ===================================*/
void SHAFT() {
  unsigned long TIMER_servo = millis();
  while((millis() - TIMER_servo) <= sweepSPEED) {
    while(ANGLE_shaft == ANGLE_mid_Shaft) {
      counter_gait++;
      SONAR_READ_ALL();
      LIGHT_COMPARE_EXECUTE();
      SIDE_AVOID();
      flag_RUN_time = 1;
      break;
    }
  }
  if(ANGLE_prev < ANGLE_shaft && ANGLE_shaft < ANGLE_max_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft += ANGLE_res;
  } else if(ANGLE_shaft >= ANGLE_max_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft -= ANGLE_res;
  } else if(ANGLE_prev > ANGLE_shaft && ANGLE_shaft > ANGLE_min_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft -= ANGLE_res;
  } else if(ANGLE_shaft <= ANGLE_min_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft += ANGLE_res;
  }
  SERVO_shaft.writeMicroseconds(ANGLE_shaft);
}

void SHAFT_REVERSE() {
  if(ANGLE_prev < ANGLE_shaft) {
    ANGLE_prev = ANGLE_shaft + 1;
  } else if(ANGLE_prev > ANGLE_shaft) {
    ANGLE_prev = ANGLE_shaft - 1;
  }
}
/*================================ FIN MOVIMIENTO SHAFT ================================*/

/*===================================== TRANSICIÓN =====================================*/
void TRANSITION_GAIT() {
  ANGLE_front_left_record = ANGLE_front_left;
  ANGLE_front_right_record = ANGLE_front_right;
  ANGLE_back_left_record = ANGLE_back_left;
  ANGLE_back_right_record = ANGLE_back_right;
  ANGLE_shaft_record = ANGLE_shaft;
  int flag = HIGH;
  int counter = 0;
  while(flag == HIGH) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    counter++;
    ANGLE_front_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_left_record, ANGLE_mid_FLeft);
    ANGLE_front_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_right_record, ANGLE_mid_FRight);
    ANGLE_back_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_left_record, ANGLE_mid_BLeft);
    ANGLE_back_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_right_record, ANGLE_mid_BRight);
    SERVO_shaft.writeMicroseconds(ANGLE_shaft);
    SERVO_front_left.writeMicroseconds(ANGLE_front_left);
    SERVO_front_right.writeMicroseconds(ANGLE_front_right);
    SERVO_back_left.writeMicroseconds(ANGLE_back_left);
    SERVO_back_right.writeMicroseconds(ANGLE_back_right);
    if(counter == ((ANGLE_sweep * 2) / ANGLE_res)) {
      flag = LOW;
      START();
      flag_transition_rotate = 0;
    }
  }
}

void TRANSITION_START() {
  if(ANGLE_shaft == ANGLE_mid_Shaft || (ANGLE_shaft > ANGLE_mid_Shaft && ANGLE_shaft > ANGLE_prev) || (ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_shaft < ANGLE_prev)) {
    ANGLE_sweep_val = 0;
  } else {
    flag_transition_start = 0;
  }
}

void START() {
  ANGLE_prev = random((ANGLE_shaft), (ANGLE_shaft + 2));
  if(ANGLE_prev == ANGLE_shaft) {
    ANGLE_prev -= 1;
  }
  flag_transition_start = 1;
}

int ROTATE_RANDOM() {
  return random(0, 2);
}
/*================================== FIN TRANSICIÓN ==================================*/

/*======================================== CAMINAR ========================================*/
void FORWARD() {
  while(flag_transition_rotate == 2) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 1;
  while(flag_shaft_reverse == 0) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 1;
  while(flag_transition_start == 1) {
    SHAFT();
    TRANSITION_START();
    WALK();
  }
  SHAFT();
  ANGLE_sweep_val = ANGLE_sweep;
  WALK();
}

void RETREAT() {
  while(flag_transition_rotate == 2) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 1;
  while(flag_shaft_reverse == 1) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 0;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    WALK();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = (ANGLE_sweep * -1);
  WALK();
}

void WALK() {
  if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_BRight);
  } else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_BRight);
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right));
  }
  SERVO_front_left.writeMicroseconds(ANGLE_front_left);
  SERVO_front_right.writeMicroseconds(ANGLE_front_right);
  SERVO_back_left.writeMicroseconds(ANGLE_back_left);
  SERVO_back_right.writeMicroseconds(ANGLE_back_right);
}
/*===================================== FIN CAMINAR =====================================*/

/*======================================= GIRAR =======================================*/
void ROTATE_LEFT() {
  while(flag_transition_rotate == 1) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 2;
  while(flag_shaft_reverse == 2) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 3;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    ROTATE();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = ANGLE_sweep;
  ROTATE();
}

void ROTATE_RIGHT() {
  while(flag_transition_rotate == 1) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 2;
  while(flag_shaft_reverse == 3) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 2;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    ROTATE();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = (ANGLE_sweep * -1);
  ROTATE();
}

void ROTATE() {
  if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FLeft + ANGLE_sweep_val), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FRight - ANGLE_sweep_val), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BLeft - ANGLE_sweep_val), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BRight + ANGLE_sweep_val), ANGLE_mid_BRight);
  } else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft - ANGLE_sweep_val));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight + ANGLE_sweep_val));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft + ANGLE_sweep_val));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight - ANGLE_sweep_val));
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FLeft - ANGLE_sweep_val), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FRight + ANGLE_sweep_val), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BLeft + ANGLE_sweep_val), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BRight - ANGLE_sweep_val), ANGLE_mid_BRight);
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft + ANGLE_sweep_val));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight - ANGLE_sweep_val));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft - ANGLE_sweep_val));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight + ANGLE_sweep_val));
  }
  SERVO_front_left.writeMicroseconds(ANGLE_front_left);
  SERVO_front_right.writeMicroseconds(ANGLE_front_right);
  SERVO_back_left.writeMicroseconds(ANGLE_back_left);
  SERVO_back_right.writeMicroseconds(ANGLE_back_right);
}
/*==================================== FIN GIRAR ====================================*/

/*_________________________________________________________________________________________
/*##################################### FIN MARCHA #####################################*/

/*================================= LECTURA ULTRASÓNICA =================================*/
void SONAR_READ_ALL() {
  for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) {
    distance[SONAR_id] = int (SONAR_READ(SONAR_id));
  }
}

float SONAR_READ(int index) {
  float SONAR_distance;
  digitalWrite(PIN_trig[index], HIGH);
  delayMicroseconds(SONAR_TrigSig);
  digitalWrite(PIN_trig[index], LOW);
  float SONAR_EcInterval = float (pulseIn(PIN_ec[index], HIGH, SONAR_MaxEc));
  while(SONAR_EcInterval > 0.0) {
    SONAR_distance = SONAR_EcInterval * (SOUND_speed / 2.0);
    break;
  }
  while(SONAR_EcInterval == 0.0) {
    SONAR_distance = 501.0;
    break;
  }
  return SONAR_distance;
}
/*============================= FIN LECTURA ULTRASÓNICA =============================*/

/*==================================== DETECCIÓN LUZ ====================================*/
void LIGHT_COMPARE_EXECUTE() {
  PHOTO_Front_Left = analogRead(PIN_PHOTO[FRONT_LEFT]);
  PHOTO_Front_Right = analogRead(PIN_PHOTO[FRONT_RIGHT]);
  PHOTO_Back_Left = analogRead(PIN_PHOTO[BACK_LEFT]);
  PHOTO_Back_Right = analogRead(PIN_PHOTO[BACK_RIGHT]);

  if((PHOTO_Front_Left + PHOTO_Front_Right) >= (PHOTO_Back_Left + PHOTO_Back_Right)) {
    int LIGHT_Sensitivity = 50;
    if(LIGHT_COMPARE() > LIGHT_Sensitivity) {
      LIGHT_left = LIGHT_COMPARE();
      LIGHT_right = 0;
    } else if(LIGHT_COMPARE() < -LIGHT_Sensitivity) {
      LIGHT_left = 0;
      LIGHT_right = LIGHT_COMPARE();
    } else {
      LIGHT_left = 0;
      LIGHT_right = 0;
    }
  } else {
    if(PHOTO_Back_Left > PHOTO_Back_Right) {
      LIGHT_right = 0;
      LIGHT_left = ANGLE_turnMAX;
    } else if(PHOTO_Back_Left < PHOTO_Back_Right) {
      LIGHT_right = -ANGLE_turnMAX;
      LIGHT_left = 0;
    } else {
      int RANDOM_back_light = random(0,2);
      if(RANDOM_back_light == 0) {
        LIGHT_right = 0;
        LIGHT_left = ANGLE_turnMAX;
      } else if(RANDOM_back_light == 1) {
        LIGHT_right = -ANGLE_turnMAX;
        LIGHT_left = 0;
      }
    }
  }
}

int LIGHT_COMPARE() {
  int LIGHT_rate;
  if(PHOTO_Front_Left > PHOTO_Front_Right) {
    LIGHT_rate = PHOTO_Front_Left;
  } else if(PHOTO_Front_Right > PHOTO_Front_Left) {
    LIGHT_rate = PHOTO_Front_Right;
  } else {
    LIGHT_rate = PHOTO_Front_Right;
  }
  int LIGHT_compareRAW = PHOTO_Front_Left - PHOTO_Front_Right;
  LIGHT_compareRAW = map(LIGHT_compareRAW, -LIGHT_rate, LIGHT_rate, -ANGLE_turnMAX, ANGLE_turnMAX);
  return LIGHT_compareRAW;
}
/*================================= FIN DETECCIÓN LUZ =================================*/

/*====================================== COMPORTAMIENTO ======================================*/
void RETREAT_AVOID() {
  counter_gait = 0;
  while(counter_gait <= counter_gait_max) {
    RETREAT();
  }
}

void ROTATE_LEFT_AVOID() {
  counter_gait = 0;
  rotate_random = 2;
  while(counter_gait <= counter_gait_max) {
    ROTATE_LEFT();
  }
}

void ROTATE_RIGHT_AVOID() {
  counter_gait = 0;
  rotate_random = 2;
  while(counter_gait <= counter_gait_max) {
    ROTATE_RIGHT();
  }
}

void ROTATE_RANDOM_AVOID() {
  rotate_random = ROTATE_RANDOM();
  while(rotate_random == 0) {
    ROTATE_LEFT_AVOID();
  }
  while(rotate_random == 1) {
    ROTATE_RIGHT_AVOID();
  }
}

void SIDE_AVOID() {
  if(distance[LEFT] <= distTurn && distance[RIGHT] > distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    SONAR_left = 0;
    SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnMAX, 0));
  } else if(distance[RIGHT] <= distTurn && distance[LEFT] > distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    SONAR_right = 0;
    SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnMAX, 0);
  } else if(distance[LEFT] <= distTurn && distance[RIGHT] <= distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    if(distance[LEFT] < distance[RIGHT]) {
      SONAR_left = 0;
      SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnNARROW, 0));
    } else if(distance[RIGHT] < distance[LEFT]) {
      SONAR_right = 0;
      SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnNARROW, 0);
    }
  } else {
    SONAR_right = 0;
    SONAR_left = 0;
  }
}
/*================================== FIN COMPORTAMIENTO ==================================*/

Esquemáticos

Ver imagen del esquemático en el proyecto original.