void Read(int id, int addr, int len)
{
int result = COMM_TXFAIL, error = 0;
int ivalue = 0;
long lvalue = 0;
if(len == 1)
result = DXL.ReadByte(id, addr, &ivalue, &error);
else if(len == 2)
result = DXL.ReadWord(id, addr, &ivalue, &error);
else if(len == 4)
result = DXL.ReadDWord(id, addr, &lvalue, &error);
if(result != COMM_RXSUCCESS)
{
fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
return;
}
if(error != 0)
PrintErrorCode(error);
if(len == 1)
fprintf(stderr, "\n READ VALUE : %d \n\n", ivalue);
else if(len == 2)
fprintf(stderr, "\n READ VALUE : (UNSIGNED) %u , (SIGNED) %d \n\n", ivalue, ivalue);
else
fprintf(stderr, "\n READ VALUE : (UNSIGNED) %lu , (SIGNED) %d \n\n", lvalue, lvalue);
}
void BeltTest()
{
int result;
long lvalue1 , lvalue2;
while(true)
{
if(kbhit())
break;
result = DXL.ReadDWord(17, 611, &lvalue1, 0);
result = DXL.ReadDWord(18, 611, &lvalue2, 0);
fprintf(stderr, "\n READ VALUE : %d , %d \n\n", lvalue1, lvalue2);
}
}
void Refresh(int id, int addr, int len)
{
int result = COMM_TXFAIL, error = 0;
int ivalue = 0;
long lvalue = 0;
fprintf(stderr, "\n [ESC] : Quit monitoring \n\n");
while(1)
{
if(len == 1)
result = DXL.ReadByte(id, addr, &ivalue, &error);
else if(len == 2)
result = DXL.ReadWord(id, addr, &ivalue, &error);
else if(len == 4)
result = DXL.ReadDWord(id, addr, &lvalue, &error);
if(result != COMM_RXSUCCESS)
{
//fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
continue;
}
if(error != 0)
PrintErrorCode(error);
if(len == 1)
fprintf(stderr, " READ VALUE : %.3d [0x%.2X] \r", ivalue, ivalue);
else if(len == 2)
fprintf(stderr, " READ VALUE : %.5d [0x%.2X 0x%.2X] \r", ivalue, DXL_LOBYTE(ivalue), DXL_HIBYTE(ivalue));
else
fprintf(stderr, " READ VALUE : %.10ld [0x%.2X 0x%.2X 0x%.2X 0x%.2X] \r",
lvalue, DXL_LOBYTE(DXL_LOWORD(lvalue)), DXL_HIBYTE(DXL_LOWORD(lvalue)), DXL_LOBYTE(DXL_HIWORD(lvalue)), DXL_HIBYTE(DXL_HIWORD(lvalue)));
if(kbhit())
{
char c = _getch();
if(c == 0x1b)
break;
}
}
fprintf(stderr, "\n\n");
}
