CULET program for duplicator Turing machine. http://www.comunidad.escom.ipn.mx/ALIROB/CULET

duplicator.c

#include "cubelet.h"

#define DOWN_SENSOR 83755
#define UP_SENSOR 88467
#define THRESHOLD_CERCANY 250
#define DESCENT_TIME_ONE 1200
#define DESCENT_TIME_CERO 700
#define POWER_ROTATOR 76
#define POWER_ROTATOR_RISE 75

int state;
int state_machine;
int direction;

#define RIGHT 1
#define LEFT 2

void setup()
{
  block_value = 0;
  state = 0;
  state_machine = 0;
}

void loop()
{
  switch (state){
    case 0:
      wait(2000);
      state = 1;
      break;
    case 1:
      if (get_block_value(DOWN_SENSOR) < 128) {
        switch (state_machine) {
          case 0:
            state_machine = 1;
            state = 40;
            direction = RIGHT;
            break;
          case 1:
            state_machine = 0;
            state = 40;
            direction = RIGHT;
            break;
          case 2:
            state_machine = 3;
            state = 40;
            direction = LEFT;
            break;
          case 3:
            state_machine = 2;
            state = 40;
            direction = LEFT;
            break;
          case 4:
            state_machine = 5;
            state = 60;
            direction = LEFT;
            break;
          case 5:
            state_machine = 4;
            state = 40;
            direction = LEFT;
            break;
        }
      } else {
        switch (state_machine) {
          case 0:
            state_machine = 4;
            state = 40;
            direction = LEFT;
            break;
          case 1:
            state_machine = 2;
            state = 50;
            direction = LEFT;
            break;
          case 2:
            state_machine = 1;
            state = 50;
            direction = RIGHT;
            break;
          case 3:
            state_machine = 2;
            state = 50;
            direction = LEFT;
            break;
          case 4:
            state_machine = 4;
            state = 40;
            direction = RIGHT;
            break;
        }
      }
    break;
  case 40: 
    if (direction == RIGHT) { 
      state = 41; 
    } else { 
      state = 42; 
    } 
    break; 
  case 41: 
    block_value = 1; 
    if (get_block_value(DOWN_SENSOR) >= THRESHOLD_CERCANY) { 
      state = 43; 
    } 
    break; 
  case 42: 
    block_value = 2; 
    if (get_block_value(DOWN_SENSOR) >= THRESHOLD_CERCANY) { 
      state = 43; 
    } 
    break; 
  case 43: 
    if (get_block_value(DOWN_SENSOR) < THRESHOLD_CERCANY) { 
      if (direction == LEFT)
        wait(200);
      else
        wait(100);
      block_value = 0; 
      wait(200); 
      state = 1; 
    } 
    break; 
  case 50:
    set_rotate_direction(FORWARD);
    set_rotate(POWER_ROTATOR);
    wait(DESCENT_TIME_ONE);
    set_rotate(0);
    wait(500);
    state = 51;
    break;
  case 51:
    set_rotate_direction(BACKWARD);
    set_rotate(POWER_ROTATOR_RISE);
    if (get_block_value(UP_SENSOR) > 128) {
      set_rotate(0);
      wait(500);
      state = 40;
    }
    break;
  case 60:
    set_rotate_direction(BACKWARD);
    set_rotate(POWER_ROTATOR);
    wait(DESCENT_TIME_CERO);
    set_rotate(0);
    wait(500);
    state = 61;
    break;
  case 61:
    set_rotate_direction(FORWARD);
    set_rotate(POWER_ROTATOR_RISE);
    if (get_block_value(UP_SENSOR) > 128) {
      set_rotate(0);
      wait(500);
      state = 40;
    }
    break;
  }
}