From ae53a3182512dda1c6e802288ff2783c29155812 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 5 Jun 2026 21:23:35 -0500 Subject: [PATCH 1/3] fix(bldc_motor): Ensure reading the shaft angle doesnt change the filter / update state --- components/bldc_motor/include/bldc_motor.hpp | 50 ++++++++++++-------- 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/components/bldc_motor/include/bldc_motor.hpp b/components/bldc_motor/include/bldc_motor.hpp index f34fb3eb7..d5d8a3734 100644 --- a/components/bldc_motor/include/bldc_motor.hpp +++ b/components/bldc_motor/include/bldc_motor.hpp @@ -482,28 +482,16 @@ class BldcMotor : public BaseComponent { } /** - * @brief Return the shaft angle, in radians. - * @return Motor shaft angle in radians. + * @brief Return the most recently sampled shaft angle, in radians. + * @return Cached motor shaft angle in radians. */ - float get_shaft_angle() { - // if no sensor linked return previous value ( for open loop ) - if (!sensor_) - return shaft_angle_; - auto sensed = angle_filter_ ? angle_filter_(sensor_->get_radians()) : sensor_->get_radians(); - return (float)sensor_direction_ * sensed - sensor_offset_; - } + float get_shaft_angle() { return shaft_angle_; } /** - * @brief Return the shaft velocity, in radians per second (rad/s). - * @return Motor shaft velocity (rad/s). + * @brief Return the most recently sampled shaft velocity, in radians per second (rad/s). + * @return Cached motor shaft velocity (rad/s). */ - float get_shaft_velocity() { - // if no sensor linked return previous value ( for open loop ) - if (!sensor_) - return shaft_velocity_; - auto sensed = velocity_filter_ ? velocity_filter_(sensor_->get_rpm()) : sensor_->get_rpm(); - return (float)sensor_direction_ * sensed * RPM_TO_RADS; - } + float get_shaft_velocity() { return shaft_velocity_; } /** * @brief Get the electrical angle of the motor - using the mechanical @@ -612,13 +600,13 @@ class BldcMotor : public BaseComponent { // representation. if (motion_control_type_ != detail::MotionControlType::ANGLE_OPENLOOP && motion_control_type_ != detail::MotionControlType::VELOCITY_OPENLOOP) { - shaft_angle_ = get_shaft_angle(); + this->sample_shaft_angle(); } // get angular velocity TODO the velocity reading probably also shouldn't // happen in open loop modes? - shaft_velocity_ = get_shaft_velocity(); + this->sample_shaft_velocity(); // set internal target variable target_ = new_target; @@ -738,7 +726,8 @@ class BldcMotor : public BaseComponent { bool success = align_sensor(); if (run_sensor_update_) sensor_->update(ec); - shaft_angle_ = get_shaft_angle(); + this->sample_shaft_angle(); + this->sample_shaft_velocity(); if (current_sense_) { success &= align_current_sense(); @@ -748,6 +737,24 @@ class BldcMotor : public BaseComponent { logger_.debug("Init FOC completed: {}", success); } + float sample_shaft_angle() { + if (!sensor_) { + return shaft_angle_; + } + auto sensed = angle_filter_ ? angle_filter_(sensor_->get_radians()) : sensor_->get_radians(); + shaft_angle_ = (float)sensor_direction_ * sensed - sensor_offset_; + return shaft_angle_; + } + + float sample_shaft_velocity() { + if (!sensor_) { + return shaft_velocity_; + } + auto sensed = velocity_filter_ ? velocity_filter_(sensor_->get_rpm()) : sensor_->get_rpm(); + shaft_velocity_ = (float)sensor_direction_ * sensed * RPM_TO_RADS; + return shaft_velocity_; + } + bool align_sensor() { using namespace std::chrono_literals; int exit_flag = 1; // success @@ -773,6 +780,7 @@ class BldcMotor : public BaseComponent { sensor_->update(ec); std::this_thread::sleep_for(1ms * 2); } + // take an angle in the middle if (run_sensor_update_) sensor_->update(ec); From 2a3df20d1fef0f2b25c07ac29ae2f5fa96a486fd Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 5 Jun 2026 22:55:18 -0500 Subject: [PATCH 2/3] fix sa and comments --- components/bldc_motor/include/bldc_motor.hpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/components/bldc_motor/include/bldc_motor.hpp b/components/bldc_motor/include/bldc_motor.hpp index d5d8a3734..44cdf85c5 100644 --- a/components/bldc_motor/include/bldc_motor.hpp +++ b/components/bldc_motor/include/bldc_motor.hpp @@ -482,16 +482,18 @@ class BldcMotor : public BaseComponent { } /** - * @brief Return the most recently sampled shaft angle, in radians. + * @brief Return the most recently sampled (or estimated in open-loop) shaft + * angle, in radians. * @return Cached motor shaft angle in radians. */ - float get_shaft_angle() { return shaft_angle_; } + float get_shaft_angle() const { return shaft_angle_; } /** - * @brief Return the most recently sampled shaft velocity, in radians per second (rad/s). + * @brief Return the most recently sampled (or estimated in open-loop) shaft + * velocity, in radians per second (rad/s). * @return Cached motor shaft velocity (rad/s). */ - float get_shaft_velocity() { return shaft_velocity_; } + float get_shaft_velocity() const { return shaft_velocity_; } /** * @brief Get the electrical angle of the motor - using the mechanical @@ -724,8 +726,14 @@ class BldcMotor : public BaseComponent { } // align sensor and motor bool success = align_sensor(); - if (run_sensor_update_) + if (run_sensor_update_) { sensor_->update(ec); + if (ec) { + logger_.error("Sensor update failed during init_foc: {}", ec.message()); + status_ = Status::FAILED_CALIBRATION; + return; + } + } this->sample_shaft_angle(); this->sample_shaft_velocity(); From 4bf183077c89de93bb6052c457f6afc5f26a7000 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 5 Jun 2026 23:07:48 -0500 Subject: [PATCH 3/3] fix bldc_haptics after making interface const --- components/bldc_haptics/include/bldc_haptics.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/components/bldc_haptics/include/bldc_haptics.hpp b/components/bldc_haptics/include/bldc_haptics.hpp index 154e93227..e02947578 100644 --- a/components/bldc_haptics/include/bldc_haptics.hpp +++ b/components/bldc_haptics/include/bldc_haptics.hpp @@ -20,11 +20,11 @@ concept MotorConcept = requires { &FOO::move); ///< Move the motor to a new target (position, velocity, or torque depending on ///< the motor control type) static_cast( - &FOO::set_motion_control_type); ///< Set the motion control type - static_cast(&FOO::loop_foc); ///< Run the FOC loop - static_cast(&FOO::get_shaft_angle); ///< Get the shaft angle - static_cast(&FOO::get_shaft_velocity); ///< Get the shaft velocity - static_cast(&FOO::get_electrical_angle); ///< Get the electrical angle + &FOO::set_motion_control_type); ///< Set the motion control type + static_cast(&FOO::loop_foc); ///< Run the FOC loop + static_cast(&FOO::get_shaft_angle); ///< Get the shaft angle + static_cast(&FOO::get_shaft_velocity); ///< Get the shaft velocity + static_cast(&FOO::get_electrical_angle); ///< Get the electrical angle }; /// @brief Class which creates haptic feedback for the user by vibrating the