Motors program for CULET. http://www.comunidad.escom.ipn.mx/ALIROB/CULET

motor.c

/**
 * motor.c
 * Program for the two drive cubes.
 * CULET: a Turing machine with Cubelets and Lego.
 */

#include "cubelet.h"

#define CONTROLLER 85562
#define MOTOR_POWER 80

void setup()
{
  block_value = 0;
}

void loop()
{
  uint8_t intruction = get_block_value(CONTROLLER);
  switch (intruction) {
    case 0:
      set_drive(0);
      break;
    case 1:
      set_drive_direction(FORWARD);
      set_drive(MOTOR_POWER);
      break;
    case 2:
      set_drive_direction(BACKWARD);
      set_drive(MOTOR_POWER);
      break;
    default:
      break;
  }
}