public ushort GetModelNumber(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort modelNumber = dynamixel.pingGetModelNum(_portNumber, (int)protocol, servoId);
VerifyLastMessage(servoId, protocol);
return(modelNumber);
}
private byte ReadByte(byte servoId, ushort address, DynamixelProtocol protocol)
{
byte incoming = dynamixel.read1ByteTxRx(_portNumber, (int)protocol, servoId, address);
VerifyLastMessage(servoId, protocol);
return(incoming);
}
private ushort ReadUInt16(byte servoId, ushort address, DynamixelProtocol protocol)
{
ushort incoming = dynamixel.read2ByteTxRx(_portNumber, (int)protocol, servoId, address);
VerifyLastMessage(servoId, protocol);
return(incoming);
}
public void SetTorque(byte servoId, bool on, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
byte torqueFlag = on ? (byte)1 : (byte)0;
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_TORQUE_ENABLE : ADDR_XL_TORQUE_ENABLE;
WriteByte(servoId, address, torqueFlag, protocol);
}
public void MoveToBlocking(byte servoId, ushort goal, DynamixelProtocol protocol = DynamixelProtocol.Version1, int querryDelay = 50)
{
SetGoalPosition(servoId, goal);
while (IsMoving(servoId, protocol))
{
Thread.Sleep(querryDelay);
}
}
public void SetServoMode(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
if (protocol != DynamixelProtocol.Version1)
{
throw new NotImplementedException("Support for protocol 1 only");
}
WriteUInt16(servoId, ADDR_MX_CW_ANGLE_LIMIT, 0, protocol);
WriteUInt16(servoId, ADDR_MX_CCW_ANGLE_LIMIT, 1023, protocol);
}
public int GetPresentLoad(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_LOAD : ADDR_XL_PRESENT_LOAD;
ushort loadData = ReadUInt16(servoId, address, protocol);
bool ccw = Convert.ToBoolean(loadData & 1 << 10);
int load = (loadData & ~(1 << 10)) / 10;
load = ccw ? -load : load;
return(load);
}
public void SetId(byte servoId, byte newServoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
if (Ping(newServoId, protocol))
{
throw new InvalidOperationException("New ID alredy taken");
}
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_ID : ADDR_XL_ID;
WriteByte(servoId, address, newServoId, protocol);
}
public void MoveToAll(byte[] servoIds, ushort[] goals, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
if (servoIds.Length != goals.Length)
{
throw new ArgumentException($"{nameof(servoIds)} and {nameof(goals)} have to be the same length");
}
for (int index = 0; index < servoIds.Length; index++)
{
SetGoalPosition(servoIds[index], goals[index], protocol);
}
}
public ServoTelemetrics GetTelemetrics(byte servo, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ServoTelemetrics telemetrics = new ServoTelemetrics
{
Id = servo,
Voltage = GetVoltage(servo, protocol),
Temperature = GetTemperature(servo, protocol),
Load = GetPresentLoad(servo, protocol)
};
return(telemetrics);
}
public byte[] Search(byte startId = 0, byte endId = 252, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
List <byte> found = new List <byte>();
for (byte i = startId; i < endId; i++)
{
if (Ping(i, protocol))
{
found.Add(i);
}
}
return(found.ToArray());
}
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);
}
public void MoveToAllBlocking(byte[] servoIds, ushort[] goals, DynamixelProtocol protocol = DynamixelProtocol.Version1, int querryDelay = 50)
{
if (servoIds.Length != goals.Length)
{
throw new ArgumentException($"{nameof(servoIds)} and {nameof(goals)} have to be the same length");
}
for (int index = 0; index < servoIds.Length; index++)
{
SetGoalPosition(servoIds[index], goals[index], protocol);
}
while (servoIds.Any(index => IsMoving(index, protocol)))
{
Thread.Sleep(querryDelay);
}
}
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 byte GetTemperature(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_TEMP : ADDR_XL_PRESENT_TEMPERATURE;
return(ReadByte(servoId, address, protocol));
}
public void SetTorqueGoal(byte servoId, ushort torque, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_TORQUE_LIMIT : ADDR_XL_GOAL_TORQUE;
WriteUInt16(servoId, address, torque, protocol);
}
public void SetGoalPositionInDegrees(byte servoId, float angle, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
SetGoalPosition(servoId, DegreesToUnits(angle), protocol);
}
public void SetGoalPosition(byte servoId, ushort goalPosition, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_GOAL_POSITION : ADDR_XL_GOAL_POSITION;
WriteUInt16(servoId, address, goalPosition, protocol);
}
private void WriteUInt16(byte servoId, ushort address, ushort data, DynamixelProtocol protocol)
{
dynamixel.write2ByteTxRx(_portNumber, (int)protocol, servoId, address, data);
VerifyLastMessage(servoId, protocol);
}
public void SetCwMinAngleLimit(byte servoId, ushort minAngle, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
WriteUInt16(servoId, ADDR_MX_CW_ANGLE_LIMIT, minAngle, protocol);
}
public void GroupSyncSetGoalPositionInDegrees(byte[] servoIds, float[] goalPositionsInDegrees, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort[] goalPositions = goalPositionsInDegrees.Select(DegreesToUnits).ToArray();
GroupSyncSetGoalPosition(servoIds, goalPositions, protocol);
}
public void SetMovingSpeed(byte servoId, ushort movingSpeed, bool cw = false, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
if (movingSpeed > 1023)
{
throw new ArgumentOutOfRangeException($"{nameof(movingSpeed)} is too high. Max value is 1023");
}
if (movingSpeed > 0 && cw) // if speed zero and bit for CW set true servo mode won't work
{
movingSpeed |= 1 << 10; // set direction bit high
}
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_MOVING_SPEED : ADDR_XL_GOAL_VELOCITY;
WriteUInt16(servoId, address, movingSpeed, protocol);
}
public float GetPresentPositionInDegrees(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
return(UnitsToDegrees(GetPresentPosition(servoId, protocol)));
}
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);
}
public float GetVoltage(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_VOLTAGE : ADDR_XL_PRESENT_VOLTAGE;
return(ReadByte(servoId, address, protocol) / 10f);
}
public ushort GetPresentPosition(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_POSITION : ADDR_XL_PRESENT_POSITION;
return(ReadUInt16(servoId, address, protocol));
}
public void SetComplianceMargin(byte servoId, byte complianceMargin, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
SetClockwiseComplianceMargin(servoId, complianceMargin, protocol);
SetCounterClockwiseComplianceMargin(servoId, complianceMargin, protocol);
}
public ushort GetPresentSpeed(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_SPEED : ADDR_XL_PRESENT_VELOCITY;
return(ReadUInt16(servoId, address, protocol));
}
public static void SwitchId(byte servo1Id, byte servo2Id, DynamixelDriver driver, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
// find empty index
byte freeIndex = 255;
for (byte index = 1; index < 252; index++)
{
if (!driver.Ping(index, protocol))
{
freeIndex = index;
break;
}
}
if (freeIndex == 255)
{
throw new InvalidOperationException("Can't find free ID");
}
driver.SetId(servo1Id, freeIndex, protocol);
driver.SetId(servo2Id, servo1Id, protocol);
driver.SetId(freeIndex, servo2Id, protocol);
}
public bool IsMoving(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
{
ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_PRESENT_MOVING : ADDR_XL_PRESENT_MOVING;
return(ReadByte(servoId, address, protocol) > 0);
}
