Autonomus car with Cubelets

left_motor.c

#include "cubelet.h"

/**
 * Sensor izquierdo: 88467
 * Sensor centro: 88373
 * Sensor derecho: 83755
 */

void setup()
{
  block_value = 0;
}

void loop()
{
  uint8_t centro = get_block_value(828618);
  uint8_t izquierda = get_block_value(83755);
  uint8_t derecha = get_block_value(88373);

  /* Centro ocupado. */
  if (centro > 100)
  {
    /* Izquierda ocupada. */
    if (izquierda < derecha)
    {
      /* Girar a la derecha. */
      set_drive_direction(BACKWARD);
      set_drive(255);
    }
    /* Izquierda libre. */
    else
    {
      /* Girar a la izquierda. */
      set_drive_direction(FORWARD);
      set_drive(255);
    }
  }
  /* Sin obstáculos al centro. */
  else
  {
    if (izquierda > 128 || derecha > 128)
    {
      if (izquierda > derecha)
      {
        /* Girar a la derecha. */
        set_drive_direction(BACKWARD);
        set_drive(255);
      }
      else
      {
        /* Girar a la izquierda. */
        set_drive_direction(FORWARD);
        set_drive(255);
      }
    }
    else
    {
      /* Avanzar. */
      set_drive_direction(FORWARD);
      set_drive(255);
    }
  }

  wait(100);
}

rear_motor.c

#include "cubelet.h"

/**
 * Sensor izquierdo: 88467
 * Sensor centro: 88373
 * Sensor derecho: 83755
 */

void setup()
{
  block_value = 0;
}

void loop()
{
  uint8_t centro = get_block_value(828618);
  uint8_t izquierda = get_block_value(83755);
  uint8_t derecha = get_block_value(88373);

  /* Centro ocupado. */
  if (centro > 100)
  {
    /*  */
    if (izquierda < derecha)
    {
      /* Girar a la derecha. */
      set_drive_direction(FORWARD);
      set_drive(128);
    }
    /* Izquierda libre. */
    else
    {
      /* Girar a la izquierda. */
      set_drive_direction(FORWARD);
      set_drive(128);
    }
  }
  /* Sin obstáculos al centro. */
  else
  {
    if (izquierda > 128 || derecha > 128)
    {
      if (izquierda > derecha)
      {
        /* Girar a la derecha. */
        set_drive_direction(FORWARD);
        set_drive(128);
      }
      else
      {
        /* Girar a la izquierda. */
        set_drive_direction(FORWARD);
        set_drive(128);
      }
    }
    else
    {
      /* Avanzar. */
      set_drive_direction(FORWARD);
      set_drive(255);
    }
  }

  wait(100);
}

right_motor.c

#include "cubelet.h"

/**
 * Sensor izquierdo: 83755
 * Sensor centro: 828618
 * Sensor derecho: 88373
 */

void setup()
{
  block_value = 0;
}

void loop()
{
  uint8_t centro = get_block_value(828618);
  uint8_t izquierda = get_block_value(83755);
  uint8_t derecha = get_block_value(88373);

  /* Centro ocupado. */
  if (centro > 100)
  {
    /* Izquierda ocupada. */
    if (izquierda < derecha)
    {
      /* Girar a la derecha. */
      set_drive_direction(FORWARD);
      set_drive(255);
    }
    /* Izquierda libre. */
    else
    {
      /* Girar a la izquierda. */
      set_drive_direction(BACKWARD);
      set_drive(255);
    }
  }
  /* Sin obstáculos al centro. */
  else
  {
    if (izquierda > 128 || derecha > 128)
    {
      if (izquierda > derecha)
      {
        /* Girar a la derecha. */
        set_drive_direction(FORWARD);
        set_drive(255);
      }
      else
      {
        /* Girar a la izquierda. */
        set_drive_direction(BACKWARD);
        set_drive(255);
      }
    }
    else
    {
      /* Avanzar. */
      set_drive_direction(FORWARD);
      set_drive(255);
    }
  }

  wait(100);
}

https://www.youtube.com/watch?v=Q5ueOMFNhwQ