Programacion C con CM-510: leyendo y moviendo Dynamixel AX-12 (II)

Programación C con CM-510: leyendo y moviendo Dynamixel AX-12 (II)

En este programa utilizaremos un servo AX-12 para mover otro tal cual movamos el primero. Utilizaremos el ejemplo anterior con algunos cambios para incorporar esta nueva función, como habitualmente ocurre cuando se desarrolla software.

La función que realiza este control es void controlPorAX12(int parametroIdAX12Entrada, int parametroIdAX12Salida):

[sourcecode language="c"]
int esperaMientrasSeSitua(int idAX12)
{
int estado=COMM_TXSUCCESS;
boolean enMovimiento=true;

while (enMovimiento==true)
{
enMovimiento = dxl_read_byte(idAX12, P_MOVING);
estado = dxl_get_result();
if (estado!=COMM_RXSUCCESS)
break;
}

return estado;
}

void controlPorAX12(int parametroIdAX12Entrada, int parametroIdAX12Salida)
{
int posicion=512;

dxl_write_word( parametroIdAX12Entrada, P_GOAL_POSITION_L, posicion);
esperaMientrasSeSitua(parametroIdAX12Entrada);
dxl_write_byte (parametroIdAX12Entrada, P_TORQUE_ENABLE, 0); // quitar toque

dxl_write_word( parametroIdAX12Salida, P_GOAL_POSITION_L, posicion);
esperaMientrasSeSitua(parametroIdAX12Salida);

while(posicion>5 && posicion <1000)
{
posicion = dxl_read_word( parametroIdAX12Entrada, P_PRESENT_POSITION_L );
dxl_write_word( parametroIdAX12Salida, P_GOAL_POSITION_L, posicion );
}
printf ("nFin controlPorAX12 idAXEntrada:%i, posicion: %in", parametroIdAX12Entrada, posicion);
}
[/sourcecode]

Pero previamente hemos de entrar por RoboPlus Terminal el ID del servo AX-12 mando y del que queremos mover.

[sourcecode language="c"]
void datosDesdeOtroAX12()
{
int idAX12Entrada=0;
int idAX12Salida=0;

idAX12Entrada=obtenerId("de entrada");
if (idAX12Entrada!=0)
{
idAX12Salida=obtenerId("de salida");
if (idAX12Salida!=0)
{
printf("nFinaliza llevando el AX12 %i de entrada a su posición inicial (0) o final (1023)n", idAX12Entrada);
controlPorAX12(idAX12Entrada, idAX12Salida);
}
}
}
[/sourcecode]

La función principal (main) quedaría así:

[sourcecode language="c"]
int main(void)
{
init();

// ordenesDesdeTerminal(); el ejemplo anterior refactorizado en una función
datosDesdeOtroAX12();

puts("The End");

return 0;
}
[/sourcecode]

Aquí te puedes descargar el ejemplo completo