Added Rack & Pinions to Science Subsystem

src/description/ros2_control/science/science.servo.ros2_control.xacro

@@ -9,6 +9,20 @@
                 <plugin>servo_ros2_control/SERVOHardwareInterface</plugin>
             </hardware>
 
+            <joint name="rack_and_pinion_left">
+                <command_interface name="position"/>
+                <command_interface name="velocity"/>
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+
+            <joint name="rack_and_pinion_right">
+                <command_interface name="position"/>
+                <command_interface name="velocity"/>
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+
             <joint name="scoop_a">
                 <command_interface name="position"/>
                 <command_interface name="velocity"/>

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"]

src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp

@@ -102,6 +102,7 @@ class ScienceManual : public controller_interface::ControllerInterface
   std::vector<std::string> stepper_joints_;
   std::vector<std::string> talon_joints_;
   std::vector<std::string> servo_joints_;
+  std::vector<std::string> 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
   };

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,17 +218,17 @@ 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");
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 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<int>(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"

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"