diff --git a/src/instructions/clear.rs b/src/instructions/clear.rs index c7c5d55..7561e61 100644 --- a/src/instructions/clear.rs +++ b/src/instructions/clear.rs @@ -5,6 +5,11 @@ use crate::{Bus, Response, TransferError, WriteError}; /// The parameters for the CLEAR command to clear the revolution counter. const CLEAR_REVOLUTION_COUNT: [u8; 5] = [0x01, 0x44, 0x58, 0x4C, 0x22]; +/// The parameters for the CLEAR command to clear the error state. +/// +/// This is only supported on some motors. +const CLEAR_ERROR: [u8; 5] = [0x01, 0x45, 0x52, 0x43, 0x4C]; + impl Bus where ReadBuffer: AsRef<[u8]> + AsMut<[u8]>, @@ -22,7 +27,7 @@ where /// /// If you want to broadcast this instruction, it may be more convenient to use [`Self::broadcast_clear_revolution_counter()`] instead. pub fn clear_revolution_counter(&mut self, motor_id: u8) -> Result, TransferError> { - self.write_instruction(motor_id, instruction_id::CLEAR, CLEAR_REVOLUTION_COUNT.len(), encode_parameters)?; + self.write_instruction(motor_id, instruction_id::CLEAR, CLEAR_REVOLUTION_COUNT.len(), clear_revolution_count_parameters)?; Ok(super::read_response_if_not_broadcast(self, motor_id)?) } @@ -36,12 +41,44 @@ where packet_id::BROADCAST, instruction_id::CLEAR, CLEAR_REVOLUTION_COUNT.len(), - encode_parameters, + clear_revolution_count_parameters, + )?; + Ok(()) + } + + /// Clear the error of a motor. + /// + /// This will reset the "error code" register to 0 if the error can be cleared. + /// If the error cannot be cleared, the function returns a [`MotorError`](crate::MotorError) with error code `0x01`. + /// + /// This instruction is currently only implemented on the Dynamixel Y series. + pub fn clear_error(&mut self, motor_id: u8) -> Result, TransferError> { + self.write_instruction(motor_id, instruction_id::CLEAR, CLEAR_ERROR.len(), clear_error_parameters)?; + Ok(super::read_response_if_not_broadcast(self, motor_id)?) + + } + + /// Try to clear the error of all motors on the bus. + /// + /// This will reset the "error code" register to 0 if the error can be cleared + /// and if the instruction is supported by the motor. + /// + /// This instruction is currently only implemented on the Dynamixel Y series. + pub fn broadcast_clear_error(&mut self) -> Result<(), WriteError> { + self.write_instruction( + packet_id::BROADCAST, + instruction_id::CLEAR, + CLEAR_ERROR.len(), + clear_error_parameters, )?; Ok(()) } } -fn encode_parameters(buffer: &mut [u8]) { +fn clear_revolution_count_parameters(buffer: &mut [u8]) { buffer.copy_from_slice(&CLEAR_REVOLUTION_COUNT) } + +fn clear_error_parameters(buffer: &mut [u8]) { + buffer.copy_from_slice(&CLEAR_ERROR) +}