diff --git a/components/bldc_haptics/include/bldc_haptics.hpp b/components/bldc_haptics/include/bldc_haptics.hpp index ac5c27194..13050024e 100644 --- a/components/bldc_haptics/include/bldc_haptics.hpp +++ b/components/bldc_haptics/include/bldc_haptics.hpp @@ -186,25 +186,26 @@ template class BldcHaptics : public BaseComponent { /// @return Current position of the haptic motor float get_position() const { return current_position_; } + /// @brief Sets the current position of the haptic motor to + /// within the bounds of the current detent config + /// @param position Position to set the haptics to + void set_position(const int position) { + current_position_ = std::clamp(position, detent_config_.min_position, detent_config_.max_position); + } + /// @brief Configure the detents for the haptic motor + /// @param config Detent config void update_detent_config(const detail::DetentConfig &config) { std::unique_lock lk(detent_mutex_); // update the detent center current_detent_center_ = motor_.get().get_shaft_angle(); - if (current_position_ < config.min_position) { - // if the current position is less than the min position, set the current - // position to the min position - current_position_ = config.min_position; - } else if (current_position_ > config.max_position) { - // if the current position is greater than the max position, set the - // current position to the max position - current_position_ = config.max_position; - } - // update the detent config detent_config_ = config; + // set position after setting the config + set_position(current_position_); + // Update derivative factor of torque controller based on detent width. If // the D factor is large on coarse detents, the motor ends up making noise // because the P&D factors amplify the noise from the sensor. This is a