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