Source code for T-bugs. https://youtu.be/1kzht-VDU5w

motor.c

/**
 * Program for the motor block.
 * T-bugs.
 * ALIROB IPN.
 */

#include "cubelet.h"

/** ID's of the rotator. */
#define CONTROLER 816343

/**
 * First-setup function.
 *
 * Always move forward.
 */

void setup()
{
  block_value = 0;
  set_drive_direction(FORWARD);
}

/**
 * Function of execution.
 *
 * Move accordingly to the orders of the rotator block.
 */

void loop()
{
  uint8_t instruction = get_block_value(CONTROLER);
  switch (instruction) {
    case 0:
      set_drive(0);
      break;
    case 1:
      set_drive(255);
      break;
    default:
      set_drive(0);
  }
}

rotator.c

/**
 * Program for the rotator block.
 * T-bugs.
 * ALIROB IPN.
 */

#include "cubelet.h"

/** ID's of other cubes. */
#define SENSOR_RIGHT 828618
#define SENSOR_LEFT 88373

/** Constants for personalization. */
#define ROTATION_TIME_ONE 500
#define ROTATION_TIME_TWO 300
#define ROTATOR_POWER 255
#define PROXIMITY_LEVEL 100

/**
 * First-setup function.
 *
 * The block value of this cube controls the operation of the
 * motor: a zero commands a stop; an one commands move forward.
 */

void setup()
{
  block_value = 0;
}

/**
 * Determine the turning time when are obstacles in front.
 *
 * The time of turning is proportional to the distance of the
 * obstacle: the closer the object, the greater the turning time.
 *
 * \param distance the value obtained of the distance sensors.
 * \return the amount of miliseconds to rotate.
 */

uint16_t calcTurningTime(uint8_t distance)
{
  return ROTATION_TIME_ONE + ((distance - PROXIMITY_LEVEL + 1) * 1.5);
}

/**
 * Function of execution.
 *
 * Obtain the lectures of both sensors. If there is nothing close, then
 * set the block value in 1 (move forward the motor) and alternate the
 * move of the rotator. If there is an obstacle, then set the block
 * value in 0 (stop the motor) and rotate in the direction of the
 * less closer object.
 */

void loop()
{
  uint8_t distance_right = get_block_value(SENSOR_RIGHT);
  uint8_t distance_left = get_block_value(SENSOR_LEFT);

  if (distance_left > PROXIMITY_LEVEL || distance_right > PROXIMITY_LEVEL)
  {
    block_value = 0;
    if (distance_right > distance_left)
    {
    	set_rotate_direction(FORWARD);
      set_rotate(ROTATOR_POWER);
  		wait(calcTurningTime(distance_right));
    }
    else
    {
    	set_rotate_direction(BACKWARD);
      set_rotate(ROTATOR_POWER);
  		wait(calcTurningTime(distance_left));
    }
  }
  else
  {
    block_value = 1;
    set_rotate_direction(FORWARD);
    set_rotate(ROTATOR_POWER);
  	wait(ROTATION_TIME_TWO);
  	set_rotate_direction(BACKWARD);
  	set_rotate(ROTATOR_POWER);
    wait(ROTATION_TIME_TWO);
  }
}