public void sendTossModeCommand(SerialPort2Dynamixel sp2d)
{
byte[] buffer = { (byte)'t', (byte)'\r' };
sp2d.rawWrite(buffer, 2);
System.Threading.Thread.Sleep(100);
sp2d.rawRead();
}
public short getPosition(SerialPort2Dynamixel sp2d, int id)
{
//byte[] localbuffer = new byte[MaxBufferSize];
int size = getReadPositionCommand(buffer, (byte)id);
byte[] res = sp2d.query(buffer, size, true);
short position = -1;
if (res != null)
{
int length = res.Length;
if (res != null && length > 4 && res[4] == 0)
{
byte l = 0;
byte h = res[5];
if (length > 6)
{
l = res[6];
}
position = fromHexHLConversionToShort(h, l);
}
}
return position;
}
public bool setPosition(SerialPort2Dynamixel sp2d, int id, int goal)
{
bool couldSet = false;
short position = (short)goal;
int size = getSetPositionCommand(buffer, (byte)id, (short)goal);
byte[] res = sp2d.query(buffer, size, false);
//ushort value = 1;
if (res != null && res.Length > 4 && res[4] == 0)
couldSet = true;
return couldSet;
}
