/// <summary>
/// Adds a servo to the controller
/// </summary>
/// <param name="ID">Servo ID</param>
/// <param name="mode">JOG mode</param>
public void AddServo(byte ID, HerkulexDescription.JOG_MODE mode, UInt16 initialPosition)
{
Servo servo = new Servo(ID, mode);
Servos.Add(ID, servo);
Servos[ID].SetAbsolutePosition(initialPosition);
//reply to all packets
RAM_WRITE(ID, HerkulexDescription.RAM_ADDR.ACK_Policy, 1, 0x02); //reply to I_JOG / S_JOG
RecoverErrors(servo);
}
private void I_JOG(Servo servo)
{
byte[] dataToSend = new byte[5];
dataToSend[0] = (byte)(servo.GetTargetAbsolutePosition() >> 0);
dataToSend[1] = (byte)(servo.GetTargetAbsolutePosition() >> 8);
dataToSend[2] = servo.GetSETByte();
dataToSend[3] = servo.GetID();
dataToSend[4] = servo.GetPlaytime();
EncodeAndSendPacket(serialPort, servo.GetID(), (byte)HerkulexDescription.CommandSet.I_JOG, dataToSend);
}
/// <summary>
/// Servo polled event
/// </summary>
/// <param name="servo"></param>
public virtual void OnInfosUpdated(Servo servo)
{
var handler = InfosUpdatedEvent;
if (handler != null)
{
handler(this, new InfosUpdatedArgs
{
Servo = servo
});
}
}
/// <summary>
/// Error occured event
/// </summary>
/// <param name="servo"></param>
public virtual void OnHerkulexError(Servo servo)
{
//Ne doit être appelé que si il y a une erreur
var handler = HerkulexErrorEvent;
if (handler != null)
{
handler(this, new HerkulexErrorArgs
{
Servo = servo
});
}
}
//poll (ram read) ack
private void Decoder_OnRamReadAckEvent(object sender, Hklx_RAM_READ_Ack_Args e)
{
Servo pulledServo = null;
UInt16 actualAbsolutePosition = 0;
if (Servos.ContainsKey(e.PID))
{
RamReadAckReceivedEvent.Set();
actualAbsolutePosition = (ushort)(e.ReceivedData[1] << 8);
actualAbsolutePosition += (ushort)(e.ReceivedData[0] << 0);
Servos.TryGetValue(e.PID, out pulledServo);
//getting flags and error details
pulledServo.IsMoving = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.Moving_flag) ? (true) : (false));
pulledServo.IsInposition = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.Inposition_flag) ? (true) : (false));
pulledServo.IsMotorOn = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.MOTOR_ON_flag) ? (true) : (false));
pulledServo.CheckSumError = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.CheckSumError) ? (true) : (false));
pulledServo.UnknownCommandError = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.Unknown_Command) ? (true) : (false));
pulledServo.ExceedRegRangeError = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.Exceed_REG_RANGE) ? (true) : (false));
pulledServo.GarbageDetectedError = (e.StatusDetails.Contains(HerkulexDescription.ErrorStatusDetail.Garbage_detected) ? (true) : (false));
pulledServo.ActualAbsolutePosition = actualAbsolutePosition;
//getting errors
pulledServo.Exceed_input_voltage_limit = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Exceed_input_voltage_limit)) ? (true) : (false);
pulledServo.Exceed_allowed_pot_limit = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Exceed_allowed_pot_limit)) ? (true) : (false);
pulledServo.Exceed_Temperature_limit = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Exceed_Temperature_limit)) ? (true) : (false);
pulledServo.Invalid_packet = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Invalid_packet)) ? (true) : (false);
pulledServo.Overload_detected = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Overload_detected)) ? (true) : (false);
pulledServo.Driver_fault_detected = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.Driver_fault_detected)) ? (true) : (false);
pulledServo.EEP_REG_distorted = (e.StatusErrors.Contains(HerkulexDescription.ErrorStatus.EEP_REG_distorted)) ? (true) : (false);
//if any error flag is true, set HerkulexErrorEvent
if (pulledServo.Exceed_input_voltage_limit == true ||
pulledServo.Exceed_allowed_pot_limit == true ||
pulledServo.Exceed_Temperature_limit == true ||
pulledServo.Invalid_packet == true ||
pulledServo.Overload_detected == true ||
pulledServo.Driver_fault_detected == true ||
pulledServo.EEP_REG_distorted == true)
{
OnHerkulexError(pulledServo);
if (AutoRecoverMode == true)
{
RecoverErrors(pulledServo);
}
}
OnInfosUpdated(pulledServo);
}
}
/// <summary>
/// Recovers the servo from error state
/// </summary>
/// <param name="servo">Servo instance</param>
public void RecoverErrors(Servo servo)
{
ClearAllErrors(servo.GetID());
SetTorqueMode(servo.GetID(), HerkulexDescription.TorqueControl.TorqueOn);
}