From 23ea1da35a15c67b4714c7a57279ff7632ad22a9 Mon Sep 17 00:00:00 2001 From: Eric D'Urso Date: Wed, 13 May 2026 19:17:40 -0400 Subject: [PATCH 1/5] initial rev servo lims --- CMakeLists.txt | 5 +- config/esw.yaml | 2 + config/mast_gimbal.yaml | 3 +- config/science.yaml | 2 + esw/lim/lim.hpp | 49 +++++++++++++ esw/mast_gimbal_hw_bridge.cpp | 33 ++++++++- esw/science_hw_bridge.cpp | 21 ++++++ esw/servo/servo.hpp | 130 +++++++++++++++++++++++++++++++--- 8 files changed, 231 insertions(+), 14 deletions(-) create mode 100644 esw/lim/lim.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 34d11d6d..88c1206f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -340,10 +340,13 @@ if (EXISTS ${MROVER_EMBEDDED_ROOT_DIR} AND NOT APPLE) mrover_add_header_only_library(abs esw/abs) target_link_libraries(abs INTERFACE can_device units parameter_utils) + mrover_add_header_only_library(lim esw/lim) + target_link_libraries(lim INTERFACE can_device units parameter_utils) + macro(mrover_add_esw_bridge_node name sources) mrover_add_node(${name} ${sources}) ament_target_dependencies(${name} rclcpp std_srvs dynamixel_sdk) - target_link_libraries(${name} can_device units motor science servo abs) + target_link_libraries(${name} can_device units motor science servo abs lim) endmacro() # esw hardware bridge nodes diff --git a/config/esw.yaml b/config/esw.yaml index 83d386e0..d8f39279 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -26,6 +26,8 @@ can_bridge_0: id: 0x10 pdlb: id: 0x11 + limit_board: + id: 0x12 front_left: id: 0x20 middle_left: diff --git a/config/mast_gimbal.yaml b/config/mast_gimbal.yaml index b91290b5..beab586f 100644 --- a/config/mast_gimbal.yaml +++ b/config/mast_gimbal.yaml @@ -15,10 +15,10 @@ mast_gimbal_hw_bridge: current_limit: 1750.0 profile_acceleration: 10.0 profile_velocity: 60.0 - boot_position: 0.0 gimbal_yaw: id: 4 mode: "limited" + use_hw_limits: true operating_mode: "extended_position" position_multiplier: 6.25 forward_limit: 6.108 @@ -31,4 +31,3 @@ mast_gimbal_hw_bridge: current_limit: 1000.0 profile_acceleration: 100.0 profile_velocity: 300.0 - boot_position: 3.14 diff --git a/config/science.yaml b/config/science.yaml index 739a3fec..cc4490d2 100644 --- a/config/science.yaml +++ b/config/science.yaml @@ -3,6 +3,8 @@ science_hw_bridge: funnel: id: 5 mode: "optimal" + use_index_homing: true + index_limit: 0.0 operating_mode: "extended_position" position_multiplier: 5.859375 position_p: 6000.0 diff --git a/esw/lim/lim.hpp b/esw/lim/lim.hpp new file mode 100644 index 00000000..310f8361 --- /dev/null +++ b/esw/lim/lim.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include "MRoverCAN.hpp" +#include "can_device.hpp" +#include +#include + +namespace mrover { + + class LimitSwitchBoard { + rclcpp::Node::SharedPtr mNode; + CANDevice mDevice; + + bool mLimitAPressed{}; + bool mLimitBPressed{}; + + public: + LimitSwitchBoard(rclcpp::Node::SharedPtr node, std::string masterName, std::string deviceName) + : mNode{std::move(node)}, + mDevice{mNode, std::move(masterName), std::move(deviceName), + [this](CANMsg_t const& msg) -> void { processMessage(msg); }} { + } + + void processMessage(CANMsg_t const& msg) { + std::visit([this](auto const& decoded) -> void { + using T = std::decay_t; + + if constexpr (std::is_same_v) { + // update local state + mLimitAPressed = decoded.lim_a; + mLimitBPressed = decoded.lim_b; + } else { + RCLCPP_WARN(mNode->get_logger(), "limit switch board received unexpected message type %s", typeid(T).name()); + } + }, + msg); + } + + [[nodiscard]] auto getLimitA() const -> bool { + return mLimitAPressed; + } + + [[nodiscard]] auto getLimitB() const -> bool { + return mLimitBPressed; + } + + }; + +} // namespace mrover diff --git a/esw/mast_gimbal_hw_bridge.cpp b/esw/mast_gimbal_hw_bridge.cpp index 1256b725..e4c4f14f 100644 --- a/esw/mast_gimbal_hw_bridge.cpp +++ b/esw/mast_gimbal_hw_bridge.cpp @@ -10,9 +10,11 @@ #include #include +#include #include #include +#include namespace mrover { @@ -36,6 +38,8 @@ namespace mrover { mControllerState.names.push_back(servoName); } + mYawLimBoard = std::make_shared(shared_from_this(), "jetson", "limit_board"); + mTimerGroup = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); mServiceGroup = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -50,26 +54,53 @@ namespace mrover { rmw_qos_profile_services_default, mServiceGroup); + mHomingService = this->create_service( + "home_gimbal_yaw", + [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { + if (mServos.count("gimbal_yaw") > 0) { + mServos.at("gimbal_yaw")->startHoming(true); + res->success = true; + res->message = "Homing initiated for gimbal yaw (moving towards reverse limit)."; + } else { + res->success = false; + res->message = "gimbal_yaw servo not found."; + } + }, + rmw_qos_profile_services_default, + mServiceGroup + ); + mPublishTimer = this->create_wall_timer( std::chrono::milliseconds(100), [this]() -> void { publishDataCallback(); }, mTimerGroup); + mHWLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { + if (mServos.count("gimbal_yaw") > 0 && mYawLimBoard) { + mServos.at("gimbal_yaw")->updateHardwareLimits( + mYawLimBoard->getLimitA(), + mYawLimBoard->getLimitB() + ); + } + }, mTimerGroup); + mGimbalStatePub = this->create_publisher("gimbal_controller_state", 10); } private: std::vector mServoNames = {"gimbal_pitch", "gimbal_yaw"}; std::unordered_map> mServos; - + std::shared_ptr mYawLimBoard; std::string mU2D2DeviceName; rclcpp::CallbackGroup::SharedPtr mServiceGroup; rclcpp::CallbackGroup::SharedPtr mTimerGroup; rclcpp::Service::SharedPtr mPositionService; + rclcpp::Service::SharedPtr mHomingService; rclcpp::Publisher::SharedPtr mGimbalStatePub; rclcpp::TimerBase::SharedPtr mPublishTimer; + rclcpp::TimerBase::SharedPtr mHWLimitTimer; msg::ControllerState mControllerState; auto servoPositionCallback(srv::ServoPosition::Request::SharedPtr const& req, srv::ServoPosition::Response::SharedPtr const& res) -> void { diff --git a/esw/science_hw_bridge.cpp b/esw/science_hw_bridge.cpp index eb11efda..387143c6 100644 --- a/esw/science_hw_bridge.cpp +++ b/esw/science_hw_bridge.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -51,6 +52,24 @@ namespace mrover { rmw_qos_profile_services_default, mServiceGroup); + mFunnelHoming = this->create_service( + "sp_home_funnel", + [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { + mFunnelServo->startHoming(true); + res->success = true; + res->message = "Homing initiated for funnel."; + }, + rmw_qos_profile_services_default, + mServiceGroup + ); + + mFunnelLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { + if (mAuger) { + bool funnelIndexHit = (mAuger->getLimitsHitBits() & 0x01) != 0; + mFunnelServo->updateIndexLimit(funnelIndexHit); + } + }); + mSPThrottleSub = create_subscription("sp_thr_cmd", 1, [this](msg::Throttle::ConstSharedPtr const& msg) -> void { processThrottleCmd(msg); }); mPublishDataTimer = create_wall_timer( @@ -87,6 +106,8 @@ namespace mrover { rclcpp::CallbackGroup::SharedPtr mServiceGroup; rclcpp::Service::SharedPtr mFunnelPositionService; + rclcpp::TimerBase::SharedPtr mFunnelLimitTimer; + rclcpp::Service::SharedPtr mFunnelHoming; rclcpp::TimerBase::SharedPtr mPublishDataTimer; rclcpp::Subscription::SharedPtr mSPThrottleSub; rclcpp::Publisher::SharedPtr mControllerStatePub; diff --git a/esw/servo/servo.hpp b/esw/servo/servo.hpp index 69894cb3..4919fa76 100644 --- a/esw/servo/servo.hpp +++ b/esw/servo/servo.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include @@ -47,7 +48,7 @@ namespace mrover { double mPositionMultiplier{1}; uint8_t mServoID{}; uint8_t mAtLimit{0}; - ServoPosition mBootPosition; + // ServoPosition mBootPosition; rclcpp::Node::SharedPtr mNode; rclcpp::Publisher::SharedPtr mConfigPub; @@ -60,7 +61,18 @@ namespace mrover { std::atomic mCachedRawCurrent{0}; std::atomic mCachedStatus{U2D2::Status::CommRxWaiting}; - public: + bool mUseHardwareLimits{false}; + std::atomic mIsHomed{true}; + std::atomic mHwFwdHit{false}; + std::atomic mHwRevHit{false}; + double mForwardLimitRad{0}; + double mReverseLimitRad{0}; + + bool mUseIndexHoming{false}; + std::atomic mIndexHit{false}; + double mIndexLimitRad{0}; + + public: enum class ServoProperty { PositionPGain = 84, PositionIGain = 82, @@ -113,10 +125,10 @@ namespace mrover { mCachedRawCurrent = msg->current; mCachedStatus = static_cast(msg->status); - if (!mHasReceivedData) { - mHasReceivedData = true; - setCurrentPosition(mBootPosition); - } + // if (!mHasReceivedData) { + // mHasReceivedData = true; + // setCurrentPosition(mBootPosition); + // } }); int servoID; @@ -161,6 +173,10 @@ namespace mrover { } auto getPosition(double& pos) const -> U2D2::Status { + if (!mIsHomed) { + pos = std::numeric_limits::quiet_NaN(); + return mCachedStatus.load(); + } auto currentPositionAndStatus = getCurrentServoPosition(); pos = (static_cast(currentPositionAndStatus.first) / SERVO_TICKS) * TAU; return currentPositionAndStatus.second; @@ -182,6 +198,7 @@ namespace mrover { } auto setPosition(ServoPosition const position) -> U2D2::Status { + if ((mUseHardwareLimits || mUseIndexHoming) && !mIsHomed) return U2D2::Status::Active; mGoalPosition = static_cast((position / TAU) * static_cast(SERVO_TICKS)); auto currentPositionAndStatus = getCurrentServoPosition(); @@ -192,7 +209,19 @@ namespace mrover { mAtLimit = 0; - if (mMode == ServoMode::Limited) { + if (mUseHardwareLimits) { + auto const currentRaw = currentPositionAndStatus.first; + if (mHwFwdHit.load() && mGoalPosition > currentRaw) { + mGoalPosition = currentRaw; + mAtLimit |= 0x01; + } + if (mHwRevHit.load() && mGoalPosition < currentRaw) { + mGoalPosition = currentRaw; + mAtLimit |= 0x02; + } + publishWrite(ADDR_GOAL_POSITION, 4, offsetToRaw(mGoalPosition)); + + } else if (mMode == ServoMode::Limited) { if (!isWithinLimits(mGoalPosition)) { mGoalPosition = clampToLimits(mGoalPosition); } @@ -222,10 +251,17 @@ namespace mrover { } auto setVelocityTarget(ServoVelocity const velocity) -> U2D2::Status { + if ((mUseHardwareLimits || mUseIndexHoming) && !mIsHomed) return U2D2::Status::Active; mLastCommandTimeNs = mNode->now().nanoseconds(); mVelocityTimedOut = false; - double const rpm = velocity * (60.0 / TAU); + double targetVel = velocity; + if (mUseHardwareLimits) { + if (mHwFwdHit.load() && targetVel > 0) targetVel = 0.0; + if (mHwRevHit.load() && targetVel < 0) targetVel = 0.0; + } + + double const rpm = targetVel * (60.0 / TAU); auto const velocityTicks = static_cast(rpm / SERVO_RPM_PER_TICK); publishWrite(ADDR_GOAL_VELOCITY, 4, static_cast(velocityTicks)); @@ -255,6 +291,70 @@ namespace mrover { return rpm * (TAU / 60.0); } + auto startHoming(bool forward) -> void { + if (!mUseHardwareLimits && !mUseIndexHoming) return; + double homingTarget = 0.0; + if (mUseHardwareLimits) { + homingTarget = forward ? (mForwardLimitRad + 10.0) : (mReverseLimitRad - 10.0); + } else if (mUseIndexHoming) { + auto currentPositionAndStatus = getCurrentServoPosition(); + double currentPos = (static_cast(currentPositionAndStatus.first) / SERVO_TICKS) * TAU; + homingTarget = forward ? (currentPos + 100.0) : (currentPos - 100.0); + } + auto const targetTicks = static_cast((homingTarget / TAU) * static_cast(SERVO_TICKS)); + mGoalPosition = targetTicks; + publishWrite(ADDR_GOAL_POSITION, 4, offsetToRaw(targetTicks)); + } + + auto updateHardwareLimits(bool fwd_hit, bool rev_hit) -> void { + if (!mUseHardwareLimits) return; + + bool const prevFwd = mHwFwdHit.exchange(fwd_hit); + bool const prevRev = mHwRevHit.exchange(rev_hit); + + if (fwd_hit && !prevFwd) snapToLimit(mForwardLimitRad, true); + if (rev_hit && !prevRev) snapToLimit(mReverseLimitRad, true); + } + + auto updateIndexLimit(bool hit) -> void { + if (!mUseIndexHoming) return; + bool const prevHit = mIndexHit.exchange(hit); + if (hit && !prevHit) { + bool wasHomed = mIsHomed.load(); + snapToLimit(mIndexLimitRad, !wasHomed); + } + } + + auto snapToLimit(double limitRad, bool halt = true) -> void { + auto currentPositionAndStatus = getCurrentServoPosition(); + double currentPos = (static_cast(currentPositionAndStatus.first) / SERVO_TICKS) * TAU; + double rotations = std::round((currentPos - limitRad) / TAU); + double exactPosition = limitRad + rotations * TAU; + setCurrentPosition(exactPosition); + mIsHomed.store(true); + if (halt) { + if (mOperatingMode == OperatingMode::Velocity) { + publishWrite(ADDR_GOAL_VELOCITY, 4, 0); + } else { + mGoalPosition = rawToOffset(mCachedRawPosition.load()); + publishWrite(ADDR_GOAL_POSITION, 4, offsetToRaw(mGoalPosition)); + } + } + } + + void updateHardwareLimits() { + int64_t ticks = rawToOffset(mCachedRawPosition.load()); + double currentPos = (static_cast(ticks) / SERVO_TICKS) * TAU; + if (mHwFwdHit.load() && mGoalPosition > currentPos) { + mGoalPosition = currentPos; + mAtLimit |= 0x01; + } + if (mHwRevHit.load() && mGoalPosition < currentPos) { + mGoalPosition = currentPos; + mAtLimit |= 0x02; + } + } + private: auto publishWrite(uint8_t addr, uint8_t len, uint32_t val) const -> void { msg::ServoIn msg; @@ -266,6 +366,9 @@ namespace mrover { auto updateConfigFromParameters() -> void { std::string modeString; + bool useIndexHoming; + double indexLimit; + bool useHwLimits; std::string operatingModeStr; double forwardLimit; double reverseLimit; @@ -281,6 +384,9 @@ namespace mrover { std::vector parameters = { {std::format("{}.mode", mServoName), modeString, std::string("optimal")}, + {std::format("{}.use_index_homing", mServoName), useIndexHoming, false}, + {std::format("{}.index_limit", mServoName), indexLimit, 0.0}, + {std::format("{}.use_hw_limits", mServoName), useHwLimits, false}, {std::format("{}.operating_mode", mServoName), operatingModeStr, std::string("position")}, {std::format("{}.position_multiplier", mServoName), mPositionMultiplier, 1.0}, {std::format("{}.reverse_limit", mServoName), reverseLimit, 0.0}, @@ -293,8 +399,8 @@ namespace mrover { {std::format("{}.current_limit", mServoName), currentLimit, 1750.0}, {std::format("{}.velocity_limit", mServoName), velocityLimit, 445.0}, {std::format("{}.profile_acceleration", mServoName), profileAcceleration, 100.0}, - {std::format("{}.profile_velocity", mServoName), profileVelocity, 100.0}, - {std::format("{}.boot_position", mServoName), mBootPosition, 0.0}}; + {std::format("{}.profile_velocity", mServoName), profileVelocity, 100.0}}; + // {std::format("{}.boot_position", mServoName), mBootPosition, 0.0}}; ParameterWrapper::declareParameters(mNode.get(), parameters); @@ -330,6 +436,10 @@ namespace mrover { (void) setProperty(ServoProperty::ProfileAcceleration, static_cast(profileAcceleration)); (void) setProperty(ServoProperty::ProfileVelocity, static_cast(profileVelocity)); + mUseHardwareLimits = useHwLimits; + mUseIndexHoming = useIndexHoming; + mIndexLimitRad = indexLimit; + if (mUseHardwareLimits || mUseIndexHoming) mIsHomed = false; mAdjustedReverseLimit = static_cast((reverseLimit / TAU) * SERVO_TICKS); mAdjustedForwardLimit = static_cast((forwardLimit / TAU) * SERVO_TICKS); } From 229df56b44579f44ee95e1531b08c576e878532e Mon Sep 17 00:00:00 2001 From: Eric D'Urso Date: Wed, 13 May 2026 22:30:25 -0400 Subject: [PATCH 2/5] test on mast gimbal note: lowered allowable pwm off yaw servo to 400 --- config/mast_gimbal.yaml | 8 ++--- esw/lim/lim.hpp | 1 - esw/mast_gimbal_hw_bridge.cpp | 36 ++++++++++---------- esw/science_hw_bridge.cpp | 17 +++++----- esw/servo/servo.hpp | 63 ++++++++++++++++------------------- launch/jetson_can.launch.py | 8 ++++- 6 files changed, 64 insertions(+), 69 deletions(-) diff --git a/config/mast_gimbal.yaml b/config/mast_gimbal.yaml index beab586f..f5ac0f98 100644 --- a/config/mast_gimbal.yaml +++ b/config/mast_gimbal.yaml @@ -23,11 +23,11 @@ mast_gimbal_hw_bridge: position_multiplier: 6.25 forward_limit: 6.108 reverse_limit: 0.174 - position_p: 4000.0 + position_p: 2000.0 position_i: 0.0 position_d: 0.0 velocity_p: 0.0 velocity_i: 0.0 - current_limit: 1000.0 - profile_acceleration: 100.0 - profile_velocity: 300.0 + current_limit: 300.0 + profile_acceleration: 20.0 + profile_velocity: 150.0 diff --git a/esw/lim/lim.hpp b/esw/lim/lim.hpp index 310f8361..3876144e 100644 --- a/esw/lim/lim.hpp +++ b/esw/lim/lim.hpp @@ -43,7 +43,6 @@ namespace mrover { [[nodiscard]] auto getLimitB() const -> bool { return mLimitBPressed; } - }; } // namespace mrover diff --git a/esw/mast_gimbal_hw_bridge.cpp b/esw/mast_gimbal_hw_bridge.cpp index e4c4f14f..49433eeb 100644 --- a/esw/mast_gimbal_hw_bridge.cpp +++ b/esw/mast_gimbal_hw_bridge.cpp @@ -12,9 +12,9 @@ #include #include +#include #include #include -#include namespace mrover { @@ -55,20 +55,19 @@ namespace mrover { mServiceGroup); mHomingService = this->create_service( - "home_gimbal_yaw", - [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { - if (mServos.count("gimbal_yaw") > 0) { - mServos.at("gimbal_yaw")->startHoming(true); - res->success = true; - res->message = "Homing initiated for gimbal yaw (moving towards reverse limit)."; - } else { - res->success = false; - res->message = "gimbal_yaw servo not found."; - } - }, - rmw_qos_profile_services_default, - mServiceGroup - ); + "home_gimbal_yaw", + [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { + if (mServos.count("gimbal_yaw") > 0) { + mServos.at("gimbal_yaw")->startHoming(false); + res->success = true; + res->message = "homing initiated"; + } else { + res->success = false; + res->message = "gimbal_yaw servo not found"; + } + }, + rmw_qos_profile_services_default, + mServiceGroup); mPublishTimer = this->create_wall_timer( std::chrono::milliseconds(100), @@ -78,11 +77,10 @@ namespace mrover { mHWLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { if (mServos.count("gimbal_yaw") > 0 && mYawLimBoard) { mServos.at("gimbal_yaw")->updateHardwareLimits( - mYawLimBoard->getLimitA(), - mYawLimBoard->getLimitB() + mYawLimBoard->getLimitB(), + mYawLimBoard->getLimitA() ); - } - }, mTimerGroup); + } }, mTimerGroup); mGimbalStatePub = this->create_publisher("gimbal_controller_state", 10); } diff --git a/esw/science_hw_bridge.cpp b/esw/science_hw_bridge.cpp index 387143c6..78b5111e 100644 --- a/esw/science_hw_bridge.cpp +++ b/esw/science_hw_bridge.cpp @@ -53,15 +53,14 @@ namespace mrover { mServiceGroup); mFunnelHoming = this->create_service( - "sp_home_funnel", - [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { - mFunnelServo->startHoming(true); - res->success = true; - res->message = "Homing initiated for funnel."; - }, - rmw_qos_profile_services_default, - mServiceGroup - ); + "sp_home_funnel", + [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { + mFunnelServo->startHoming(true); + res->success = true; + res->message = "Homing initiated for funnel."; + }, + rmw_qos_profile_services_default, + mServiceGroup); mFunnelLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { if (mAuger) { diff --git a/esw/servo/servo.hpp b/esw/servo/servo.hpp index 4919fa76..23a95976 100644 --- a/esw/servo/servo.hpp +++ b/esw/servo/servo.hpp @@ -4,13 +4,13 @@ #include #include #include +#include #include #include #include #include #include #include -#include #include @@ -48,7 +48,6 @@ namespace mrover { double mPositionMultiplier{1}; uint8_t mServoID{}; uint8_t mAtLimit{0}; - // ServoPosition mBootPosition; rclcpp::Node::SharedPtr mNode; rclcpp::Publisher::SharedPtr mConfigPub; @@ -72,7 +71,7 @@ namespace mrover { std::atomic mIndexHit{false}; double mIndexLimitRad{0}; - public: + public: enum class ServoProperty { PositionPGain = 84, PositionIGain = 82, @@ -124,11 +123,6 @@ namespace mrover { mCachedRawVelocity = msg->velocity; mCachedRawCurrent = msg->current; mCachedStatus = static_cast(msg->status); - - // if (!mHasReceivedData) { - // mHasReceivedData = true; - // setCurrentPosition(mBootPosition); - // } }); int servoID; @@ -284,7 +278,14 @@ namespace mrover { return U2D2::Status::Active; } - [[nodiscard]] auto getLimitStatus() const -> uint8_t { return mAtLimit; } + [[nodiscard]] auto getLimitStatus() const -> uint8_t { + uint8_t status = mAtLimit; + if (mUseHardwareLimits) { + if (mHwFwdHit.load()) status |= 0x01; + if (mHwRevHit.load()) status |= 0x02; + } + return status; + } [[nodiscard]] auto getVelocityLimitRadPerSec() const -> double { double const rpm = mVelocityLimit * SERVO_RPM_PER_TICK; @@ -302,7 +303,7 @@ namespace mrover { homingTarget = forward ? (currentPos + 100.0) : (currentPos - 100.0); } auto const targetTicks = static_cast((homingTarget / TAU) * static_cast(SERVO_TICKS)); - mGoalPosition = targetTicks; + mGoalPosition = targetTicks; publishWrite(ADDR_GOAL_POSITION, 4, offsetToRaw(targetTicks)); } @@ -312,8 +313,8 @@ namespace mrover { bool const prevFwd = mHwFwdHit.exchange(fwd_hit); bool const prevRev = mHwRevHit.exchange(rev_hit); - if (fwd_hit && !prevFwd) snapToLimit(mForwardLimitRad, true); - if (rev_hit && !prevRev) snapToLimit(mReverseLimitRad, true); + if (fwd_hit && !prevFwd) snapToLimit(mForwardLimitRad, true, true); + if (rev_hit && !prevRev) snapToLimit(mReverseLimitRad, true, true); } auto updateIndexLimit(bool hit) -> void { @@ -321,40 +322,31 @@ namespace mrover { bool const prevHit = mIndexHit.exchange(hit); if (hit && !prevHit) { bool wasHomed = mIsHomed.load(); - snapToLimit(mIndexLimitRad, !wasHomed); + snapToLimit(mIndexLimitRad, !wasHomed, false); } } - auto snapToLimit(double limitRad, bool halt = true) -> void { - auto currentPositionAndStatus = getCurrentServoPosition(); - double currentPos = (static_cast(currentPositionAndStatus.first) / SERVO_TICKS) * TAU; - double rotations = std::round((currentPos - limitRad) / TAU); - double exactPosition = limitRad + rotations * TAU; - setCurrentPosition(exactPosition); + auto snapToLimit(double limitRad, bool halt = true, bool forceExact = false) -> void { + if (forceExact) { + setCurrentPosition(limitRad); + } else { + auto currentPositionAndStatus = getCurrentServoPosition(); + double currentPos = (static_cast(currentPositionAndStatus.first) / SERVO_TICKS) * TAU; + double rotations = std::round((currentPos - limitRad) / TAU); + double exactPosition = limitRad + rotations * TAU; + setCurrentPosition(exactPosition); + } mIsHomed.store(true); if (halt) { if (mOperatingMode == OperatingMode::Velocity) { publishWrite(ADDR_GOAL_VELOCITY, 4, 0); } else { - mGoalPosition = rawToOffset(mCachedRawPosition.load()); + mGoalPosition = rawToOffset(mCachedRawPosition.load()); publishWrite(ADDR_GOAL_POSITION, 4, offsetToRaw(mGoalPosition)); } } } - void updateHardwareLimits() { - int64_t ticks = rawToOffset(mCachedRawPosition.load()); - double currentPos = (static_cast(ticks) / SERVO_TICKS) * TAU; - if (mHwFwdHit.load() && mGoalPosition > currentPos) { - mGoalPosition = currentPos; - mAtLimit |= 0x01; - } - if (mHwRevHit.load() && mGoalPosition < currentPos) { - mGoalPosition = currentPos; - mAtLimit |= 0x02; - } - } - private: auto publishWrite(uint8_t addr, uint8_t len, uint32_t val) const -> void { msg::ServoIn msg; @@ -367,7 +359,7 @@ namespace mrover { auto updateConfigFromParameters() -> void { std::string modeString; bool useIndexHoming; - double indexLimit; + double indexLimit; bool useHwLimits; std::string operatingModeStr; double forwardLimit; @@ -400,7 +392,6 @@ namespace mrover { {std::format("{}.velocity_limit", mServoName), velocityLimit, 445.0}, {std::format("{}.profile_acceleration", mServoName), profileAcceleration, 100.0}, {std::format("{}.profile_velocity", mServoName), profileVelocity, 100.0}}; - // {std::format("{}.boot_position", mServoName), mBootPosition, 0.0}}; ParameterWrapper::declareParameters(mNode.get(), parameters); @@ -439,6 +430,8 @@ namespace mrover { mUseHardwareLimits = useHwLimits; mUseIndexHoming = useIndexHoming; mIndexLimitRad = indexLimit; + mForwardLimitRad = forwardLimit; + mReverseLimitRad = reverseLimit; if (mUseHardwareLimits || mUseIndexHoming) mIsHomed = false; mAdjustedReverseLimit = static_cast((reverseLimit / TAU) * SERVO_TICKS); mAdjustedForwardLimit = static_cast((forwardLimit / TAU) * SERVO_TICKS); diff --git a/launch/jetson_can.launch.py b/launch/jetson_can.launch.py index 71fe0823..4b90c2be 100644 --- a/launch/jetson_can.launch.py +++ b/launch/jetson_can.launch.py @@ -41,4 +41,10 @@ def generate_launch_description(): ], ) - return LaunchDescription([can_bridge_0_node, can_bridge_1_node, can_bridge_2_node]) + return LaunchDescription( + [ + can_bridge_0_node, + # can_bridge_1_node, + # can_bridge_2_node, + ] + ) From f47e08b363e038e2711d430feff4af3055e89940 Mon Sep 17 00:00:00 2001 From: Eric D'Urso Date: Wed, 13 May 2026 22:38:29 -0400 Subject: [PATCH 3/5] notes --- esw/science_hw_bridge.cpp | 4 ++-- esw/servo/servo.hpp | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/esw/science_hw_bridge.cpp b/esw/science_hw_bridge.cpp index 78b5111e..93b5c075 100644 --- a/esw/science_hw_bridge.cpp +++ b/esw/science_hw_bridge.cpp @@ -57,14 +57,14 @@ namespace mrover { [this](std_srvs::srv::Trigger::Request::SharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void { mFunnelServo->startHoming(true); res->success = true; - res->message = "Homing initiated for funnel."; + res->message = "homing initiated for funnel"; }, rmw_qos_profile_services_default, mServiceGroup); mFunnelLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { if (mAuger) { - bool funnelIndexHit = (mAuger->getLimitsHitBits() & 0x01) != 0; + bool funnelIndexHit = (mAuger->getLimitsHitBits() & 0x01) != 0; // TODO verify that this is the correct bit mFunnelServo->updateIndexLimit(funnelIndexHit); } }); diff --git a/esw/servo/servo.hpp b/esw/servo/servo.hpp index 23a95976..d3ba793d 100644 --- a/esw/servo/servo.hpp +++ b/esw/servo/servo.hpp @@ -430,9 +430,12 @@ namespace mrover { mUseHardwareLimits = useHwLimits; mUseIndexHoming = useIndexHoming; mIndexLimitRad = indexLimit; + mForwardLimitRad = forwardLimit; mReverseLimitRad = reverseLimit; + if (mUseHardwareLimits || mUseIndexHoming) mIsHomed = false; + mAdjustedReverseLimit = static_cast((reverseLimit / TAU) * SERVO_TICKS); mAdjustedForwardLimit = static_cast((forwardLimit / TAU) * SERVO_TICKS); } From 316d292f29ecc87fd856380c5d0de6863f5ea018 Mon Sep 17 00:00:00 2001 From: Eric D'Urso Date: Wed, 13 May 2026 22:43:49 -0400 Subject: [PATCH 4/5] revert launch files --- launch/jetson_can.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/jetson_can.launch.py b/launch/jetson_can.launch.py index 4b90c2be..0411431e 100644 --- a/launch/jetson_can.launch.py +++ b/launch/jetson_can.launch.py @@ -44,7 +44,7 @@ def generate_launch_description(): return LaunchDescription( [ can_bridge_0_node, - # can_bridge_1_node, - # can_bridge_2_node, + can_bridge_1_node, + can_bridge_2_node, ] ) From bbb173cc65c53096792d1599ddce4887932767be Mon Sep 17 00:00:00 2001 From: Eric D'Urso Date: Tue, 26 May 2026 23:52:11 -0600 Subject: [PATCH 5/5] fix homing sp --- config/science.yaml | 2 +- esw/mast_gimbal_hw_bridge.cpp | 2 +- esw/servo/servo.hpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/config/science.yaml b/config/science.yaml index cc4490d2..ede6d4dc 100644 --- a/config/science.yaml +++ b/config/science.yaml @@ -4,7 +4,7 @@ science_hw_bridge: id: 5 mode: "optimal" use_index_homing: true - index_limit: 0.0 + index_limit: 3.14 operating_mode: "extended_position" position_multiplier: 5.859375 position_p: 6000.0 diff --git a/esw/mast_gimbal_hw_bridge.cpp b/esw/mast_gimbal_hw_bridge.cpp index 49433eeb..132f12a5 100644 --- a/esw/mast_gimbal_hw_bridge.cpp +++ b/esw/mast_gimbal_hw_bridge.cpp @@ -77,7 +77,7 @@ namespace mrover { mHWLimitTimer = this->create_wall_timer(std::chrono::milliseconds(20), [this]() -> void { if (mServos.count("gimbal_yaw") > 0 && mYawLimBoard) { mServos.at("gimbal_yaw")->updateHardwareLimits( - mYawLimBoard->getLimitB(), + mYawLimBoard->getLimitB(), mYawLimBoard->getLimitA() ); } }, mTimerGroup); diff --git a/esw/servo/servo.hpp b/esw/servo/servo.hpp index d3ba793d..6ecc2cdd 100644 --- a/esw/servo/servo.hpp +++ b/esw/servo/servo.hpp @@ -321,6 +321,7 @@ namespace mrover { if (!mUseIndexHoming) return; bool const prevHit = mIndexHit.exchange(hit); if (hit && !prevHit) { + RCLCPP_INFO(mNode->get_logger(), "hit & homing!!"); bool wasHomed = mIsHomed.load(); snapToLimit(mIndexLimitRad, !wasHomed, false); }