feat(bldc_haptics): add set_position function (#372)

components/bldc_haptics/include/bldc_haptics.hpp

@@ -186,25 +186,26 @@ template <MotorConcept M> 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<int>(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<std::mutex> 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