diff --git a/rosdep-keys.txt b/rosdep-keys.txt index 4a9c4743..019dcdd8 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -85,4 +85,6 @@ libncurses-dev usbutils tinyxml2 clang-tidy -python3-vcstool \ No newline at end of file +python3-vcstool +joy_linux +position_controllers \ No newline at end of file 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}, ], ), 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/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/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index f7f77a26..15cc5a9d 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,7 @@ void TalonSRXWrapper::write() { if (output < 0) output += sensor_ticks_; } else { - output = (command_)*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 @@ -229,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/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 0b473d96..fc2bb13f 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -8,70 +8,59 @@ 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() - return LaunchDescription( - [ - # Node( - # package="joy", - # executable="joy_node", - # name="joy_node_a", - # parameters=[ - # { - # "device_id": id_drive, - # "deadzone": 0.05, - # } - # ], - # remappings=[("/joy", "/drive/joy")], - # ), - Node( - package="joy", - executable="joy_node", - name="joy_node_arm", - condition=IfCondition(PythonExpression([str(id_arm), " >= 0"])), - parameters=[ - { - "device_id": id_arm, - "deadzone": 0.05, - } - ], - 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="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", + 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..0d0fc6cb 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: @@ -144,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); } @@ -158,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; @@ -187,6 +191,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 +206,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) { 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..92dd7165 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 + 0.0 + 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 + 0.0 + 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 + 0.0 + 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 + 0.0 + 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" /> - +