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); }