diff --git a/src/bringup/launch/science.launch.py b/src/bringup/launch/science.launch.py new file mode 100644 index 00000000..29c6ea79 --- /dev/null +++ b/src/bringup/launch/science.launch.py @@ -0,0 +1,199 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch_ros.actions import Node + + +# Drive owns the default /controller_manager. Science runs in /science to avoid the collision. +_SCIENCE_NS = "science" +_SCIENCE_CM = f"/{_SCIENCE_NS}/controller_manager" + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'mode', + default_value='standalone', + choices=['standalone', 'jetson', 'base_station'], + description=( + "'jetson': hardware nodes on the rover. " + "'base_station': operator teleop nodes. " + "'standalone': all nodes on one machine." + ), + ), + DeclareLaunchArgument('use_sim', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('use_mock_hardware', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('mock_sensor_commands', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument( + 'robot_controller', + default_value='rear_ackermann_controller', + choices=['front_ackermann_controller', 'ackermann_steering_controller', 'rear_ackermann_controller'], + ), + DeclareLaunchArgument('deactivate_talon', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('can_interface', default_value='can1'), + OpaqueFunction(function=launch_setup), + ]) + + +def launch_setup(context, *args, **kwargs): + mode = LaunchConfiguration('mode').perform(context) + use_sim = LaunchConfiguration('use_sim').perform(context) + use_mock_hardware = LaunchConfiguration('use_mock_hardware').perform(context) + mock_sensor_commands = LaunchConfiguration('mock_sensor_commands').perform(context) + robot_controller = LaunchConfiguration('robot_controller').perform(context) + deactivate_talon = LaunchConfiguration('deactivate_talon').perform(context) + can_interface = LaunchConfiguration('can_interface').perform(context) + + drive_share = get_package_share_directory('drive_bringup') + science_bringup_share = get_package_share_directory('science_bringup') + description_share = get_package_share_directory('description') + athena_gps_share = get_package_share_directory('athena_gps') + mag_heading_share = get_package_share_directory('mag_heading') + led_indicator_share = get_package_share_directory('led_indicator') + + # ------------------------------------------------------------------ + # Drive: unmodified subsystem launch, root namespace. + # ------------------------------------------------------------------ + drive = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(drive_share, 'launch', 'athena_drive.launch.py')), + launch_arguments={ + 'mode': mode, + 'use_sim': use_sim, + 'use_mock_hardware': use_mock_hardware, + 'mock_sensor_commands': mock_sensor_commands, + 'robot_controller': robot_controller, + 'can_interface': can_interface, + }.items(), + ) + + # ------------------------------------------------------------------ + # GPS + heading + LED indicator: unmodified, root namespace. + # ------------------------------------------------------------------ + gps = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(athena_gps_share, 'launch', 'gps_launch.py')), + launch_arguments={'sim': use_sim}.items(), + ) + heading = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(mag_heading_share, 'launch', 'mag_heading.launch.py')), + ) + led = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(led_indicator_share, 'launch', 'led_indicator.launch.py')), + ) + + # ------------------------------------------------------------------ + # Science: inlined, namespaced under /science so its CM/topics don't clash with drive. + # The science subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. + # ------------------------------------------------------------------ + science_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_science.urdf.xacro') + science_controllers_yaml = os.path.join(science_bringup_share, 'config', 'athena_science_controllers.yaml') + science_joystick_yaml = os.path.join(science_bringup_share, 'config', 'joystick.yaml') + + science_urdf = Command([ + FindExecutable(name='xacro'), ' ', + science_urdf_xacro, + ' prefix:=""', + ' use_mock_hardware:=', use_mock_hardware, + ' mock_sensor_commands:=', mock_sensor_commands, + ' deactivate_talon:=', deactivate_talon, ' ', + ' can_interface:=', can_interface, ' ', + ]) + + active_controllers = [ + 'science_controller', + 'laser_gpio_controller', + 'fluoro_led_gpio_controller', + ] + inactive_controllers = [ + 'joint_group_velocity_controller', + 'joint_group_position_controller', + ] + + science_cm = Node( + package='controller_manager', + executable='ros2_control_node', + namespace=_SCIENCE_NS, + output='both', + parameters=[science_controllers_yaml], + remappings=[('~/robot_description', 'robot_description')], + ) + + science_rsp = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=_SCIENCE_NS, + output='both', + parameters=[{'robot_description': science_urdf}], + ) + + def spawner(name, inactive=False): + args = [name, '-c', _SCIENCE_CM, '--param-file', science_controllers_yaml] + if inactive: + args.append('--inactive') + return Node( + package='controller_manager', + executable='spawner', + namespace=_SCIENCE_NS, + arguments=args, + ) + + jsb = spawner('joint_state_broadcaster') + motor_status = spawner('motor_status_controller') + active_spawners = [spawner(c) for c in active_controllers] + inactive_spawners = [spawner(c, inactive=True) for c in inactive_controllers] + + # Sequence: CM up → JSB → motor_status → active spawners (chained) → inactive spawners (chained). + delay_jsb = RegisterEventHandler(OnProcessStart( + target_action=science_cm, + on_start=[TimerAction(period=3.0, actions=[jsb])], + )) + + delay_motor_status = RegisterEventHandler(OnProcessExit( + target_action=jsb, + on_exit=[motor_status], + )) + + chained = [] + prev = motor_status + for sp in active_spawners + inactive_spawners: + chained.append(RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[sp]))) + prev = sp + + science_hardware = GroupAction([ + science_cm, + science_rsp, + delay_jsb, + delay_motor_status, + *chained, + ]) + + science_joystick = Node( + package='teleop', + executable='joystick', + namespace=_SCIENCE_NS, + name='joystick', + output='screen', + parameters=[science_joystick_yaml], + remappings=[ + ('controller_input', 'science_manual'), + ('/controller_input', '/science_manual'), + ], + ) + + if mode == 'jetson': + return [drive, science_hardware, gps, heading, led] + elif mode == 'base_station': + return [drive, science_joystick] + else: # standalone + return [drive, science_hardware, science_joystick, gps, heading, led] \ No newline at end of file diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index 24f7d2b6..32f3bc06 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -13,7 +13,7 @@ ${can_interface} 0x70 - + - + diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro index 37987cf4..2a25a88a 100644 --- a/src/description/ros2_control/science/science.mock.ros2_control.xacro +++ b/src/description/ros2_control/science/science.mock.ros2_control.xacro @@ -42,7 +42,7 @@ - + 2 0 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 c0046f2c..9c156054 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -11,12 +11,31 @@ 0x80 - + 0x1 1 - 235 - 0.00016666666 - standard + 300 + 0.00016666666 + continuous + revolute + + + + + + + + + + + + + + 0x0 + 1 + 300 + 0.00016666666 + continuous revolute @@ -30,12 +49,12 @@ - + 0x2 1 - 235 - 0.00016666666 - standard + 300 + 0.00016666666 + continuous revolute @@ -49,12 +68,13 @@ - - 0x3 + + 0x3 1 - 178 - standard - revolute + 300 + 0.00016666666 + continuous + revolute @@ -67,12 +87,13 @@ - - 0x5 + + 0x4 1 - 178 - standard - revolute + 300 + 0.00016666666 + continuous + revolute @@ -85,11 +106,10 @@ - - 0x6 + + 0x5 1 - 360 - 0.00016666666 + 178 standard revolute @@ -104,11 +124,10 @@ - - 0x4 + + 0x6 1 - 360 - 0.00016666666 + 178 standard revolute @@ -124,7 +143,7 @@ - 0x0 + 0x7 1 270 0.00016666666 @@ -141,6 +160,8 @@ + + diff --git a/src/description/ros2_control/science/science.talon.ros2_control.xacro b/src/description/ros2_control/science/science.talon.ros2_control.xacro index aabe1b57..735897c3 100644 --- a/src/description/ros2_control/science/science.talon.ros2_control.xacro +++ b/src/description/ros2_control/science/science.talon.ros2_control.xacro @@ -16,8 +16,8 @@ - - 2 + + 5 revolute 1425.1 @@ -38,8 +38,8 @@ - - 0 + + 6 revolute 64.0 diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index d8e053bb..fd9d635a 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -36,10 +36,10 @@ - + - + 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 aec7761f..24ffb475 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -29,15 +29,16 @@ joints: - stepper_motor_a - stepper_motor_b - - servo_rack_and_pinion_l - - servo_rack_and_pinion_r - - servo_scoop_a + - servo_scoops_lift_f + - servo_scoops_lift_b + - servo_sampler_lift_f + - servo_sampler_lift_b + - servo_conveyor_belt + - servo_scoop_f - servo_scoop_b + - servo_auger_lift - dc_scoop - - servo_sampler_lift_l - - servo_sampler_lift_l - dc_auger - - servo_auger_lift interface_name: position /**/joint_group_velocity_controller: @@ -45,40 +46,57 @@ joints: - stepper_motor_a - stepper_motor_b - - servo_rack_and_pinion_l - - servo_rack_and_pinion_r - - servo_scoop_a + - servo_scoops_lift_f + - servo_scoops_lift_b + - servo_sampler_lift_f + - servo_sampler_lift_b + - servo_conveyor_belt + - servo_scoop_f - servo_scoop_b + - servo_auger_lift - dc_scoop - - servo_sampler_lift_l - - servo_sampler_lift_r - dc_auger - - servo_auger_lift interface_name: velocity /**/science_controller: ros__parameters: + # Joint names # Pumps - pump_a: "stepper_motor_a" - pump_b: "stepper_motor_b" + pump_right: "stepper_motor_a" + pump_left: "stepper_motor_b" - # Lift - lift_rack_and_pinion_l: "servo_rack_and_pinion_l" - lift_rack_and_pinion_r: "servo_rack_and_pinion_r" + # Scoops Lift + scoops_lift_f: "servo_scoops_lift_f" + scoops_lift_b: "servo_scoops_lift_b" + + # Sampler + sampler_lift_f: "servo_sampler_lift_f" + sampler_lift_b: "servo_sampler_lift_b" + + # Conveyor Belt + conveyor_belt: "servo_conveyor_belt" # Scoops - scoop_servos: ["servo_scoop_a", "servo_scoop_b"] + scoop_servos: ["servo_scoop_f", "servo_scoop_b"] scoop_spinner: "dc_scoop" - # Sampler - sampler_lift_l: "servo_sampler_lift_l" - sampler_lift_r: "servo_sampler_lift_r" auger_spinner: "dc_auger" - cap: "servo_auger_lift" + auger_lift: "servo_auger_lift" interface_name: position command_interfaces: [position, velocity] state_interfaces: [position, velocity] + # Joint limits + velocity_limits_pumps: [20.0, 0.0, 0.6, 0.0] + velocity_limits_scoops_lift: [200.0, 0.1, 0.0, 0.3] + position_range_scoop_servo_f: [20.0, 120.0] + position_range_scoop_servo_b: [0.0, 110.0] + velocity_limits_scoop_spinner: [30.0, 0.0, 0.6, 0.0] + velocity_limits_sampler_lift: [200.0, 0.0, 0.0, 0.6] + velocity_limits_auger: [2.5, 0.0, 0.0, 0.6] + position_range_auger_lift: [5.0, 360.0] + velocity_limits_conveyor_belt: [200.0, 0.0, 0.0, 0.6] + # Laser GPIO Controller Configuration /**/laser_gpio_controller: ros__parameters: @@ -120,15 +138,16 @@ joints: - stepper_motor_a - stepper_motor_b - - servo_rack_and_pinion_l - - servo_rack_and_pinion_r - - servo_scoop_a + - servo_scoops_lift_f + - servo_scoops_lift_b + - servo_sampler_lift_f + - servo_sampler_lift_b + - servo_conveyor_belt + - servo_scoop_f - servo_scoop_b + - servo_auger_lift - dc_scoop - - servo_sampler_lift_l - - servo_sampler_lift_r - dc_auger - - servo_auger_lift gpios: - spectrometry_laser - fluoro_led @@ -146,17 +165,17 @@ dc_scoop: interfaces: - status_request - - maintenance_request - - maintenance_frame_high - - maintenance_frame_low - - maintenance_data_count + # - maintenance_request + # - maintenance_frame_high + # - maintenance_frame_low + # - maintenance_data_count dc_auger: interfaces: - status_request - - maintenance_request - - maintenance_frame_high - - maintenance_frame_low - - maintenance_data_count + # - maintenance_request + # - maintenance_frame_high + # - maintenance_frame_low + # - maintenance_data_count state_interfaces: stepper_motor_a: interfaces: @@ -164,13 +183,16 @@ stepper_motor_b: interfaces: - status - servo_rack_and_pinion_l: + servo_scoops_lift_f: + interfaces: + - status + servo_scoops_lift_b: interfaces: - status - servo_rack_and_pinion_r: + servo_conveyor_belt: interfaces: - status - servo_scoop_a: + servo_scoop_f: interfaces: - status servo_scoop_b: @@ -179,10 +201,10 @@ dc_scoop: interfaces: - status - servo_sampler_lift_l: + servo_sampler_lift_f: interfaces: - status - servo_sampler_lift_r: + servo_sampler_lift_b: interfaces: - status dc_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 6af6d49c..8be2da32 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 @@ -105,13 +105,16 @@ class ScienceManual : public controller_interface::ControllerInterface science_manual::Params params_; // Pumps - bool pump_toggle = false; - bool prev_pump_button_ = false; + int active_pump = 0; // 0 for pump a; 1 for pump b + int pump_right_toggle = 0; + int pump_left_toggle = 0; + bool prev_pump_up_button_ = false; + bool prev_pump_down_button_ = false; // Servos - bool servo_scoop_a_toggle = false; - bool prev_servo_scoop_a_button_ = false; - bool servo_scoop_b_toggle = false; + bool servo_scoop_f_toggle = true; + bool prev_servo_scoop_f_button_ = false; + bool servo_scoop_b_toggle = true; bool prev_servo_scoop_b_button_ = false; // Sampler @@ -124,25 +127,25 @@ class ScienceManual : public controller_interface::ControllerInterface int servo_scoop_b_counter; // TESTING - std::string pump_a; - std::string pump_b; - std::string lift_rack_and_pinion_l; - std::string lift_rack_and_pinion_r; + std::string pump_right; + std::string pump_left; + std::string scoops_lift_f; + std::string scoops_lift_b; std::vector scoop_servos; std::string scoop_spinner; - std::string sampler_lift_l; - std::string sampler_lift_r; + std::string sampler_lift_f; + std::string sampler_lift_b; + std::string conveyor_belt; std::string auger_spinner; std::string auger_lift; std::vector state_joints_; std::vector stepper_pump_joints_; - std::vector talon_joints_; - std::vector servo_joints_; - std::vector rack_pinion_joints_; + std::vector scoops_lift_joints_; std::vector dc_joints_; + std::vector servo_joints_; std::vector joints_; - //std::string talon_auger_; + //std::string dc_auger_; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; @@ -166,69 +169,60 @@ class ScienceManual : public controller_interface::ControllerInterface control_mode_type current_mode_{control_mode_type::STAGE1}; void load_velocity_limits(); // called in on_configure() - void send_commands( - double lift_cmd, - double stepper_cmd, - double scoop_cmd, - double auger_cmd, - double rack_left_cmd, - double rack_right_cmd - //double talon_auger_cmd - ); - }; - static constexpr double max_lift_velocity = 1.0; static constexpr double max_stepper_velocity = 1.0; static constexpr double scoop_talon_velocity = 1.0; static constexpr double auger_velocity = 1.0; - double stepper_cmd = 0.0; - double scoop_servo_a_position = 0.0; - double scoop_servo_b_position = 0.0; - double auger_lift_position = 0.0; - double rack_left_position = 0.0; - double rack_right_position = 0.0; - double sampler_lift_pos_l = 0.0; - double sampler_lift_pos_r = 360.0; + double pump_right_cmd = 0.0; + double pump_left_cmd = 0.0; + double scoops_lift_front_vel = 0.0; + double scoops_lift_back_vel = 0.0; + double sampler_lift_front_vel = 0.0; + double sampler_lift_back_vel = 0.0; + double conveyor_belt_vel = 0.0; + double scoop_servo_front_pos = 0.0; + double scoop_servo_back_pos = 0.0; + double auger_lift_pos = 0.0; enum CommandInterfaces { - // ----- PUMPS ----- - // --- Pump (position) --- - IDX_PUMP_A_VELOCITY = 0, - IDX_PUMP_B_VELOCITY = 1, - - - // ----- LIFT ----- - // --- Rack and Pinion (position) --- - IDX_LEFT_LIFT_POSITION = 2, - IDX_RIGHT_LIFT_POSITION = 3, - - - // ----- SCOOPS ----- - // --- Spinner (velocity) --- - IDX_SCOOP_SPINNER_VELOCITY = 4, - - // --- Servos (position) --- - IDX_SCOOP_A_POSITION = 5, - IDX_SCOOP_B_POSITION = 6, + // ----- STEPPERS ----- + // --- Pump --- + IDX_PUMP_RIGHT_VELOCITY = 0, + IDX_PUMP_LEFT_VELOCITY = 1, + + // ----- SERVOS ----- + // --- Scoops Lift --- + IDX_SCOOPS_LIFT_FRONT_VELOCITY = 2, + IDX_SCOOPS_LIFT_BACK_VELOCITY = 3, + + // --- Sampler Lift ---- + IDX_SAMPLER_LIFT_FRONT_VELOCITY = 4, + IDX_SAMPLER_LIFT_BACK_VELOCITY = 5, + + // --- Conveyor Belt --- + IDX_CONVEYOR_BELT_VELOCITY = 6, + + // --- Scoop Servos --- + IDX_SCOOP_FRONT_POSITION = 7, + IDX_SCOOP_BACK_POSITION = 8, + + // ----- Auger_lift (cap) ----- + IDX_AUGER_LIFT_POSITION = 9, - // ----- SAMPLER ----- - // --- Lift (velocity) ---- - IDX_SAMPLER_LIFT_LEFT_VELOCITY = 7, - IDX_SAMPLER_LIFT_RIGHT_VELOCITY = 8, - - // ----- Auger (velocity) ----- - IDX_AUGER_SPINNER_VELOCITY = 9, + // ----- DC MOTORS ----- + // --- Scoop Spinner --- + IDX_SCOOP_SPINNER_VELOCITY = 10, - // ----- Auger_lift (position) ----- - IDX_AUGER_LIFT_POSITION = 10, + // ----- Auger ----- + IDX_AUGER_SPINNER_VELOCITY = 11, // Total number of interfaces CMD_ITFS_COUNT }; - +}; }; // namespace science_controllers diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index c6997dd7..51288832 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -19,7 +19,7 @@ #include "controller_interface/helpers.hpp" -#define DEBUG_MODE 0 +#define DEBUG_MODE 1 namespace { @@ -44,7 +44,7 @@ controller_interface::CallbackReturn ScienceManual::on_init() { control_mode_.initRT(control_mode_type::STAGE1); - servo_scoop_a_toggle = false; + // servo_scoop_f_toggle = false; servo_scoop_b_counter = 0; // TESTING try { @@ -67,22 +67,23 @@ controller_interface::CallbackReturn ScienceManual::on_configure( current_mode_ = control_mode_type::STAGE1; // Fill each joint variable - pump_a = params_.pump_a; - pump_b = params_.pump_b; - lift_rack_and_pinion_l = params_.lift_rack_and_pinion_l; - lift_rack_and_pinion_r = params_.lift_rack_and_pinion_r; + pump_right = params_.pump_right; + pump_left = params_.pump_left; + scoops_lift_f = params_.scoops_lift_f; + scoops_lift_b = params_.scoops_lift_b; + sampler_lift_f = params_.sampler_lift_f; + sampler_lift_b = params_.sampler_lift_b; + conveyor_belt = params_.conveyor_belt; scoop_servos = params_.scoop_servos; scoop_spinner = params_.scoop_spinner; - sampler_lift_l = params_.sampler_lift_l; - sampler_lift_r = params_.sampler_lift_r; auger_spinner = params_.auger_spinner; auger_lift = params_.auger_lift; // Fill composite joint groups stepper_pump_joints_.clear(); stepper_pump_joints_ = { - params_.pump_a, - params_.pump_b + params_.pump_right, + params_.pump_left }; dc_joints_.clear(); @@ -92,27 +93,28 @@ controller_interface::CallbackReturn ScienceManual::on_configure( }; servo_joints_ = params_.scoop_servos; - servo_joints_.push_back(params_.sampler_lift_l); - servo_joints_.push_back(params_.sampler_lift_r); + servo_joints_.push_back(params_.sampler_lift_f); + servo_joints_.push_back(params_.sampler_lift_b); servo_joints_.push_back(params_.auger_lift); - rack_pinion_joints_.clear(); - rack_pinion_joints_.push_back(params_.lift_rack_and_pinion_l); - rack_pinion_joints_.push_back(params_.lift_rack_and_pinion_r); + scoops_lift_joints_.clear(); + scoops_lift_joints_.push_back(params_.scoops_lift_f); + scoops_lift_joints_.push_back(params_.scoops_lift_b); // Populate joints vector joints_.clear(); - joints_.push_back(pump_a); - joints_.push_back(pump_b); - joints_.push_back(lift_rack_and_pinion_l); - joints_.push_back(lift_rack_and_pinion_r); + joints_.push_back(pump_right); + joints_.push_back(pump_left); + joints_.push_back(scoops_lift_f); + joints_.push_back(scoops_lift_b); joints_.insert(joints_.end(), scoop_servos.begin(), scoop_servos.end()); joints_.push_back(scoop_spinner); - joints_.push_back(sampler_lift_l); - joints_.push_back(sampler_lift_r); + joints_.push_back(sampler_lift_f); + joints_.push_back(sampler_lift_b); + joints_.push_back(conveyor_belt); joints_.push_back(auger_spinner); joints_.push_back(auger_lift); @@ -132,13 +134,6 @@ controller_interface::CallbackReturn ScienceManual::on_configure( prev_buttons_.assign(joystick_buttons, 0); - // Start the Rack and Pinions off at their intended "0" position - // rack_left_position = params_.position_range_lift_left[1]; - // rack_right_position = params_.position_range_lift_right[0]; // Orientation is flipped, so we start from top - - rack_left_position = 185; - rack_right_position = 185; - // QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); @@ -146,7 +141,7 @@ controller_interface::CallbackReturn ScienceManual::on_configure( // Reference subscriber ref_subscriber_ = get_node()->create_subscription( - "/science_manual", subscribers_qos, + "/science/science_manual", subscribers_qos, std::bind(&ScienceManual::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -155,7 +150,7 @@ controller_interface::CallbackReturn ScienceManual::on_configure( // State publisher s_publisher_ = get_node()->create_publisher( - "/science_manual/state", rclcpp::QoS(rclcpp::KeepLast(1))); + "/science/science_manual/state", rclcpp::QoS(rclcpp::KeepLast(1))); state_publisher_ = std::make_unique(s_publisher_); if (state_publisher_ && state_publisher_->trylock()) { @@ -190,23 +185,32 @@ controller_interface::InterfaceConfiguration ScienceManual::command_interface_co cfg.names.push_back(joint + "/velocity"); } - // Lift - for (const auto & joint : rack_pinion_joints_) { - cfg.names.push_back(joint + "/position"); + // Scoops Lift + for (const auto & joint : scoops_lift_joints_) { + cfg.names.push_back(joint + "/velocity"); } - // Scoops - cfg.names.push_back(params_.scoop_spinner + "/velocity"); + // Sampler Lift + cfg.names.push_back(sampler_lift_f + "/velocity"); + cfg.names.push_back(sampler_lift_b + "/velocity"); + + // Conveyor Belt + cfg.names.push_back(conveyor_belt + "/velocity"); + + // Scoop servos for (const auto & joint : scoop_servos) { cfg.names.push_back(joint + "/position"); } + // Auger Lift + cfg.names.push_back(auger_lift + "/position"); + + // Scoop spinner + cfg.names.push_back(scoop_spinner + "/velocity"); + // Sampler - cfg.names.push_back(sampler_lift_l + "/position"); - cfg.names.push_back(sampler_lift_r + "/position"); cfg.names.push_back(auger_spinner + "/velocity"); - cfg.names.push_back(auger_lift + "/position"); return cfg; } @@ -248,7 +252,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( stepper_pump_joints_.clear(); dc_joints_.clear(); servo_joints_.clear(); - rack_pinion_joints_.clear(); + scoops_lift_joints_.clear(); state_joints_.clear(); RCLCPP_INFO(get_node()->get_logger(), "Manual controller deactivated and released interfaces"); @@ -257,7 +261,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( controller_interface::return_type ScienceManual::update( const rclcpp::Time & /*time*/, - const rclcpp::Duration & period) + const rclcpp::Duration & /*period*/) { auto current_ref = input_ref_.readFromRT(); @@ -265,75 +269,80 @@ controller_interface::return_type ScienceManual::update( return controller_interface::return_type::OK; } - double dt = period.seconds(); auto msg = *current_ref; // shared_ptr int stage_idx = static_cast(current_mode_); // corresponds to STAGE1..STAGE4 // -- Pumps -- // Stepper motors command (velocity-like value, but we feed as position target here) - bool pump_button = (msg->buttons.size() > 2 && msg->buttons[2]); - if (pump_button && !prev_pump_button_) { - pump_toggle = !pump_toggle; + if(msg->buttons[12]){ // Left arrow + active_pump = 0; + } + else if (msg->buttons[11]){ // Right arrow + active_pump = 1; } - prev_pump_button_ = pump_button; - stepper_cmd = pump_toggle * params_.velocity_limits_pumps[stage_idx]; - - - // -- Lift -- - // Rack & pinion - // Use stage-dependent "speed" for rack & pinion motion (reuse stepper limits) - double rack_speed = params_.velocity_limits_lift[stage_idx]; - double axis_left = (msg->axes.size() > 1) ? msg->axes[1] : 0.0; - double axis_right = (msg->axes.size() > 1) ? msg->axes[3] : 0.0; - // Integrate axes into positions (position += axis * speed * dt) + if(active_pump == 0){ // Pump A + if (msg->buttons[13] && !prev_pump_up_button_) { // Up arrow + pump_right_toggle++; + } + if (msg->buttons[14] && !prev_pump_down_button_) { // Down Arrow + pump_right_toggle--; + } + pump_right_toggle = std::clamp(pump_right_toggle, -1, 1); + } + else if (active_pump == 1){ // Pump B + if (msg->buttons[13] && !prev_pump_up_button_) { // Up arrow + pump_left_toggle++; + } + if (msg->buttons[14] && !prev_pump_down_button_) { // Down Arrow + pump_left_toggle--; + } + pump_left_toggle = std::clamp(pump_left_toggle, -1, 1); + } - double left_gain = 1.0; - double right_gain = 1.0; - // rack_left_position += axis_left * rack_speed * left_gain * dt; - // rack_right_position -= axis_right * rack_speed * right_gain * dt; + prev_pump_up_button_ = msg->buttons[13]; + prev_pump_down_button_ = msg->buttons[14]; - // ** TEMPORARY FOR SAR - bool lift_button = (msg->buttons.size() > 11 && msg->buttons[12]); - if (lift_button && !prev_lift_button) { - lift_toggle = !lift_toggle; - } - prev_lift_button = lift_button; + pump_right_cmd = pump_right_toggle * params_.velocity_limits_pumps[stage_idx]; + pump_left_cmd = pump_left_toggle * params_.velocity_limits_pumps[stage_idx]; + + // -- Scoops Lift -- // Hardcoded values for top and bottom lift position - rack_left_position = lift_toggle ? 203.0 : 156.0; - rack_right_position = lift_toggle ? 137.0 : 225.0; + scoops_lift_front_vel = msg->axes[1] * params_.velocity_limits_scoops_lift[stage_idx]; + scoops_lift_back_vel = -msg->axes[1] * params_.velocity_limits_scoops_lift[stage_idx]; // ** - // -- Scoops -- + // Sampler Lift + // double sampler_lift_speed = params_.velocity_limits_sampler_lift[stage_idx]; + double axis_sampler_lift = (msg->axes.size() > 3) ? msg->axes[3] : 0.0; + sampler_lift_front_vel = axis_sampler_lift * params_.velocity_limits_sampler_lift[stage_idx]; + sampler_lift_back_vel = axis_sampler_lift * params_.velocity_limits_sampler_lift[stage_idx]; + // sampler_lift_front_vel += sampler_lift_speed * axis_sampler_lift * dt; + // sampler_lift_back_vel -= sampler_lift_speed * axis_sampler_lift * dt; + // Scoop Spinner (Right Trigger for Vel, Right Bumper for direction) double scoop_axis = (msg->axes.size() > 5) ? msg->axes[5] : 0.0; bool scoop_reverse = (msg->buttons.size() > 5 && msg->buttons[5]); double scoop_spinner_cmd = scoop_axis * params_.velocity_limits_scoop_spinner[stage_idx] * (scoop_reverse ? -1.0 : 1.0); + // Conveyor Belt L/R Right joystick + conveyor_belt_vel = params_.velocity_limits_conveyor_belt[stage_idx] * msg->axes[2]; + // Scoop Servo - // Servo A is square - bool servo_scoop_a_button = (msg->buttons.size() > 1 && msg->buttons[3]); - if (servo_scoop_a_button && !prev_servo_scoop_a_button_) { - servo_scoop_a_toggle = !servo_scoop_a_toggle; + bool servo_scoop_f_button = (msg->buttons.size() > 1 && msg->buttons[3]); + if (servo_scoop_f_button && !prev_servo_scoop_f_button_) { + servo_scoop_f_toggle = !servo_scoop_f_toggle; } - prev_servo_scoop_a_button_ = servo_scoop_a_button; - scoop_servo_a_position = servo_scoop_a_toggle * params_.position_range_scoop_servo[1]; + prev_servo_scoop_f_button_ = servo_scoop_f_button; + scoop_servo_front_pos = servo_scoop_f_toggle * params_.position_range_scoop_servo_f[1]; - // Servo B is X bool servo_scoop_b_button = (msg->buttons.size() > 0 && msg->buttons[0]); if (servo_scoop_b_button && !prev_servo_scoop_b_button_) { servo_scoop_b_toggle = !servo_scoop_b_toggle; } prev_servo_scoop_b_button_ = servo_scoop_b_button; - scoop_servo_b_position = servo_scoop_b_toggle * params_.position_range_scoop_servo[1]; - - // -- Sampler -- - // Sampler Lift - double sampler_lift_speed = params_.velocity_limits_sampler_lift[stage_idx]; - double axis_sampler_lift = (msg->axes.size() > 3) ? msg->axes[3] : 0.0; - sampler_lift_pos_l += sampler_lift_speed * axis_sampler_lift * dt; - sampler_lift_pos_r -= sampler_lift_speed * axis_sampler_lift * dt; + scoop_servo_back_pos = servo_scoop_b_toggle * params_.position_range_scoop_servo_b[1]; // Auger (Left Trigger for Vel, Left Bumper for direction) double auger_axis = (msg->axes.size() > 4) ? msg->axes[4] : 0.0; @@ -346,7 +355,7 @@ controller_interface::return_type ScienceManual::update( // auger_lift_toggle = !auger_lift_toggle; // } // prev_auger_lift_button_ = auger_lift_button; - // auger_lift_position = auger_lift_toggle ? params_.position_range_auger_lift[1] : params_.position_range_auger_lift[0]; + // auger_lift_pos = auger_lift_toggle ? params_.position_range_auger_lift[1] : params_.position_range_auger_lift[0]; double auger_lift_axis = (msg->axes.size() > 0) ? msg->axes[0] : 0.0; // Use absolute value so direction doesn't matter @@ -360,74 +369,152 @@ controller_interface::return_type ScienceManual::update( // Default = max_pos // More joystick deflection → move toward min_pos - auger_lift_position = max_pos - axis_mag * (max_pos - min_pos); + auger_lift_pos = max_pos - axis_mag * (max_pos - min_pos); // Clamp Positions - rack_left_position = std::clamp(rack_left_position, 0.0, 360.0); - rack_right_position = std::clamp(rack_right_position, 0.0, 360.0); - scoop_servo_a_position = std::clamp(scoop_servo_a_position, params_.position_range_scoop_servo[0], params_.position_range_scoop_servo[1]); - scoop_servo_b_position = std::clamp(scoop_servo_b_position, params_.position_range_scoop_servo[0], params_.position_range_scoop_servo[1]); - sampler_lift_pos_l = std::clamp(sampler_lift_pos_l, params_.position_range_sampler_lift[0], params_.position_range_sampler_lift[1]); - sampler_lift_pos_r = std::clamp(sampler_lift_pos_r, params_.position_range_sampler_lift[0], params_.position_range_sampler_lift[1]); - auger_lift_position = std::clamp(auger_lift_position, params_.position_range_auger_lift[0], params_.position_range_auger_lift[1]); + // scoops_lift_front_vel = std::clamp(scoops_lift_front_vel, 0.0, 360.0); + // scoops_lift_back_vel = std::clamp(scoops_lift_back_vel, 0.0, 360.0); + scoop_servo_front_pos = std::clamp(scoop_servo_front_pos, params_.position_range_scoop_servo_f[0], params_.position_range_scoop_servo_f[1]); + scoop_servo_back_pos = std::clamp(scoop_servo_back_pos, params_.position_range_scoop_servo_b[0], params_.position_range_scoop_servo_b[1]); + // sampler_lift_front_vel = std::clamp(sampler_lift_front_vel, params_.position_range_sampler_lift[0], params_.position_range_sampler_lift[1]); + // sampler_lift_back_vel = std::clamp(sampler_lift_back_vel, params_.position_range_sampler_lift[0], params_.position_range_sampler_lift[1]); + auger_lift_pos = std::clamp(auger_lift_pos, params_.position_range_auger_lift[0], params_.position_range_auger_lift[1]); // SET VALUES // Stepper motors (position) - command_interfaces_[IDX_PUMP_A_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); - command_interfaces_[IDX_PUMP_B_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); - // DC motors (velocity) - command_interfaces_[IDX_SAMPLER_LIFT_LEFT_VELOCITY].set_value(sampler_lift_pos_l * (M_PI / 180.0)); - command_interfaces_[IDX_SAMPLER_LIFT_RIGHT_VELOCITY].set_value(sampler_lift_pos_r * (M_PI / 180.0)); - command_interfaces_[IDX_SCOOP_SPINNER_VELOCITY].set_value(scoop_spinner_cmd); + command_interfaces_[IDX_PUMP_RIGHT_VELOCITY].set_value(pump_right_cmd * (M_PI / 180.0)); + command_interfaces_[IDX_PUMP_LEFT_VELOCITY].set_value(pump_left_cmd * (M_PI / 180.0)); + + // -- Servos -- + // Scoops Lift servos + command_interfaces_[IDX_SCOOPS_LIFT_FRONT_VELOCITY].set_value(scoops_lift_front_vel * (M_PI / 180.0)); + command_interfaces_[IDX_SCOOPS_LIFT_BACK_VELOCITY].set_value(scoops_lift_back_vel * (M_PI / 180.0)); + + // Sampler Lift servos + command_interfaces_[IDX_SAMPLER_LIFT_FRONT_VELOCITY].set_value(sampler_lift_front_vel * (M_PI / 180.0)); + command_interfaces_[IDX_SAMPLER_LIFT_BACK_VELOCITY].set_value(sampler_lift_back_vel * (M_PI / 180.0)); + + // Conveyor Belt + command_interfaces_[IDX_CONVEYOR_BELT_VELOCITY].set_value(conveyor_belt_vel * (M_PI / 180.0)); + // Scoop servos - command_interfaces_[IDX_SCOOP_A_POSITION].set_value(scoop_servo_a_position * (M_PI / 180.0)); - servo_scoop_b_counter++; // TESTING - // if (servo_scoop_b_counter % 100 == 0) { // TESTING - command_interfaces_[IDX_SCOOP_B_POSITION].set_value(scoop_servo_b_position * (M_PI / 180.0)); - // } - // Auger & auger_lift + command_interfaces_[IDX_SCOOP_FRONT_POSITION].set_value(scoop_servo_front_pos * (M_PI / 180.0)); + command_interfaces_[IDX_SCOOP_BACK_POSITION].set_value(scoop_servo_back_pos * (M_PI / 180.0)); + + // Auger Lift + command_interfaces_[IDX_AUGER_LIFT_POSITION].set_value(auger_lift_pos * (M_PI / 180.0)); + + // -- DC -- + // Scoops Spinner + command_interfaces_[IDX_SCOOP_SPINNER_VELOCITY].set_value(scoop_spinner_cmd); + + // Auger Spinner command_interfaces_[IDX_AUGER_SPINNER_VELOCITY].set_value(auger_spinner_cmd); - command_interfaces_[IDX_AUGER_LIFT_POSITION].set_value(auger_lift_position * (M_PI / 180.0)); - // Rack & pinion servos - command_interfaces_[IDX_LEFT_LIFT_POSITION].set_value(rack_left_position * (M_PI / 180.0)); - command_interfaces_[IDX_RIGHT_LIFT_POSITION].set_value(rack_right_position * (M_PI / 180.0)); + + // Logger std::ostringstream oss; + const char * active_pump_name = (active_pump == 0) ? "Pump A" : "Pump B"; oss << "\033[2J\033[H\n" << "========= Science Controller =========\n\n" - - << " [PUMPS]\n" - << " triangle : " << pump_button - << " | pump_toggle : " << pump_toggle - << " | stepper_cmd : " << std::fixed << std::setprecision(3) << stepper_cmd << "\n\n" - - << " [LIFT]\n" - << " right_joystick_btn : " << lift_button - << " | lift_toggle : " << lift_toggle << "\n" - << " rack_left_position : " << rack_left_position << "\n" - << " rack_right_position : " << rack_right_position << "\n\n" - - << " [SCOOPS]\n" - << " right_trigger_axis : " << scoop_axis - << " | right_bumper : " << scoop_reverse - << " | scoop_spinner_cmd : " << scoop_spinner_cmd << "\n" - << " square : " << servo_scoop_a_button - << " | toggle : " << servo_scoop_a_toggle - << " | scoop_servo_a_pos : " << scoop_servo_a_position << "\n" - << " x : " << servo_scoop_b_button - << " | toggle : " << servo_scoop_b_toggle - << " | scoop_servo_b_pos : " << scoop_servo_b_position << "\n\n" - - << " [SAMPLER]\n" - << " right_joystick_ud : " << axis_sampler_lift - << " | sampler_lift_pos_l : " << sampler_lift_pos_l - << " | sampler_lift_pos_r : " << sampler_lift_pos_r << "\n" - << " left_trigger_axis : " << auger_axis - << " | left_bumper : " << auger_reverse - << " | auger_spinner_cmd : " << auger_spinner_cmd << "\n" - << " circle : " - << ((msg->buttons.size() > 1) ? msg->buttons[1] : 0) - << " | auger_lift_cmd : " << auger_lift_position << "\n"; + << " active pump: " << active_pump_name << " (" << active_pump << ")\n\n" + << std::fixed << std::setprecision(3) + << " " + << std::left << std::setw(4) << "idx" + << std::setw(18) << "actuator" + << std::setw(30) << "xbox control" + << std::setw(24) << "input value" + << "command\n" + << " " + << std::setw(4) << "---" + << std::setw(18) << "-----------------" + << std::setw(30) << "-----------------------------" + << std::setw(24) << "-----------------------" + << "----------------\n" + << " " + << std::setw(4) << "[0]" + << std::setw(18) << "Right Pump " + << std::setw(30) << "Right D-pad, Up/Down D-pad" + << std::setw(24) << ("R/U/D=" + std::to_string((msg->buttons.size() > 11) ? msg->buttons[11] : 0) + "/" + + std::to_string((msg->buttons.size() > 13) ? msg->buttons[13] : 0) + "/" + + std::to_string((msg->buttons.size() > 14) ? msg->buttons[14] : 0) + + " t=" + std::to_string(pump_right_toggle)) + << pump_right_cmd << " deg/s\n" + << " " + << std::setw(4) << "[1]" + << std::setw(18) << "Left Pump" + << std::setw(30) << "Left D-pad, Up/Down D-pad" + << std::setw(24) << ("L/U/D=" + std::to_string((msg->buttons.size() > 12) ? msg->buttons[12] : 0) + "/" + + std::to_string((msg->buttons.size() > 13) ? msg->buttons[13] : 0) + "/" + + std::to_string((msg->buttons.size() > 14) ? msg->buttons[14] : 0) + + " t=" + std::to_string(pump_left_toggle)) + << pump_left_cmd << " deg/s\n" + << " " + << std::setw(4) << "[2]" + << std::setw(18) << "Scoops Lift Front" + << std::setw(30) << "Left joystick U/D" + << std::setw(24) << ("axis[1]=" + std::to_string((msg->axes.size() > 1) ? msg->axes[1] : 0.0)) + << scoops_lift_front_vel << " deg/s\n" + << " " + << std::setw(4) << "[3]" + << std::setw(18) << "Scoops Lift Back" + << std::setw(30) << "Left joystick U/D" + << std::setw(24) << ("axis[1]=" + std::to_string((msg->axes.size() > 1) ? msg->axes[1] : 0.0)) + << scoops_lift_back_vel << " deg/s\n" + << " " + << std::setw(4) << "[4]" + << std::setw(18) << "Sampler Lift Front" + << std::setw(30) << "Right joystick U/D" + << std::setw(24) << ("axis[3]=" + std::to_string(axis_sampler_lift)) + << sampler_lift_front_vel << " deg/s\n" + << " " + << std::setw(4) << "[5]" + << std::setw(18) << "Sampler Lift Back" + << std::setw(30) << "Right joystick U/D" + << std::setw(24) << ("axis[3]=" + std::to_string(axis_sampler_lift)) + << sampler_lift_back_vel << " deg/s\n" + << " " + << std::setw(4) << "[6]" + << std::setw(18) << "Conveyor Belt" + << std::setw(30) << "Right joystick L/R" + << std::setw(24) << ("axis[2]=" + std::to_string((msg->axes.size() > 2) ? msg->axes[2] : 0.0)) + << conveyor_belt_vel << " deg/s\n" + << " " + << std::setw(4) << "[7]" + << std::setw(18) << "Scoop Door Front" + << std::setw(30) << "Y button" + << std::setw(24) << ("btn[3]=" + std::to_string((msg->buttons.size() > 3) ? msg->buttons[3] : 0) + + " t=" + std::to_string(servo_scoop_f_toggle)) + << scoop_servo_front_pos << " deg\n" + << " " + << std::setw(4) << "[8]" + << std::setw(18) << "Scoop Door Back" + << std::setw(30) << "A button" + << std::setw(24) << ("btn[0]=" + std::to_string((msg->buttons.size() > 0) ? msg->buttons[0] : 0) + + " t=" + std::to_string(servo_scoop_b_toggle)) + << scoop_servo_back_pos << " deg\n" + << " " + << std::setw(4) << "[9]" + << std::setw(18) << "Auger Lift" + << std::setw(30) << "Left joystick L/R" + << std::setw(24) << ("axis[0]=" + std::to_string((msg->axes.size() > 0) ? msg->axes[0] : 0.0)) + << auger_lift_pos << " deg\n" + << " " + << std::setw(4) << "[10]" + << std::setw(18) << "Scoop Spinner" + << std::setw(30) << "Right trigger, Right bumper" + << std::setw(24) << ("trig/btn=" + std::to_string(scoop_axis) + "/" + + std::to_string((msg->buttons.size() > 5) ? msg->buttons[5] : 0) + + " r=" + std::to_string(scoop_reverse)) + << scoop_spinner_cmd << " deg/s\n" + << " " + << std::setw(4) << "[11]" + << std::setw(18) << "Auger Spinner" + << std::setw(30) << "Left trigger, Left bumper" + << std::setw(24) << ("trig/btn=" + std::to_string(auger_axis) + "/" + + std::to_string((msg->buttons.size() > 4) ? msg->buttons[4] : 0) + + " r=" + std::to_string(auger_reverse)) + << auger_spinner_cmd << " deg/s\n"; if(DEBUG_MODE == 1){ RCLCPP_INFO(get_node()->get_logger(), "%s", oss.str().c_str()); @@ -438,9 +525,9 @@ controller_interface::return_type ScienceManual::update( 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_.set_point = pump_right_cmd; state_publisher_->msg_.process_value = auger_spinner_cmd; - state_publisher_->msg_.command = sampler_lift_pos_l; + state_publisher_->msg_.command = sampler_lift_front_vel; state_publisher_->unlockAndPublish(); } } @@ -452,4 +539,4 @@ controller_interface::return_type ScienceManual::update( #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - science_controllers::ScienceManual, controller_interface::ControllerInterface) \ No newline at end of file + science_controllers::ScienceManual, controller_interface::ControllerInterface) diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index eb1938cf..07fc2322 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -31,7 +31,7 @@ science_manual: type: string_array default_value: ["velocity", "position"] - pump_a: + pump_right: type: string default_value: "stepper_motor_a" description: "First Stepper Motor" @@ -39,7 +39,7 @@ science_manual: validation: not_empty<>: null - pump_b: + pump_left: type: string default_value: "stepper_motor_b" description: "Second Stepper Motor" @@ -47,25 +47,43 @@ science_manual: validation: not_empty<>: null - lift_rack_and_pinion_l: + scoops_lift_f: type: string - default_value: "servo_rack_and_pinion_l" + default_value: "servo_scoops_lift_f" description: "The Servo motor controller for the left-sided rack and pinion" read_only: true validation: not_empty<>: null - lift_rack_and_pinion_r: + scoops_lift_b: type: string - default_value: "servo_rack_and_pinion_r" + default_value: "servo_scoops_lift_b" description: "The Servo motor controller for the right-sided rack and pinion" read_only: true validation: not_empty<>: null + sampler_lift_f: + type: string + default_value: "servo_sampler_lift_f" + description: "(Optional)" + read_only: false + + sampler_lift_b: + type: string + default_value: "servo_sampler_lift_b" + description: "(Optional)" + read_only: false + + conveyor_belt: + type: string + default_value: "servo_conveyor_belt" + description: "(Optional)" + read_only: false + scoop_servos: type: string_array - default_value: ["servo_scoop_a", "servo_scoop_b"] + default_value: ["servo_scoop_f", "servo_scoop_b"] description: "The servos for the scoop (2 servos)" read_only: true validation: @@ -80,18 +98,6 @@ science_manual: validation: not_empty<>: null - sampler_lift_l: - type: string - default_value: "servo_sampler_lift_l" - description: "(Optional)" - read_only: false - - sampler_lift_r: - type: string - default_value: "servo_sampler_lift_r" - description: "(Optional)" - read_only: false - auger_spinner: type: string default_value: "dc_auger" @@ -113,60 +119,46 @@ science_manual: validation: fixed_size<>: 4 - # Lift - velocity_limits_lift: + # Scoops + # Scoops Lift + velocity_limits_scoops_lift: type: double_array - default_value: [20.0, 0.1, 0.0, 0.3] + default_value: [200.0, 0.1, 0.0, 0.3] validation: fixed_size<>: 4 - position_range_lift_left: - type: double_array - default_value: [75.0, 235.0] - validation: - fixed_size<>: 2 - - position_range_lift_right: + # Scoops + position_range_scoop_servo_f: type: double_array - default_value: [30.0, 235.0] + default_value: [20.0, 120.0] validation: fixed_size<>: 2 - # Scoops - position_range_scoop_servo: + position_range_scoop_servo_b: type: double_array - default_value: [0.0, 120.0] + default_value: [0.0, 110.0] validation: fixed_size<>: 2 + # Scoops Spinner velocity_limits_scoop_spinner: type: double_array - default_value: [15.0, 0.0, 0.6, 0.0] + default_value: [30.0, 0.0, 0.6, 0.0] validation: fixed_size<>: 4 # Sampler - position_range_sampler_lift: - type: double_array - default_value: [0.0, 360.0] - validation: - fixed_size<>: 2 - + # Sampler Lift velocity_limits_sampler_lift: type: double_array - default_value: [50.0, 0.0, 0.0, 0.6] + default_value: [200.0, 0.0, 0.0, 0.6] validation: fixed_size<>: 4 + # Auger velocity_limits_auger: type: double_array - default_value: [5.0, 0.0, 0.0, 0.6] - validation: - fixed_size<>: 4 - - velocity_limits_auger_lift: - type: double_array - default_value: [10.0, 0.0, 0.0, 0.6] + default_value: [2.5, 0.0, 0.0, 0.6] validation: fixed_size<>: 4 @@ -175,3 +167,10 @@ science_manual: default_value: [5.0, 360.0] validation: fixed_size<>: 2 + + # Conveyor Belt + velocity_limits_conveyor_belt: + type: double_array + default_value: [200.0, 0.0, 0.0, 0.6] + validation: + fixed_size<>: 4 diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index f9d9c771..22085c89 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -57,7 +57,7 @@ def __init__(self): self.controller = None self.button_data = None self.hat_data = None - self.activation = 0.08 + self.activation = 0.05 self.joystick_index = None # Pygame Controller @@ -89,7 +89,7 @@ def __init__(self): time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: - print("Joystick connected.") + self.get_logger().info("Joystick connected.") pygame.joystick.init() joysticks = pygame.joystick.get_count() break @@ -118,13 +118,18 @@ def __init__(self): for i in range(self.controller.get_numhats()): self.hat_data[i] = (0, 0) - self.previous_axes = np.zeros(6) - self.previous_buttons = np.zeros(13) + self.axes_count = self.controller.get_numaxes() + self.button_count = self.controller.get_numbuttons() + self.hats_count = self.controller.get_numhats() + self.full_button_count = self.button_count + 4*self.hats_count # each hat has a tuple (x, y) which can be -1, 0, or 1 + + self.previous_axes = np.zeros(self.axes_count) + self.previous_buttons = np.zeros(self.full_button_count) def controller_inputs(self): - joystick_vels = self.previous_axes - button_activations = self.previous_buttons + joystick_vels = np.zeros(self.axes_count) + button_activations = np.zeros(self.full_button_count) for event in pygame.event.get(): if event.type == pygame.JOYAXISMOTION: self.axis_data[event.axis] = round(event.value,2) @@ -214,8 +219,8 @@ def controller_inputs(self): joystick_vels[5] = 0 # Buttons - for i in range(13): - button_activations[i] = self.button_data[i] + for i in range(self.button_count): + button_activations[i] = self.button_data.get(i, False) elif(self.joystick_type == 1): @@ -290,8 +295,9 @@ def controller_inputs(self): joystick_vels[3] = 0 # Buttons - for i in range(13): - button_activations[i] = self.button_data[i] + for i in range(self.button_count): + button_activations[i] = self.button_data.get(i, False) + elif(self.joystick_type == 2 or self.joystick_type == 3): """ @@ -313,8 +319,13 @@ def controller_inputs(self): [5] = right bumper [6] = TV [7] = Menu - [8] = left joystick button - [9] = right joystick button + [8] = X button + [9] = left joystick button + [10] = right joystick button + [11] = Right D-pad + [12] = Left D-pad + [13] = Up D-pad + [14] = Down D-pad """ # Left stick: left and right @@ -364,10 +375,23 @@ def controller_inputs(self): joystick_vels[5] = 0 # Buttons - for i in range(min(10, len(button_activations))): + for i in range(self.button_count): button_activations[i] = self.button_data.get(i, False) + + + # Hats + hat_x, hat_y = self.controller.get_hat(0) + button_activations[11] = (hat_x == 1) + button_activations[12] = (hat_x == -1) + + button_activations[13] = (hat_y == 1) + button_activations[14] = (hat_y == -1) + + # self.get_logger().info(f"hat_x: {hat_x} hat_y: {hat_y}") + # self.get_logger().info(f"Button count: {self.button_count} Full count: {self.full_button_count}") + # self.get_logger().info(f"button_activations: {button_activations}") else: - print('calibrate:', pprint.pprint(self.axis_data)) + self.get_logger().info('calibrate:') # Save current numpy array for joystick and buttons self.previous_axes = joystick_vels