public void GroupSyncSetGoalPosition(byte[] servoIds, ushort[] goalPositions, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
// assert
if (servoIds.Length != goalPositions.Length)
{
throw new ArgumentException($"{nameof(servoIds)} and {nameof(goalPositions)} have to be the same size");
}
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_GOAL_POSITION : ADDR_XL_GOAL_POSITION;
// initi group write
int syncGroupNum = dynamixel.groupSyncWrite(_portNumber, (int)protocol, address, 2);
for (int index = 0; index < servoIds.Length; index++)
{
byte servoId = servoIds[index];
ushort goalPosition = goalPositions[index];
// add parameters to group write
bool success = dynamixel.groupSyncWriteAddParam(syncGroupNum, servoId, goalPosition, 2);
if (!success)
{
throw new IOException("Group write adda param failed");
}
}
// Send commands
dynamixel.groupSyncWriteTxPacket(syncGroupNum);
int commResult = dynamixel.getLastTxRxResult(_portNumber, (int)protocol);
if (commResult != CommSuccess)
{
string servoIdDescriptor = servoIds.OrderBy(id => id).Aggregate(string.Empty, (current, id) => current + $" {id}");
throw new IOException(DynamixelErrorHelper.GetTxRxResultDescription(commResult) + $" group write on:{servoIdDescriptor}");
}
// clear group write cache
dynamixel.groupSyncWriteClearParam(syncGroupNum);
}
private void VerifyLastMessage(byte servoId, DynamixelProtocol protocol)
{
int commResult = dynamixel.getLastTxRxResult(_portNumber, (int)protocol);
if (commResult != CommSuccess)
{
throw new IOException(DynamixelErrorHelper.GetTxRxResultDescription(commResult) + $" on servo: {servoId}");
}
byte dxlError = dynamixel.getLastRxPacketError(_portNumber, (int)protocol);
if (dxlError != 0)
{
throw new IOException(DynamixelErrorHelper.GetRxPackErrorDescription(dxlError) + $" on servo: {servoId}");
}
}
public bool Ping(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
dynamixel.ping(_portNumber, (int)protocol, servoId);
if (dynamixel.getLastTxRxResult(_portNumber, (int)protocol) != CommSuccess)
{
return(false);
}
byte dxlError = dynamixel.getLastRxPacketError(_portNumber, (int)protocol);
if (dxlError != 0)
{
throw new IOException(DynamixelErrorHelper.GetRxPackErrorDescription(dxlError));
}
return(true);
}