El paso a seguir en la programación de piccolo es la creación del comando trasladar. Es un comando que funcionara con motores paso a paso, Estos motores se adaptaron para que puedan trasladar el piccolo a través de cuadrantes de 50 x 50 mm. El objetivo consiste en enviar un comando con dos parámetros, el primer parámetro es los cuadrantes a moverse y el segundo es el parámetro que le da sentido de giro a los motores. también esta otro comando que es el comando de girar el piccolo en su propio eje, el cual recibe dos parámetros, el primer parámetro indica la dirección y el segundo parámetro no hace nada.

En la cabecera del código se definirá las constantes para los comando a ejecutar, se le agrega al código los comando de CM_TRASLADAR y CM_GIRO. también se define la constante de PASOSXCUADRANTE, el cual es el valor a multiplicar para que avance los cuadrantes requeridos. en esta cabecera se llaman los pines par a los motores paso a paso.

#define CM_TRASLADAR 0xF9
#define CM_VERTEX2D 0xFD
#define CM_DRAW 0XFC
#define CM_NODRAW 0xFB
#define CM_STOP 0xFA
#define CM_ENTER 0xF0
#define CM_GIRO 0xF8
 
#define MEM_SIZE 100
#define MEM_TYPE uint32_t //significa que de ahora en más donde diga MEM_TYPE será tipo uint32_t
#define PASOSxCUADRANTE 250 //4096
uint8_t mem_head = 0;
uint8_t mem_tail = 0;
char temp;
bool full = 0;
 
MEM_TYPE buffer[MEM_SIZE];
stepmotor smotor1(D9,D10,D11,D12);
stepmotor smotor2(D2,D6,D7,D8);
Serial command(USBTX, USBRX);

La función de ejecutar cambia y se le agrega el comando CM_TRASLADAR. este comando ejecuta los datos que estén el buffer. Lo primero que realiza es setear la velocidad de los motores, con la variable speed. Luego crea una variable uint32_t llamada cuadrantes, la cual toma el dato del buffer y lo corre 16 bits a la derecha. Lo mismo hace con el siguiente parámetro, Crea una variable booleana y la corre esta vez 8 bits. Estos datos los envía a la función del paso a paso.

void ejecutar()
{
    command.printf("inicio de ejecutar...\n");
    for (int j = 0; j <= mem_head - 1; j++)
    {
        command.printf("%x\n", buffer[j]);
        if (buffer[j] == CM_NODRAW)
        {
            nodraw();
        }
        if (buffer[j] == CM_DRAW)
        {
            draw();
        }
        if (buffer[j] == CM_STOP)
        {
            mem_free();
            return;
        }
        temp = buffer[j] >> 24 & 0xFF;
        if (temp == CM_VERTEX2D)
        {
            uint8_t datoX = buffer[j] >> 16 & 0xFF;
            uint8_t datoY = buffer[j] >> 8 & 0xFF;
            vertex2d(datoX, datoY);
            //wait(1);
        }
        if (temp == CM_TRASLADAR) //no mover si draw se encuentra activo
        {
            uint32_t speed = 1500;
            smotor1.set_speed(speed);
            smotor2.set_speed(speed);
            uint32_t cuadrantes = buffer[j] >> 16 & 0xFF;
            bool direccion = buffer[j] >> 8 & 0xFF;
            smotor1.step(cuadrantes * PASOSxCUADRANTE, direccion);
            smotor2.step(cuadrantes * PASOSxCUADRANTE, direccion);
            wait(2);
        }
    }

En la función guardar también se le agrega la función trasladar para que pueda tomar los comando.

oid guardar()
{
    command.printf("inicio de guardar...\n");
    MEM_TYPE val; //significa que la variable "val" es del tipo uint32_t
    char dato1, dato2;
    do
    {
        dato1 = command.getc();
        if (dato1 == CM_STOP)
        {
            dato2 = command.getc();
            if (dato2 == CM_ENTER)
            {
                val = dato1;
                mem_put(val);
                return;
            }
        }
        if (dato1 == CM_NODRAW | dato1 == CM_DRAW)
        {
            dato2 = command.getc();
            if (dato2 == CM_ENTER)
            {
                val = dato1;
                mem_put(val);
            }
        }
        if (dato1 == CM_VERTEX2D)
        {
            dato2 = command.getc();
            if (dato2 <= 0x32)
            {
                val = CM_VERTEX2D;
                val = val << 8;
                val = val | dato2;
                dato2 = command.getc();
                if (dato2 <= 0x32)
                {
                    val = val << 8;
                    val = val | dato2;
                    dato2 = command.getc();
                    if (dato2 == CM_ENTER)
                    {
                        val = val << 8;
                        val = val | CM_ENTER;
                        mem_put(val);
                    }
                }
            }
        }
        if (dato1 == CM_TRASLADAR)
        {
            dato2 = command.getc();
            if (dato2 <= 0xEF)
            {
                val = CM_TRASLADAR;
                val = val << 8;
                val = val | dato2;
                dato2 = command.getc();
                if (dato2 <= 1)
                {
                    val = val << 8;
                    val = val | dato2;
                    dato2 = command.getc();
                    if (dato2 == CM_ENTER)
                    {
                        val = val << 8;
                        val = val | CM_ENTER;
                        mem_put(val);
                    }
                }
            }
        }

En la funcion Girar en su propio eje

void girar(uint8_t sentido)
{
    command.printf(">>>> %d \n",sentido);
    uint32_t speed = 1500;
    smotor1.set_speed(speed);
    smotor2.set_speed(speed);
    if(sentido==1)
    {
        command.printf("pa la derecha!!\n");
        smotor1.step(7* PASOSxCUADRANTE, 0);
        smotor2.step(7 * PASOSxCUADRANTE, 1);
        
    }else
    {
        command.printf("pa la izquierda!\n");
        smotor1.step(7 * PASOSxCUADRANTE, 1);
        smotor2.step(7 * PASOSxCUADRANTE, 0);   
    }
    wait(2);
}

Principal

En el código principal se llama los comando que se van a utilizar, para este caso se agrega el comando el comando girar.

El comando giro cuando es llamado, entra y el siguiente dato lo guarda en sentido, y luego evalúa sentido. toma nuevamente un dato, y toma el ultimo dato y lo evalúa , si es el comando Enter vaya a la función Girar. y le envía el parámetro de sentido.

int main()
{
    command.baud(9600);
    init_servo();
    //home(); //llama a no_draw y va a 0,0
 
    char read_cc;
    while (1)
    {
        read_cc = command.getc();
        switch (read_cc)
        {
        case CM_GUARDAR:
            if (read_cc = command.getc() == CM_ENTER)
            {
                guardar();
            }
            break; //FE
        case CM_EJECUTAR:
            if (read_cc = command.getc() == CM_ENTER)
            {
                ejecutar();
            }
            break; //FF
        case CM_GIRO:
            uint8_t sentido = command.getc();
            if (sentido <= 1)
            {
                read_cc = command.getc();
                if (read_cc = command.getc() == CM_ENTER)
                {
                    girar(sentido);
            } 
            }   
            break; //F8
        default:
            command.printf("Paila \n");
            break;
        }