From 4b7ce85314773d5a38cefc8d28964fb53b586e8e Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sun, 22 Feb 2026 20:53:05 +0000 Subject: [PATCH 1/7] fix: new eef id and current limit --- src/Arm/arm_control/launch/end_effector.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Arm/arm_control/launch/end_effector.launch.py b/src/Arm/arm_control/launch/end_effector.launch.py index ce2e01ad..ba23a875 100644 --- a/src/Arm/arm_control/launch/end_effector.launch.py +++ b/src/Arm/arm_control/launch/end_effector.launch.py @@ -17,8 +17,9 @@ def generate_launch_description(): plugin="ros_phoenix::TalonSRX", name="end_effector", parameters=[ - {"id": 13}, + {"id": 21}, {"max_voltage": 24.0}, + {"max_current": 6.0}, {"brake_mode": True}, ], ), From e20f8d557ddc9412ea39061b5737ade018d644a3 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sun, 22 Feb 2026 20:54:00 +0000 Subject: [PATCH 2/7] chore: format --- src/Bringup/launch/control.launch.py | 8 ++++++-- src/HW-Devices/hardware/src/TalonSRXWrapper.cpp | 14 ++++++++++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/Bringup/launch/control.launch.py b/src/Bringup/launch/control.launch.py index 598e0439..6f61f2f8 100644 --- a/src/Bringup/launch/control.launch.py +++ b/src/Bringup/launch/control.launch.py @@ -111,11 +111,15 @@ def generate_launch_description(): condition=IfCondition(use_arm), ) - delayed_spawners = RegisterEventHandler( OnProcessStart( target_action=controller_manager, - on_start=[jsb_spawner, chassis_controller_spawner, arm_controller_move_it_spawner, arm_controller_servo_spawner], + on_start=[ + jsb_spawner, + chassis_controller_spawner, + arm_controller_move_it_spawner, + arm_controller_servo_spawner, + ], ) ) diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index f7f77a26..2b86629e 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -123,6 +123,15 @@ void TalonSRXWrapper::write() { } return; } + if (std::abs(command_ - position_) > M_PI && + control_type_ == motors::ControlMode::Position) { + RCLCPP_WARN_THROTTLE( + debug_node_->get_logger(), *debug_node_->get_clock(), 1000, + "%s: Large position error (%.2f) for id %d, which may indicate a " + "problem with sensor configuration or an unexpected jump in position", + __FUNCTION__, command_ - position_, id_); + return; + } double output = 0.0; if (crossover_mode_ && std::abs(command_) > M_PI) { RCLCPP_WARN_THROTTLE( @@ -140,7 +149,8 @@ void TalonSRXWrapper::write() { if (output < 0) output += sensor_ticks_; } else { - output = (command_)*sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_; + output = (command_ + M_PI) * sensor_ticks_ / (2.0 * M_PI) + + sensor_offset_ticks_; } } else if (control_type_ == motors::ControlMode::Velocity) { // Talons use d / 100ms as vel @@ -160,7 +170,7 @@ void TalonSRXWrapper::read() { double normalized = static_cast(raw_position) / sensor_ticks_; position_ = normalized * 2.0 * M_PI - M_PI; } else { - position_ = raw_position * (2.0 * M_PI) / sensor_ticks_; + position_ = raw_position * (2.0 * M_PI) / sensor_ticks_ - M_PI; } double raw_velocity = talon_controller_->GetSelectedSensorVelocity(); // Talons use d / 100ms as vel From 0cc0b108a45a6236c80718707e080d5f400131f3 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sun, 22 Feb 2026 20:55:30 +0000 Subject: [PATCH 3/7] fix: launch bugs --- .../video_streaming/config/cameras.yaml | 2 +- .../launch/controller.launch.py | 68 +++++++++++-------- .../joystick_control/src/arm.cpp | 7 ++ 3 files changed, 46 insertions(+), 31 deletions(-) diff --git a/src/Cameras/video_streaming/config/cameras.yaml b/src/Cameras/video_streaming/config/cameras.yaml index 8eb49928..7ebaeac9 100644 --- a/src/Cameras/video_streaming/config/cameras.yaml +++ b/src/Cameras/video_streaming/config/cameras.yaml @@ -11,7 +11,7 @@ input_node: path: "/dev/v4l/by-id/drive_camera" type: 0 EndEffector: - path: "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0" + path: "/dev/v4l/by-id/usb-MACROSILICON_USB_Video_20200909-video-index0" type: 0 encoded: true Bottom: diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 0b473d96..cd47170e 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -32,25 +32,29 @@ def generate_launch_description(): id_arm = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick") id_drive = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick") - return LaunchDescription( - [ - # Node( - # package="joy", - # executable="joy_node", - # name="joy_node_a", - # parameters=[ - # { - # "device_id": id_drive, - # "deadzone": 0.05, - # } - # ], - # remappings=[("/joy", "/drive/joy")], - # ), + ld = LaunchDescription() + + if id_drive != -1: + ld.add_action( + Node( + package="joy", + executable="joy_node", + name="joy_node_a", + parameters=[ + { + "device_id": id_drive, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/drive/joy")], + ) + ) + if id_arm != -1: + ld.add_action( Node( package="joy", executable="joy_node", name="joy_node_arm", - condition=IfCondition(PythonExpression([str(id_arm), " >= 0"])), parameters=[ { "device_id": id_arm, @@ -59,19 +63,23 @@ def generate_launch_description(): ], remappings=[("/joy", "/arm/joy")], ), - Node( - package="joystick_control", - executable="arm", - name="arm_teleop_node", - parameters=[parameters_file], - remappings=[("/joy", "/arm/joy")], - ), - # Node( - # package="joystick_control", - # executable="drive", - # name="drive_teleop_node", - # parameters=[parameters_file], - # remappings=[("/joy", "/drive/joy")], - # ), - ] + ) + ld.add_action( + Node( + package="joystick_control", + executable="arm", + name="arm_teleop_node", + parameters=[parameters_file], + remappings=[("/joy", "/arm/joy")], + ), + ) + ld.add_action( + Node( + package="joystick_control", + executable="drive", + name="drive_teleop_node", + parameters=[parameters_file], + remappings=[("/joy", "/drive/joy")], + ), ) + return ld diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 8771f21e..4b4ea194 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -120,6 +120,9 @@ void arm::arm_control(std::shared_ptr joystickMsg) { moveit_servo_state(true); RCLCPP_INFO(this->get_logger(), "Switched to manual control"); } + if (current_state_ != NONE) { + endeffector_control(joystickMsg); + } switch (current_state_) { case MANUAL: @@ -187,6 +190,8 @@ void arm::declareParameters() { this->declare_parameter("ik_button", 10); this->declare_parameter("manual_button", 11); this->declare_parameter("disable_button", 9); + this->declare_parameter("claw_close_button", 0); + this->declare_parameter("claw_open_button", 1); } void arm::loadParameters() { this->get_parameter("throttle.axis", kThrottleAxis); @@ -200,6 +205,8 @@ void arm::loadParameters() { this->get_parameter("ik_button", kIkButton); this->get_parameter("manual_button", kManualButton); this->get_parameter("disable_button", kDisableButton); + this->get_parameter("claw_close_button", kClawOpen); + this->get_parameter("claw_open_button", kClawClose); } int main(int argc, char **argv) { From d4a4ee990cbbd49e00b232ef510d972eba7d5c47 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sun, 22 Feb 2026 20:55:54 +0000 Subject: [PATCH 4/7] feat: arm tuning --- .../urdf/arm_urdf.ros2_control.xacro | 32 +++++++++---------- src/URDF/rover_urdf/urdf/arm_urdf.xacro | 8 ++--- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 8ac90407..727a5211 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -10,14 +10,14 @@ TalonSRX 14 - 5.0 - 0.0 + 11.0 + 0.0005 1.0 0.0 absolute 4096 1.57 - true + false false false @@ -27,14 +27,14 @@ TalonSRX 11 - 4.0 - 0.1 - 0.5 + 6.0 + 0.001 + 0.01 0.0 absolute 4096 -1.29154 - true + false false false @@ -44,14 +44,14 @@ TalonSRX 12 - 1.0 - 0.0 + 4.0 + 0.001 0.0 0.0 absolute 4096 1.6057 - true + false true true @@ -61,14 +61,14 @@ TalonSRX 19 - 2.0 - 0.1 - 0.2 + 8.5 + 0.001 + 0.001 0.0 absolute 4096 0.191986 - true + false false false @@ -78,8 +78,8 @@ TalonSRX 15 - 0.15 - 0.0 + 0.01 + 0.000001 0.0 0.0 quadrature diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.xacro index 9714cb7a..3592b557 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.xacro @@ -118,7 +118,7 @@ link="Link_2" /> - + @@ -361,7 +361,7 @@ link="Link_5" /> - + @@ -430,7 +430,7 @@ link="Link_6" /> - + @@ -499,7 +499,7 @@ link="EndEffector" /> - + From 48a5cc1fdfb6ad1b726a21648e59cb73ba9f0a93 Mon Sep 17 00:00:00 2001 From: Jetson Nano1 Date: Mon, 23 Feb 2026 03:35:39 +0000 Subject: [PATCH 5/7] fix: use joy_linux for correct device detection --- rosdep-keys.txt | 3 +- .../launch/controller.launch.py | 77 +++++++------------ 2 files changed, 31 insertions(+), 49 deletions(-) diff --git a/rosdep-keys.txt b/rosdep-keys.txt index 4a9c4743..c06fbd1b 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -85,4 +85,5 @@ libncurses-dev usbutils tinyxml2 clang-tidy -python3-vcstool \ No newline at end of file +python3-vcstool +joy_linux \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index cd47170e..fc2bb13f 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -8,62 +8,43 @@ from ament_index_python.packages import get_package_share_directory -def get_joy_id(dev_path): - """Checks for a symlink and returns the integer ID, or a default.""" - if os.path.exists(dev_path): - # Resolves /dev/joy_a -> /dev/input/jsX - real_path = os.path.realpath(dev_path) - # Pulls the digits out of the path string - try: - print(f"Found symlink {dev_path} -> {real_path}") - return int("".join(filter(str.isdigit, real_path))) - except ValueError: - print(f"ERROR: Device {real_path} not a event joystick path") - return -1 - else: - print(f"ERROR: Symlink {dev_path} does not exist") - return -1 - - def generate_launch_description(): pkg_joystick_control = get_package_share_directory("joystick_control") parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml") # Detect IDs dynamically - id_arm = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick") - id_drive = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick") + arm_dev = "/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick" + drive_dev = "/dev/input/by-id/usb-Thrustmaster_T.16000M-joystick" ld = LaunchDescription() - if id_drive != -1: - ld.add_action( - Node( - package="joy", - executable="joy_node", - name="joy_node_a", - parameters=[ - { - "device_id": id_drive, - "deadzone": 0.05, - } - ], - remappings=[("/joy", "/drive/joy")], - ) - ) - if id_arm != -1: - ld.add_action( - Node( - package="joy", - executable="joy_node", - name="joy_node_arm", - parameters=[ - { - "device_id": id_arm, - "deadzone": 0.05, - } - ], - remappings=[("/joy", "/arm/joy")], - ), + ld.add_action( + Node( + package="joy_linux", + executable="joy_linux_node", + name="joy_node_a", + parameters=[ + { + "dev": drive_dev, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/drive/joy")], ) + ) + ld.add_action( + Node( + package="joy_linux", + executable="joy_linux_node", + name="joy_node_arm", + parameters=[ + { + "dev": arm_dev, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/arm/joy")], + ), + ) ld.add_action( Node( package="joystick_control", From d41308c4631ac226398c6df8a7895fa9b1f8762c Mon Sep 17 00:00:00 2001 From: Jetson Date: Mon, 23 Feb 2026 04:02:20 +0000 Subject: [PATCH 6/7] final fixes --- rosdep-keys.txt | 3 ++- src/HW-Devices/hardware/src/TalonSRXWrapper.cpp | 6 +++--- src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro | 8 ++++---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/rosdep-keys.txt b/rosdep-keys.txt index c06fbd1b..019dcdd8 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -86,4 +86,5 @@ usbutils tinyxml2 clang-tidy python3-vcstool -joy_linux \ No newline at end of file +joy_linux +position_controllers \ No newline at end of file diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index 2b86629e..15cc5a9d 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -149,8 +149,7 @@ void TalonSRXWrapper::write() { if (output < 0) output += sensor_ticks_; } else { - output = (command_ + M_PI) * sensor_ticks_ / (2.0 * M_PI) + - sensor_offset_ticks_; + output = command_ * sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_; } } else if (control_type_ == motors::ControlMode::Velocity) { // Talons use d / 100ms as vel @@ -170,7 +169,7 @@ void TalonSRXWrapper::read() { double normalized = static_cast(raw_position) / sensor_ticks_; position_ = normalized * 2.0 * M_PI - M_PI; } else { - position_ = raw_position * (2.0 * M_PI) / sensor_ticks_ - M_PI; + position_ = raw_position * (2.0 * M_PI) / sensor_ticks_; } double raw_velocity = talon_controller_->GetSelectedSensorVelocity(); // Talons use d / 100ms as vel @@ -239,6 +238,7 @@ void TalonSRXWrapper::configure() { command_ = position_; } talon_controller_->Set(motors::ControlMode::Disabled, 0.0); + talon_controller_->ClearStickyFaults(); RCLCPP_INFO(debug_node_->get_logger(), "%s: Successfully configured Motor Controller %d", __FUNCTION__, id_); diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 727a5211..92dd7165 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -16,7 +16,7 @@ 0.0 absolute 4096 - 1.57 + 0.0 false false false @@ -33,7 +33,7 @@ 0.0 absolute 4096 - -1.29154 + 0.0 false false false @@ -50,7 +50,7 @@ 0.0 absolute 4096 - 1.6057 + 0.0 false true true @@ -67,7 +67,7 @@ 0.0 absolute 4096 - 0.191986 + 0.0 false false false From 7b5373bac20f391406f18c45419c544dd45069d7 Mon Sep 17 00:00:00 2001 From: Jetson Nano1 Date: Mon, 23 Feb 2026 04:18:35 +0000 Subject: [PATCH 7/7] fix: arm directions manual mode --- src/Teleop-Control/joystick_control/src/arm.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 4b4ea194..0d0fc6cb 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -147,13 +147,13 @@ void arm::manual_arm_control( joint_msg.header.stamp = joystickMsg->header.stamp; - joint_msg.velocities = {axes[kJoint1Axis], - axes[kJoint2Axis], + joint_msg.velocities = {-axes[kJoint1Axis], + -axes[kJoint2Axis], axes[kJoint3Axis], axes[kJoint4Axis], - static_cast(buttons[kWristYaw_positive] - - buttons[kWristYaw_negative]), - axes[kJoint6Axis]}; + -static_cast(buttons[kWristYaw_positive] - + buttons[kWristYaw_negative]), + -axes[kJoint6Axis]}; joint_pub_->publish(joint_msg); } @@ -161,6 +161,7 @@ void arm::manual_arm_control( void arm::ik_arm_control(std::shared_ptr joystickMsg) { auto twist_msg = geometry_msgs::msg::TwistStamped(); twist_msg.header.stamp = joystickMsg->header.stamp; + twist_msg.header.frame_id = "Link_6"; auto &axes = joystickMsg->axes; auto &buttons = joystickMsg->buttons;