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