integrate servo limit switches

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

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:

config/mast_gimbal.yaml

@@ -15,20 +15,19 @@ 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
       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
-      boot_position: 3.14
+      current_limit: 300.0
+      profile_acceleration: 20.0
+      profile_velocity: 150.0

config/science.yaml

@@ -3,6 +3,8 @@ science_hw_bridge:
     funnel:
       id: 5
       mode: "optimal"
+      use_index_homing: true
+      index_limit: 3.14
       operating_mode: "extended_position"
       position_multiplier: 5.859375
       position_p: 6000.0

esw/lim/lim.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "MRoverCAN.hpp"
+#include "can_device.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <units.hpp>
+
+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<decltype(decoded)>;
+
+                if constexpr (std::is_same_v<T, LIMState>) {
+                    // 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

esw/mast_gimbal_hw_bridge.cpp

@@ -10,7 +10,9 @@
 
 #include <mrover/msg/controller_state.hpp>
 #include <mrover/srv/servo_position.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
+#include <lim.hpp>
 #include <servo.hpp>
 #include <u2d2.hpp>
 
@@ -36,6 +38,8 @@ namespace mrover {
                 mControllerState.names.push_back(servoName);
             }
 
+            mYawLimBoard = std::make_shared<LimitSwitchBoard>(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,51 @@ namespace mrover {
                     rmw_qos_profile_services_default,
                     mServiceGroup);
 
+            mHomingService = this->create_service<std_srvs::srv::Trigger>(
+                    "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),
                     [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->getLimitB(),
+                        mYawLimBoard->getLimitA()
+                    );
+                } }, mTimerGroup);
+
             mGimbalStatePub = this->create_publisher<msg::ControllerState>("gimbal_controller_state", 10);
         }
 
     private:
         std::vector<std::string> mServoNames = {"gimbal_pitch", "gimbal_yaw"};
         std::unordered_map<std::string, std::shared_ptr<Servo>> mServos;
-
+        std::shared_ptr<LimitSwitchBoard> mYawLimBoard;
         std::string mU2D2DeviceName;
 
         rclcpp::CallbackGroup::SharedPtr mServiceGroup;
         rclcpp::CallbackGroup::SharedPtr mTimerGroup;
 
         rclcpp::Service<srv::ServoPosition>::SharedPtr mPositionService;
+        rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr mHomingService;
         rclcpp::Publisher<msg::ControllerState>::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 {

esw/science_hw_bridge.cpp

@@ -8,6 +8,7 @@
 #include <mrover/msg/controller_state.hpp>
 #include <mrover/msg/throttle.hpp>
 #include <mrover/srv/servo_position.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include <brushed.hpp>
 #include <science.hpp>
@@ -51,6 +52,23 @@ namespace mrover {
                     rmw_qos_profile_services_default,
                     mServiceGroup);
 
+            mFunnelHoming = this->create_service<std_srvs::srv::Trigger>(
+                    "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; // TODO verify that this is the correct bit
+                    mFunnelServo->updateIndexLimit(funnelIndexHit);
+                }
+            });
+
             mSPThrottleSub = create_subscription<msg::Throttle>("sp_thr_cmd", 1, [this](msg::Throttle::ConstSharedPtr const& msg) -> void { processThrottleCmd(msg); });
 
             mPublishDataTimer = create_wall_timer(
@@ -87,6 +105,8 @@ namespace mrover {
 
         rclcpp::CallbackGroup::SharedPtr mServiceGroup;
         rclcpp::Service<srv::ServoPosition>::SharedPtr mFunnelPositionService;
+        rclcpp::TimerBase::SharedPtr mFunnelLimitTimer;
+        rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr mFunnelHoming;
         rclcpp::TimerBase::SharedPtr mPublishDataTimer;
         rclcpp::Subscription<msg::Throttle>::SharedPtr mSPThrottleSub;
         rclcpp::Publisher<msg::ControllerState>::SharedPtr mControllerStatePub;