Skip to content

Instantly share code, notes, and snippets.

@RQF7
Created December 31, 2018 07:46
Show Gist options
  • Save RQF7/489250db2da5a32c9894ac5ca6f19536 to your computer and use it in GitHub Desktop.
Save RQF7/489250db2da5a32c9894ac5ca6f19536 to your computer and use it in GitHub Desktop.
CULET program for duplicator Turing machine. http://www.comunidad.escom.ipn.mx/ALIROB/CULET
#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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment