From 6d06b3f93a3aa2bd5a675dc8d7fd6409bce07f92 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 23:02:45 -0400 Subject: [PATCH] Comments --- src/esw/arm_hw_bridge/main.cpp | 2 - .../arm_translator_bridge/arm_translator.cpp | 4 +- src/esw/drive_bridge/main.cpp | 21 ++-- src/esw/motor_library/brushless.hpp | 106 +++++++----------- src/esw/motor_library/controller.hpp | 12 -- .../linear_joint_translation.hpp | 15 --- 6 files changed, 52 insertions(+), 108 deletions(-) delete mode 100644 src/esw/motor_library/linear_joint_translation.hpp diff --git a/src/esw/arm_hw_bridge/main.cpp b/src/esw/arm_hw_bridge/main.cpp index 7a3762427..28480cc13 100644 --- a/src/esw/arm_hw_bridge/main.cpp +++ b/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; diff --git a/src/esw/arm_translator_bridge/arm_translator.cpp b/src/esw/arm_translator_bridge/arm_translator.cpp index 8d1b91924..0be5f66fe 100644 --- a/src/esw/arm_translator_bridge/arm_translator.cpp +++ b/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 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(msg->position.at(jointDe0Index.value()))); auto rollWrapped = wrapAngle(static_cast(msg->position.at(jointDe1Index.value()))); mJointDePitchRoll = {pitchWrapped, rollWrapped}; diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index cc30f11b5..4bcbe86d5 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -5,6 +5,10 @@ #include #include +/* + * 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> 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(nh, "rover/width"); WHEEL_DISTANCE_INNER = roverWidth / 2; auto ratioMotorToWheel = requireParamAsUnit(nh, "wheel/gear_ratio"); auto wheelRadius = requireParamAsUnit(nh, "wheel/radius"); - // TODO(quintin) is dividing by ratioMotorToWheel right? WHEEL_LINEAR_TO_ANGULAR = Radians{1} / wheelRadius * ratioMotorToWheel; - auto maxLinearSpeed = requireParamAsUnit(nh, "rover/max_speed"); - assert(maxLinearSpeed > 0_mps); - - MAX_MOTOR_SPEED = maxLinearSpeed * WHEEL_LINEAR_TO_ANGULAR; - auto leftGroup = std::make_unique(nh, "drive_left"); auto rightGroup = std::make_unique(nh, "drive_right"); leftVelocityPub = nh.advertise("drive_left_velocity_cmd", 1); rightVelocityPub = nh.advertise("drive_right_velocity_cmd", 1); - // Subscribe to the ROS topic for drive commands auto twistSubscriber = nh.subscribe("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); } } diff --git a/src/esw/motor_library/brushless.hpp b/src/esw/motor_library/brushless.hpp index fc1437697..a7c1175ea 100644 --- a/src/esw/motor_library/brushless.hpp +++ b/src/esw/motor_library/brushless.hpp @@ -52,8 +52,8 @@ namespace mrover { }; struct MoteusLimitSwitchInfo { - bool isFwdPressed{}; - bool isBwdPressed{}; + bool isForwardPressed{}; + bool isBackwardPressed{}; }; template @@ -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); } diff --git a/src/esw/motor_library/controller.hpp b/src/esw/motor_library/controller.hpp index a608cd46f..3e14043b2 100644 --- a/src/esw/motor_library/controller.hpp +++ b/src/esw/motor_library/controller.hpp @@ -37,7 +37,6 @@ namespace mrover { mMoveThrottleSub{mNh.subscribe(std::format("{}_throttle_cmd", mControllerName), 1, &ControllerBase::setDesiredThrottle, this)}, mMoveVelocitySub{mNh.subscribe(std::format("{}_velocity_cmd", mControllerName), 1, &ControllerBase::setDesiredVelocity, this)}, mMovePositionSub{mNh.subscribe(std::format("{}_position_cmd", mControllerName), 1, &ControllerBase::setDesiredPosition, this)}, - // mAdjustEncoderSub{mNh.subscribe(std::format("{}_adjust_cmd", mControllerName), 1, &ControllerBase::adjustEncoder, this)}, mJointDataPub{mNh.advertise(std::format("{}_joint_data", mControllerName), 1)}, mControllerDataPub{mNh.advertise(std::format("{}_controller_data", mControllerName), 1)}, mPublishDataTimer{mNh.createTimer(ros::Duration{0.1}, &ControllerBase::publishDataCallback, this)}, @@ -88,17 +87,6 @@ namespace mrover { static_cast(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::type; - OutputPosition position = Position{msg->values.front()}; - static_cast(this)->adjust(position); - } - [[nodiscard]] auto isJointDe() const -> bool { return mControllerName == "joint_de_0" || mControllerName == "joint_de_1"; } diff --git a/src/esw/motor_library/linear_joint_translation.hpp b/src/esw/motor_library/linear_joint_translation.hpp deleted file mode 100644 index 1d7924ec3..000000000 --- a/src/esw/motor_library/linear_joint_translation.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -namespace mrover { - - auto inline convertLinVel(float velocity, float multiplier) { - return velocity * multiplier; - } - - auto inline convertLinPos(float position, float multiplier) { - return position * multiplier; - } - -} // namespace mrover