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 af691e7..2965b2c 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -43,9 +43,10 @@ joint_group_position_controller: - stepper_motor_b - talon_lift - talon_scoop + - rack_and_pinion_left + - rack_and_pinion_right - scoop_a - scoop_b - - auger - cap interface_name: position @@ -58,6 +59,8 @@ joint_group_velocity_controller: - talon_lift - talon_scoop - auger + - rack_and_pinion_left + - rack_and_pinion_right - scoop_a - scoop_b - auger @@ -75,8 +78,12 @@ science_controller: - scoop_b - auger - cap + - rack_and_pinion_left + - rack_and_pinion_right stepper_motor_a: "stepper_motor_a" stepper_motor_b: "stepper_motor_b" + 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"] 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 900575b..f526054 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 @@ -102,6 +102,7 @@ class ScienceManual : public controller_interface::ControllerInterface std::vector stepper_joints_; std::vector talon_joints_; std::vector servo_joints_; + std::vector rack_pinion_joints_; //std::string auger_spinner_; // Command subscribers and Controller State publisher @@ -130,7 +131,9 @@ class ScienceManual : public controller_interface::ControllerInterface 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 { @@ -178,6 +183,10 @@ class ScienceManual : public controller_interface::ControllerInterface // ----- 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 6bfabb1..bbbd398 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -71,6 +71,10 @@ controller_interface::CallbackReturn ScienceManual::on_configure( 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()) { @@ -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; @@ -178,6 +186,10 @@ controller_interface::InterfaceConfiguration ScienceManual::state_interface_conf 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; @@ -206,6 +218,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( stepper_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,27 +240,52 @@ 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]) ? + 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); + // --- 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); @@ -266,23 +303,30 @@ controller_interface::return_type ScienceManual::update( 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 ba89899..8e2a3eb 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -110,6 +110,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"