/// <summary>
/// Sets the torque mode on the servo
/// </summary>
/// <param name="ID">Servo ID</param>
/// <param name="mode">Torque mode</param>
public void SetTorqueMode(ServoId ID, HerkulexDescription.TorqueControl mode)
{
if (Servos.ContainsKey(ID))
{
_SetTorqueMode(ID, mode);
}
}
public void OnStatAck(ServoId pID, byte statusError, byte statusDetail)
{
OnStatAckEvent?.Invoke(this, new HerkulexManagerNS.HerkulexEventArgs.Hklx_STAT_Ack_Args
{
StatusErrors = CommonMethods.GetErrorStatusFromByte(statusError),
StatusDetails = CommonMethods.GetErrorStatusDetailFromByte(statusDetail),
PID = pID
});
}
public void OnAnyAckReveived(ServoId Pid, byte statusError, byte statusDetail)
{
OnAnyAckEvent?.Invoke(this, new HerkulexEventArgs.Hklx_AnyAck_Args
{
PID = Pid,
StatusErrors = CommonMethods.GetErrorStatusFromByte(statusError),
StatusDetails = CommonMethods.GetErrorStatusDetailFromByte(statusDetail)
});
}
public void OnRamReadAck(ServoId pID, byte statusError, byte statusDetail, byte address, byte length, byte[] data)
{
OnRamReadAckEvent?.Invoke(this, new HerkulexManagerNS.HerkulexEventArgs.Hklx_RAM_READ_Ack_Args
{
StatusErrors = CommonMethods.GetErrorStatusFromByte(statusError),
StatusDetails = CommonMethods.GetErrorStatusDetailFromByte(statusDetail),
Address = address,
Length = length,
ReceivedData = data,
PID = pID
});
}
/// <summary>
/// Adds a servo to the controller
/// </summary>
/// <param name="ID">Servo ID</param>
/// <param name="mode">JOG mode</param>
public void AddServo(ServoId ID, HerkulexDescription.JOG_MODE mode)
{
Servo servo = new Servo(ID, mode);
while (!Servos.TryAdd(ID, servo))
{
; //ON tente l'ajout tant qu'il n'est pas validé
}
//reply to all packets
RAM_WRITE(ID, HerkulexDescription.RAM_ADDR.ACK_Policy, 1, 0x02); //reply to I_JOG / S_JOG
RecoverErrors(ID);
}
private void SetPosition(ServoId id, ushort targetPosition, byte playTime)
{
//On clear une éventuelle erreur
RecoverErrors(id);
if (Servos.ContainsKey(id))
{
Servos[id].SetAbsolutePosition(targetPosition);
}
byte[] dataToSend = new byte[5];
dataToSend[0] = (byte)(targetPosition >> 0);
dataToSend[1] = (byte)(targetPosition >> 8);
dataToSend[2] = Servos[id].GetSETByte();
dataToSend[3] = (byte)id;
dataToSend[4] = playTime;
EncodeAndEnqueuePacket(id, (byte)HerkulexDescription.CommandSet.I_JOG, dataToSend);
}
public void ProcessPacket(byte packetSize, ServoId pID, byte cmd, byte checkSum1, byte checkSum2, byte[] packetData, byte statusError, byte statusDetail)
{
int dataLen = packetData.Length;
byte[] readOperationData;
switch (cmd)
{
case (byte)HerkulexDescription.CommandAckSet.ack_RAM_READ:
readOperationData = _GetOnlyDataFromReadOperations(packetData);
OnRamReadAck(pID, statusError, statusDetail, packetData[0], packetData[1], readOperationData);
break;
case (byte)HerkulexDescription.CommandAckSet.ack_STAT:
OnStatAck(pID, statusError, statusDetail);
break;
default:
break;
}
}
private void EncodeAndEnqueuePacket(ServoId pID, byte CMD, byte[] dataToSend)
{
byte packetSize = (byte)(7 + dataToSend.Length);
byte[] packet = new byte[packetSize];
packet[0] = 0xFF;
packet[1] = 0xFF;
packet[2] = packetSize;
packet[3] = (byte)pID;
packet[4] = CMD;
packet[5] = CommonMethods.CheckSum1(packet[2], packet[3], packet[4], dataToSend);
packet[6] = CommonMethods.CheckSum2(packet[5]);
for (int i = 0; i < dataToSend.Length; i++)
{
packet[7 + i] = dataToSend[i];
}
k++;
messageQueue.Enqueue(packet);
}
private void RAM_WRITE(ServoId pID, byte addr, byte length, UInt16 value)
{
if (length > 2)
{
return;
}
byte[] data = new byte[2 + length];
data[0] = (byte)addr;
data[1] = length;
if (length >= 2)
{
data[2] = (byte)(value >> 0); //little endian, LSB first
data[3] = (byte)(value >> 8);
}
else
{
data[2] = (byte)(value);
}
EncodeAndEnqueuePacket(pID, (byte)HerkulexDescription.CommandSet.RAM_WRITE, data);
}
public Servo(ServoId pID, HerkulexDescription.JOG_MODE mode)
{
ID = pID;
Mode = mode;
}
private void RAM_READ(ServoId pID, HerkulexDescription.RAM_ADDR startAddr, byte length)
{
RAM_READ(pID, (byte)startAddr, length);
}
private void RAM_READ(ServoId pID, byte startAddr, byte length)
{
byte[] data = { (byte)startAddr, length };
EncodeAndEnqueuePacket(pID, (byte)HerkulexDescription.CommandSet.RAM_READ, data);
}
private void RAM_WRITE(ServoId pID, HerkulexDescription.RAM_ADDR addr, byte length, UInt16 value)
{
RAM_WRITE(pID, (byte)addr, length, value);
}
/// <summary>
/// Clears all of the servo error statuses
/// </summary>
/// <param name="pID">Servo ID</param>
private void ClearAllErrors(ServoId pID)
{
RAM_WRITE(pID, HerkulexDescription.RAM_ADDR.Status_Error, 1, 0x00);
}
/// <summary>
/// Sets the torque control mode of the specified servo I.e BreakOn / TorqueOn / TorqueFree
/// </summary>
/// <param name="pID">Servo ID</param>
/// <param name="mode">torque mode (TorqueControl enum)</param>
private void _SetTorqueMode(ServoId pID, HerkulexDescription.TorqueControl mode)
{
RAM_WRITE(pID, HerkulexDescription.RAM_ADDR.Torque_Control, 1, (ushort)mode);
}
/// <summary>
/// Recovers the servo from error state
/// </summary>
/// <param name="servo">Servo instance</param>
public void RecoverErrors(ServoId servo)
{
ClearAllErrors(servo);
SetTorqueMode(servo, HerkulexDescription.TorqueControl.TorqueOn);
}
