From b5469beb12872016bee55486abf414e415cc3f2b Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 5 Dec 2025 22:48:46 -0500 Subject: [PATCH] Added Rack & Pinions to Science Subsystem --- .../science/science.servo.ros2_control.xacro | 14 +++ .../config/athena_science_controllers.yaml | 25 ++-- .../science_controller.hpp | 39 ++++-- .../src/science_controller.cpp | 113 ++++++++++++------ .../src/science_controller.yaml | 16 +++ 5 files changed, 150 insertions(+), 57 deletions(-) diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index 6b06352..abc0f49 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -9,6 +9,20 @@ servo_ros2_control/SERVOHardwareInterface + + + + + + + + + + + + + + diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index edca0bd..2965b2c 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -41,11 +41,12 @@ joint_group_position_controller: joints: - stepper_motor_a - stepper_motor_b - #- talon_lift - #- talon_scoop + - talon_lift + - talon_scoop + - rack_and_pinion_left + - rack_and_pinion_right - scoop_a - scoop_b - - auger - cap interface_name: position @@ -55,9 +56,11 @@ joint_group_velocity_controller: joints: - stepper_motor_a - stepper_motor_b - #- talon_lift - #- talon_scoop + - talon_lift + - talon_scoop - auger + - rack_and_pinion_left + - rack_and_pinion_right - scoop_a - scoop_b - auger @@ -69,16 +72,20 @@ science_controller: joints: - stepper_motor_a - stepper_motor_b - #- talon_lift - #- talon_scoop + - talon_lift + - talon_scoop - scoop_a - scoop_b - auger - cap + - rack_and_pinion_left + - rack_and_pinion_right stepper_motor_a: "stepper_motor_a" stepper_motor_b: "stepper_motor_b" - #talon_lift: ["talon_lift"] - #talon_scoop: "talon_scoop" + rack_and_pinion_left: "rack_and_pinion_left" + rack_and_pinion_right: "rack_and_pinion_right" + talon_lift: ["talon_lift"] + talon_scoop: "talon_scoop" scoop_servos: ["scoop_a", "scoop_b"] #auger_spinner: "auger_spinner" auger: "auger" diff --git a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp index e6941fe..8682b7d 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp @@ -100,8 +100,9 @@ class ScienceManual : public controller_interface::ControllerInterface std::vector state_joints_; std::vector stepper_joints_; - //std::vector talon_joints_; + std::vector talon_joints_; std::vector servo_joints_; + std::vector rack_pinion_joints_; //std::string auger_spinner_; // Command subscribers and Controller State publisher @@ -127,10 +128,12 @@ class ScienceManual : public controller_interface::ControllerInterface void load_velocity_limits(); // called in on_configure() void send_commands( - //double lift_cmd, + double lift_cmd, double stepper_cmd, double scoop_cmd, - double auger_cmd + double auger_cmd, + double rack_left_cmd, + double rack_right_cmd //double auger_spinner_cmd ); }; @@ -139,11 +142,13 @@ class ScienceManual : public controller_interface::ControllerInterface static constexpr double max_stepper_velocity = 1.0; static constexpr double scoop_talon_velocity = 1.0; static constexpr double auger_velocity = 1.0; - + // Closed = 0, Open = 1 double scoop_position = 0; double auger_position = 0; double cap_position = 0; + double rack_left_position = 0.0; + double rack_right_position = 0.0; /*enum CommandInterfaces { @@ -162,11 +167,27 @@ class ScienceManual : public controller_interface::ControllerInterface { IDX_STEPPER_A_POSITION = 0, IDX_STEPPER_B_POSITION = 1, - IDX_SCOOP_A_POSITION = 2, - IDX_SCOOP_B_POSITION = 3, - IDX_AUGER_POSITION = 4, - IDX_CAP_POSITION = 5, - CMD_ITFS_COUNT // total number of command interfaces + + // ----- Talons (velocity control) ----- + IDX_LIFT_TALON_VELOCITY = 2, + IDX_SCOOP_TALON_VELOCITY = 3, + + // ----- Scoop servos (position) ----- + IDX_SCOOP_A_POSITION = 4, + IDX_SCOOP_B_POSITION = 5, + + // ----- Auger servo ----- + IDX_AUGER_POSITION = 6, + + // ----- Cap servo ----- + IDX_CAP_POSITION = 7, + + // ----- Rack and Pinion servos ----- + IDX_RACK_LEFT_POSITION = 8, + IDX_RACK_RIGHT_POSITION = 9, + + // Total number of interfaces + CMD_ITFS_COUNT }; diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index f049e5c..bbbd398 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -61,16 +61,20 @@ controller_interface::CallbackReturn ScienceManual::on_configure( params_.stepper_motor_a, params_.stepper_motor_b }; -/* + talon_joints_ = { params_.talon_lift[0], // or flatten the array params_.talon_scoop - }; */ + }; servo_joints_ = params_.scoop_servos; servo_joints_.push_back(params_.auger); servo_joints_.push_back(params_.cap); + rack_pinion_joints_.clear(); + rack_pinion_joints_.push_back(params_.rack_and_pinion_left); + rack_pinion_joints_.push_back(params_.rack_and_pinion_right); + // auger_spinner_ = params_.auger_spinner; if (!params_.state_joints.empty()) { @@ -140,10 +144,10 @@ controller_interface::InterfaceConfiguration ScienceManual::command_interface_co } // Talons: control velocity - /*for (const auto & joint : talon_joints_) + for (const auto & joint : talon_joints_) { cfg.names.push_back(joint + "/velocity"); - } */ + } // Servos (scoop + cap): control position for (const auto & joint : servo_joints_) @@ -151,6 +155,10 @@ controller_interface::InterfaceConfiguration ScienceManual::command_interface_co cfg.names.push_back(joint + "/position"); } + for (const auto & joint : rack_pinion_joints_) { + cfg.names.push_back(joint + "/position"); + } + // Auger spinner (if separate): velocity control // cfg.names.push_back(auger_spinner_ + "/velocity"); return cfg; @@ -167,17 +175,21 @@ controller_interface::InterfaceConfiguration ScienceManual::state_interface_conf cfg.names.push_back(joint + "/velocity"); } - /*for (const auto & joint : talon_joints_) + for (const auto & joint : talon_joints_) { cfg.names.push_back(joint + "/velocity"); cfg.names.push_back(joint + "/position"); - } */ + } for (const auto & joint : servo_joints_) { cfg.names.push_back(joint + "/position"); } + for (const auto & joint : rack_pinion_joints_) { + cfg.names.push_back(joint + "/position"); + } + //cfg.names.push_back(auger_spinner_ + "/velocity"); //cfg.names.push_back(auger_spinner_ + "/position"); return cfg; @@ -204,8 +216,9 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } stepper_joints_.clear(); - //talon_joints_.clear(); + talon_joints_.clear(); servo_joints_.clear(); + rack_pinion_joints_.clear(); state_joints_.clear(); RCLCPP_INFO(get_node()->get_logger(), "Manual controller deactivated and released interfaces"); @@ -213,10 +226,9 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( } controller_interface::return_type ScienceManual::update( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, + const rclcpp::Duration & period) { - auto current_ref = input_ref_.readFromRT(); if (!(*current_ref)) { @@ -228,70 +240,93 @@ controller_interface::return_type ScienceManual::update( int stage_idx = static_cast(current_mode_); // corresponds to STAGE1..STAGE4 // Lift Talon - //double lift_cmd = (msg->buttons.size() > 0 && msg->buttons[0]) ? params_.velocity_limits_talon_lift[stage_idx] : 0.0; + double lift_cmd = + (msg->buttons.size() > 0 && msg->buttons[0]) ? + params_.velocity_limits_talon_lift[stage_idx] : 0.0; - // Stepper motors command - double stepper_cmd = (msg->buttons.size() > 2 && msg->buttons[2]) ? + // Stepper motors command (velocity-like value, but we feed as position target here) + double stepper_cmd = + (msg->buttons.size() > 2 && msg->buttons[2]) ? params_.velocity_limits_stepper[stage_idx] : 0.0; // Scoop Talon - /* double scoop_cmd = (msg->buttons.size() > 4 && msg->buttons[4]) ? - params_.velocity_limits_talon_scoop[stage_idx] : 0.0; */ + double scoop_cmd = + (msg->buttons.size() > 4 && msg->buttons[4]) ? + params_.velocity_limits_talon_scoop[stage_idx] : 0.0; // Auger - double auger_cmd = (msg->buttons.size() > 5 && msg->buttons[5]) ? + double auger_cmd = + (msg->buttons.size() > 5 && msg->buttons[5]) ? params_.velocity_limits_auger[stage_idx] : 0.0; // Auger Spinner - /*double auger_spinner_cmd = (msg->buttons.size() > 6 && msg->buttons[6]) ? + /*double auger_spinner_cmd = + (msg->buttons.size() > 6 && msg->buttons[6]) ? params_.velocity_limits_auger_spinner[stage_idx] : 0.0; */ - scoop_position = std::clamp(scoop_position, 0.0, 1.0); - auger_position = std::clamp(auger_position, 0.0, 1.0); - cap_position = std::clamp(cap_position, 0.0, 1.0); -/* - // Send velocity commands - //command_interfaces_[IDX_LIFT_TALON_VELOCITY].set_value(lift_cmd); - command_interfaces_[IDX_STEPPERS_VELOCITY_START].set_value(stepper_cmd); - //command_interfaces_[IDX_SCOOP_TALON_VELOCITY].set_value(scoop_cmd); - command_interfaces_[IDX_AUGER_VELOCITY].set_value(auger_cmd); - - // --- Send position commands - command_interfaces_[IDX_SCOOP_SERVO_POSITION].set_value(scoop_position); - command_interfaces_[IDX_AUGER_SERVO_POSITION].set_value(auger_position); - command_interfaces_[IDX_CAP_SERVO_POSITION].set_value(cap_position); */ - - // Apply same stepper_cmd to both steppers + // --- Rack & pinion control via joystick axes --- + // Use stage-dependent "speed" for rack & pinion motion (reuse stepper limits) + double rack_speed = params_.velocity_limits_stepper[stage_idx]; + + double dt = period.seconds(); + + // Left rack: map from axis[1] (left stick vertical) + double axis_left = (msg->axes.size() > 1) ? msg->axes[1] : 0.0; + + // Right rack: map from axis[4] (right stick vertical) + double axis_right = (msg->axes.size() > 4) ? msg->axes[4] : 0.0; + + // Integrate axes into positions (position += axis * speed * dt) + rack_left_position += axis_left * rack_speed * dt; + rack_right_position += axis_right * rack_speed * dt; + + // Clamp all servo-like positions to [0, 1] + scoop_position = std::clamp(scoop_position, 0.0, 1.0); + auger_position = std::clamp(auger_position, 0.0, 1.0); + cap_position = std::clamp(cap_position, 0.0, 1.0); + rack_left_position = std::clamp(rack_left_position, 0.0, 1.0); + rack_right_position = std::clamp(rack_right_position, 0.0, 1.0); + + // Stepper motors (position) command_interfaces_[IDX_STEPPER_A_POSITION].set_value(stepper_cmd); command_interfaces_[IDX_STEPPER_B_POSITION].set_value(stepper_cmd); - // Use auger_cmd however you like; right now just map to auger position or ignore - command_interfaces_[IDX_AUGER_POSITION].set_value(auger_position); + // Talons (velocity) + command_interfaces_[IDX_LIFT_TALON_VELOCITY].set_value(lift_cmd); + command_interfaces_[IDX_SCOOP_TALON_VELOCITY].set_value(scoop_cmd); // Scoop servos command_interfaces_[IDX_SCOOP_A_POSITION].set_value(scoop_position); command_interfaces_[IDX_SCOOP_B_POSITION].set_value(scoop_position); - // Cap servo + // Auger & cap + command_interfaces_[IDX_AUGER_POSITION].set_value(auger_position); command_interfaces_[IDX_CAP_POSITION].set_value(cap_position); + // NEW: Rack & pinion servos + command_interfaces_[IDX_RACK_LEFT_POSITION].set_value(rack_left_position); + command_interfaces_[IDX_RACK_RIGHT_POSITION].set_value(rack_right_position); + + // Reset joystick input after processing reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints); - for (const auto &joint_name : params_.joints) { + // Basic state publish (still reusing existing signals) + for (const auto & joint_name : params_.joints) { if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); state_publisher_->msg_.header.frame_id = joint_name; state_publisher_->msg_.set_point = stepper_cmd; state_publisher_->msg_.process_value = auger_cmd; - //state_publisher_->msg_.command = lift_cmd; + state_publisher_->msg_.command = lift_cmd; state_publisher_->unlockAndPublish(); } } - + return controller_interface::return_type::OK; } + } // namespace science_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index 219aba5..b8c4182 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -109,6 +109,22 @@ science_manual: description: "(Optional)" read_only: false + rack_and_pinion_left: + type: string + default_value: "rack_and_pinion_left" + description: "The Servo motor controller for the left-sided rack and pinion" + read_only: true + validation: + not_empty<>: null + + rack_and_pinion_right: + type: string + default_value: "rack_and_pinion_right" + description: "The Servo motor controller for the right-sided rack and pinion" + read_only: true + validation: + not_empty<>: null + auger: type: string default_value: "auger"