Comments

src/esw/arm_hw_bridge/main.cpp

@@ -13,7 +13,6 @@ auto armLaserCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Respon
 }
 
 auto main(int argc, char** argv) -> int {
-    // Initialize the ROS node
     ros::init(argc, argv, "arm_hw_bridge");
     ros::NodeHandle nh;
 
@@ -23,7 +22,6 @@ auto main(int argc, char** argv) -> int {
 
     ros::ServiceServer armLaserService = nh.advertiseService("enable_arm_laser", armLaserCallback);
 
-    // Enter the ROS event loop
     ros::spin();
 
     return EXIT_SUCCESS;

src/esw/arm_translator_bridge/arm_translator.cpp

@@ -15,7 +15,7 @@ namespace mrover {
     Dimensionless static constexpr PITCH_ROLL_TO_01_SCALE{40};
 
     // How often we send an adjust command to the DE motors
-    // This updates the quadrature motor source on the Moteus based on the absolute encoder readings
+    // This corrects the HALL-effect motor source on the Moteus based on the absolute encoder readings
     double static constexpr DE_OFFSET_TIMER_PERIOD = 1;
 
     ArmTranslator::ArmTranslator(ros::NodeHandle& nh) {
@@ -136,6 +136,8 @@ namespace mrover {
 
         std::optional<std::size_t> jointDe0Index = findJointByName(msg->name, "joint_de_0"), jointDe1Index = findJointByName(msg->name, "joint_de_1");
         if (jointDe0Index && jointDe1Index) {
+            // The Moteus reports auxiliary motor positions in the range [0, tau) instead of [-pi, pi)
+            // Wrap to better align with IK conventions
             auto pitchWrapped = wrapAngle(static_cast<float>(msg->position.at(jointDe0Index.value())));
             auto rollWrapped = wrapAngle(static_cast<float>(msg->position.at(jointDe1Index.value())));
             mJointDePitchRoll = {pitchWrapped, rollWrapped};

src/esw/drive_bridge/main.cpp

@@ -5,6 +5,10 @@
 #include <motors_group.hpp>
 #include <params_utils.hpp>
 
+/*
+ *  Converts from a Twist (linear and angular velocity) to the individual wheel velocities
+ */
+
 using namespace mrover;
 
 void moveDrive(geometry_msgs::Twist::ConstPtr const& msg);
@@ -13,37 +17,26 @@ ros::Publisher leftVelocityPub, rightVelocityPub;
 
 Meters WHEEL_DISTANCE_INNER;
 compound_unit<Radians, inverse<Meters>> WHEEL_LINEAR_TO_ANGULAR;
-RadiansPerSecond MAX_MOTOR_SPEED;
 
 auto main(int argc, char** argv) -> int {
-    // Initialize the ROS node
     ros::init(argc, argv, "drive_bridge");
     ros::NodeHandle nh;
 
-    // Load rover dimensions and other parameters from the ROS parameter server
     auto roverWidth = requireParamAsUnit<Meters>(nh, "rover/width");
     WHEEL_DISTANCE_INNER = roverWidth / 2;
 
     auto ratioMotorToWheel = requireParamAsUnit<Dimensionless>(nh, "wheel/gear_ratio");
     auto wheelRadius = requireParamAsUnit<Meters>(nh, "wheel/radius");
-    // TODO(quintin) is dividing by ratioMotorToWheel right?
     WHEEL_LINEAR_TO_ANGULAR = Radians{1} / wheelRadius * ratioMotorToWheel;
 
-    auto maxLinearSpeed = requireParamAsUnit<MetersPerSecond>(nh, "rover/max_speed");
-    assert(maxLinearSpeed > 0_mps);
-
-    MAX_MOTOR_SPEED = maxLinearSpeed * WHEEL_LINEAR_TO_ANGULAR;
-
     auto leftGroup = std::make_unique<MotorsGroup>(nh, "drive_left");
     auto rightGroup = std::make_unique<MotorsGroup>(nh, "drive_right");
 
     leftVelocityPub = nh.advertise<Velocity>("drive_left_velocity_cmd", 1);
     rightVelocityPub = nh.advertise<Velocity>("drive_right_velocity_cmd", 1);
 
-    // Subscribe to the ROS topic for drive commands
     auto twistSubscriber = nh.subscribe<geometry_msgs::Twist>("cmd_vel", 1, moveDrive);
 
-    // Enter the ROS event loop
     ros::spin();
 
     return 0;
@@ -62,14 +55,14 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) {
     {
         Velocity leftVelocity;
         leftVelocity.names = {"front_left", "middle_left", "back_left"};
-        // TODO(quintin): Invert in firmware
-        leftVelocity.velocities = {-left.get(), -left.get(), -left.get()};
+        left *= -1; // TODO(quintin): Invert in firmware instead
+        leftVelocity.velocities.resize(leftVelocity.names.size(), left.get());
         leftVelocityPub.publish(leftVelocity);
     }
     {
         Velocity rightVelocity;
         rightVelocity.names = {"front_right", "middle_right", "back_right"};
-        rightVelocity.velocities = {right.get(), right.get(), right.get()};
+        rightVelocity.velocities.resize(rightVelocity.names.size(), right.get());
         rightVelocityPub.publish(rightVelocity);
     }
 }

src/esw/motor_library/brushless.hpp

@@ -52,8 +52,8 @@ namespace mrover {
     };
 
     struct MoteusLimitSwitchInfo {
-        bool isFwdPressed{};
-        bool isBwdPressed{};
+        bool isForwardPressed{};
+        bool isBackwardPressed{};
     };
 
     template<IsUnit TOutputPosition>
@@ -118,10 +118,14 @@ namespace mrover {
             }
 
             moteus::Controller::Options options;
-            moteus::Query::Format queryFormat;
+            moteus::Query::Format queryFormat{};
             queryFormat.aux1_gpio = moteus::kInt8;
             queryFormat.aux2_gpio = moteus::kInt8;
-            if (this->isJointDe()) { // add joint de abs slots to CAN message
+            if (this->isJointDe()) {
+                // DE0 and DE1 have absolute encoders
+                // They are not used for their internal control loops
+                // Instead we request them at the ROS level and send adjust commands periodically
+                // Therefore we do not get them as part of normal messages and must request them explicitly
                 queryFormat.extra[0] = moteus::Query::ItemFormat{
                         .register_number = moteus::Register::kEncoder1Position,
                         .resolution = moteus::kFloat,
@@ -140,12 +144,13 @@ namespace mrover {
         }
 
         auto setDesiredPosition(OutputPosition position) -> void {
-            // only check for limit switches if at least one limit switch exists and is enabled
+            // Only check for limit switches if at least one limit switch exists and is enabled
             if ((limitSwitch0Enabled && limitSwitch0Present) || (limitSwitch1Enabled && limitSwitch0Present)) {
                 sendQuery();
 
-                MoteusLimitSwitchInfo limitSwitchInfo = getPressedLimitSwitchInfo();
-                if ((mCurrentPosition < position && limitSwitchInfo.isFwdPressed) || (mCurrentPosition > position && limitSwitchInfo.isBwdPressed)) {
+                if (auto [isFwdPressed, isBwdPressed] = getPressedLimitSwitchInfo();
+                    (mCurrentPosition < position && isFwdPressed) ||
+                    (mCurrentPosition > position && isBwdPressed)) {
                     setBrake();
                     return;
                 }
@@ -159,9 +164,6 @@ namespace mrover {
                     .maximum_torque = mMaxTorque,
                     .watchdog_timeout = mWatchdogTimeout,
             };
-            if (this->isJointDe()) {
-                ROS_INFO_STREAM(std::format("Setting position to {}", position.get()).c_str());
-            }
             moteus::CanFdFrame positionFrame = mMoteus->MakePosition(command);
             mDevice.publish_moteus_frame(positionFrame);
         }
@@ -170,27 +172,27 @@ namespace mrover {
             assert(msg->source == mControllerName);
             assert(msg->destination == mMasterName);
             auto result = moteus::Query::Parse(msg->data.data(), msg->data.size());
-            if (this->isJointDe()) {
-                ROS_INFO("controller: %s    %3d p/a/v/t=(%7.3f,%7.3f,%7.3f,%7.3f)  v/t/f=(%5.1f,%5.1f,%3d) GPIO: Aux1-%X , Aux2-%X",
-                         mControllerName.c_str(),
-                         result.mode,
-                         result.position,
-                         result.abs_position,
-                         result.velocity,
-                         result.torque,
-                         result.voltage,
-                         result.temperature,
-                         result.fault,
-                         result.aux1_gpio,
-                         result.aux2_gpio);
-            }
+            // if (this->isJointDe()) {
+            //     ROS_INFO("controller: %s    %3d p/a/v/t=(%7.3f,%7.3f,%7.3f,%7.3f)  v/t/f=(%5.1f,%5.1f,%3d) GPIO: Aux1-%X , Aux2-%X",
+            //              mControllerName.c_str(),
+            //              result.mode,
+            //              result.position,
+            //              result.abs_position,
+            //              result.velocity,
+            //              result.torque,
+            //              result.voltage,
+            //              result.temperature,
+            //              result.fault,
+            //              result.aux1_gpio,
+            //              result.aux2_gpio);
+            // }
 
             if (this->isJointDe()) {
-                mCurrentPosition = OutputPosition{result.extra[0].value}; // get value of absolute encoder if its joint_de0/1
+                mCurrentPosition = OutputPosition{result.extra[0].value}; // Get value of absolute encoder if its joint_de0/1
                 mCurrentVelocity = OutputVelocity{result.extra[1].value};
             } else {
-                mCurrentPosition = OutputPosition{result.position}; // moteus stores position in revolutions.
-                mCurrentVelocity = OutputVelocity{result.velocity}; // moteus stores position in revolutions.
+                mCurrentPosition = OutputPosition{result.position};
+                mCurrentVelocity = OutputVelocity{result.velocity};
             }
             mCurrentEffort = result.torque;
 
@@ -207,12 +209,13 @@ namespace mrover {
         }
 
         auto setDesiredVelocity(OutputVelocity velocity) -> void {
-            // only check for limit switches if at least one limit switch exists and is enabled
+            // Only check for limit switches if at least one limit switch exists and is enabled
             if ((limitSwitch0Enabled && limitSwitch0Present) || (limitSwitch1Enabled && limitSwitch0Present)) {
                 sendQuery();
 
-                MoteusLimitSwitchInfo limitSwitchInfo = getPressedLimitSwitchInfo();
-                if ((velocity > OutputVelocity{0} && limitSwitchInfo.isFwdPressed) || (velocity < OutputVelocity{0} && limitSwitchInfo.isBwdPressed)) {
+                if (auto [isFwdPressed, isBwdPressed] = getPressedLimitSwitchInfo();
+                    (velocity > OutputVelocity{0} && isFwdPressed) ||
+                    (velocity < OutputVelocity{0} && isBwdPressed)) {
                     setBrake();
                     return;
                 }
@@ -235,9 +238,7 @@ namespace mrover {
             }
         }
 
-        auto getEffort() -> double {
-            // TODO - need to properly set mMeasuredEFfort elsewhere.
-            // (Art Boyarov): return quiet_Nan, same as Brushed Controller
+        [[nodiscard]] auto getEffort() const -> double {
             return mCurrentEffort;
         }
 
@@ -252,46 +253,23 @@ namespace mrover {
         }
 
         auto getPressedLimitSwitchInfo() -> MoteusLimitSwitchInfo {
-            /*
-        Testing 2/9:
-        - Connected limit switch (common is input(black), NC to ground)
-        - Configured moteus to have all pins set as digital_input and pull_up
-        - When limit switch not pressed, aux2 = 0xD
-        - When limit switch pressed, aux1 = 0xF
-        - Note: has to be active high, so in this scenario we have to flip this bit round.
-        - This was connected to just one moteus board, not the one with a motor on it.
-
-        Stuff for Limit Switches (from Guthrie)
-        - Read from config about limit switch settings
-        - Either 1 or 0 not forward/backward
-
-        - Add a member variable in Brushless.hpp to store limit switch value.
-          pdate this limit switch variable every round in ProcessCANMessage.
-
-        - Note: we can get aux pin info even without sending a query command.
-          Tested with sending velocity commands.
-        */
-            // TODO - implement this
-            MoteusLimitSwitchInfo result{};
-            result.isFwdPressed = false;
-            result.isBwdPressed = false;
-
-            // Limit switches now wired to AUX2 (index 0 and 1)
             if (limitSwitch0Present && limitSwitch0Enabled) {
                 bool gpioState = 0b01 & mMoteusAux2Info;
-                mLimitHit.at(0) = gpioState == limitSwitch0ActiveHigh;
+                mLimitHit[0] = gpioState == limitSwitch0ActiveHigh;
             }
             if (limitSwitch1Present && limitSwitch1Enabled) {
                 bool gpioState = 0b10 & mMoteusAux2Info;
-                mLimitHit.at(1) = gpioState == limitSwitch1ActiveHigh;
+                mLimitHit[1] = gpioState == limitSwitch1ActiveHigh;
             }
 
-            result.isFwdPressed = (mLimitHit.at(0) && limitSwitch0LimitsFwd) || (mLimitHit.at(1) && limitSwitch1LimitsFwd);
-            result.isBwdPressed = (mLimitHit.at(0) && !limitSwitch0LimitsFwd) || (mLimitHit.at(1) && !limitSwitch1LimitsFwd);
+            MoteusLimitSwitchInfo result{
+                    .isForwardPressed = (mLimitHit[0] && limitSwitch0LimitsFwd) || (mLimitHit[1] && limitSwitch1LimitsFwd),
+                    .isBackwardPressed = (mLimitHit[0] && !limitSwitch0LimitsFwd) || (mLimitHit[1] && !limitSwitch1LimitsFwd),
+            };
 
-            if (result.isFwdPressed) {
+            if (result.isForwardPressed) {
                 adjust(limitSwitch0ReadjustPosition);
-            } else if (result.isBwdPressed) {
+            } else if (result.isBackwardPressed) {
                 adjust(limitSwitch1ReadjustPosition);
             }
 

src/esw/motor_library/controller.hpp

@@ -37,7 +37,6 @@ namespace mrover {
               mMoveThrottleSub{mNh.subscribe<Throttle>(std::format("{}_throttle_cmd", mControllerName), 1, &ControllerBase::setDesiredThrottle, this)},
               mMoveVelocitySub{mNh.subscribe<Velocity>(std::format("{}_velocity_cmd", mControllerName), 1, &ControllerBase::setDesiredVelocity, this)},
               mMovePositionSub{mNh.subscribe<Position>(std::format("{}_position_cmd", mControllerName), 1, &ControllerBase::setDesiredPosition, this)},
-              // mAdjustEncoderSub{mNh.subscribe<MotorsAdjust>(std::format("{}_adjust_cmd", mControllerName), 1, &ControllerBase::adjustEncoder, this)},
               mJointDataPub{mNh.advertise<sensor_msgs::JointState>(std::format("{}_joint_data", mControllerName), 1)},
               mControllerDataPub{mNh.advertise<ControllerState>(std::format("{}_controller_data", mControllerName), 1)},
               mPublishDataTimer{mNh.createTimer(ros::Duration{0.1}, &ControllerBase::publishDataCallback, this)},
@@ -88,17 +87,6 @@ namespace mrover {
             static_cast<Derived*>(this)->setDesiredPosition(position);
         }
 
-        auto adjustEncoder(MotorsAdjust::ConstPtr const& msg) -> void {
-            if (msg->names.size() != 1 || msg->names.at(0) != mControllerName || msg->values.size() != 1) {
-                ROS_ERROR("Adjust request at topic for %s ignored!", msg->names.at(0).c_str());
-                return;
-            }
-
-            using Position = typename detail::strip_conversion<OutputPosition>::type;
-            OutputPosition position = Position{msg->values.front()};
-            static_cast<Derived*>(this)->adjust(position);
-        }
-
         [[nodiscard]] auto isJointDe() const -> bool {
             return mControllerName == "joint_de_0" || mControllerName == "joint_de_1";
         }