From 7cfdfb41ead5241d75cae8ba26867001302d1a78 Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Wed, 3 Apr 2024 19:37:25 -0500 Subject: [PATCH] fix: Fix complex auto paths to function as intended This commit should make the robot follow the paths as intended. --- src/main/java/frc/robot/Constants.java | 15 ++++++--------- src/main/java/frc/robot/RobotContainer.java | 12 +++++++----- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0968079..182d0e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -163,21 +163,18 @@ public static final class VisionConstants { // Camera settings public static final String LIMELIGHT_NAME = "limelight"; - public static final String ARDUCAM_OV9281_USB_CAMERA = "Arducam_OV9281_USB_Camera"; - public static final String ARDUCAM_OV2311_USB_CAMERA = "Arducam_OV2311_USB_Camera"; - public static final String COLOR_CAMERA_NAME = "USB_2M_GS_camera"; + public static final String ARDUCAM_OV9281_USB_CAMERA_NAME = "Arducam_OV9281_USB_Camera"; + public static final String ARDUCAM_OV2311_USB_CAMERA_NAME = "Arducam_OV2311_USB_Camera"; + public static final String USB_2M_GS_CAMERA_NAME = "USB_2M_GS_camera"; // Camera offsets (NOTE: These values are placeholders and thus subject to change). - public static final Transform3d RED_OUTTAKE_CAMERA_OFFSET = new Transform3d( + public static final Transform3d ARDUCAM_OV9281_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - public static final Transform3d RED_INTAKE_CAMERA_OFFSET = new Transform3d( + public static final Transform3d ARDUCAM_OV2311_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - public static final Transform3d BLACK_OUTTAKE_CAMERA_OFFSET = new Transform3d( - new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d BLACK_INTAKE_CAMERA_OFFSET = new Transform3d( + public static final Transform3d USB_2M_GS_CAMERA_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15e5e45..3e1683b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,7 +39,9 @@ public class RobotContainer { // The robot's subsystems private final RobotVision robotVision = new RobotVision.Builder() - .addPhotonVisionCamera(VisionConstants.ARDUCAM_OV9281_USB_CAMERA, VisionConstants.RED_OUTTAKE_CAMERA_OFFSET, + .addPhotonVisionCamera(VisionConstants.ARDUCAM_OV2311_USB_CAMERA_NAME, VisionConstants.ARDUCAM_OV9281_OFFSET, + VisionConstants.APRIL_TAG_PIPELINE) + .addPhotonVisionCamera(VisionConstants.USB_2M_GS_CAMERA_NAME, VisionConstants.USB_2M_GS_CAMERA_OFFSET, VisionConstants.APRIL_TAG_PIPELINE) .build(); private final DriveSubsystem driveSubsystem = new DriveSubsystem(true, robotVision::estimateRobotPose); @@ -251,7 +253,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption) complexPath = new SequentialCommandGroup( // Shoot note new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp. - AutoConstants.BLUE_1_COMPLEX_POSITIONS.subList(0, 1)), + AutoConstants.BLUE_1_COMPLEX_POSITIONS.subList(0, 2)), // Grab note new FollowTrajectoryCommand(driveSubsystem, List.of( AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp @@ -293,7 +295,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption) complexPath = new SequentialCommandGroup( // Score note new FollowTrajectoryCommand(driveSubsystem, - AutoConstants.BLUE_3_COMPLEX_POSITIONS.subList(0, 1)), + AutoConstants.BLUE_3_COMPLEX_POSITIONS.subList(0, 2)), // Grab note new FollowTrajectoryCommand(driveSubsystem, List.of( AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(0), @@ -316,7 +318,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption) complexPath = new SequentialCommandGroup( // Shoot note new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp. - AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)), + AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 2)), // Grab note new FollowTrajectoryCommand(driveSubsystem, List.of( AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp @@ -357,7 +359,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption) complexPath = new SequentialCommandGroup( // Score note new FollowTrajectoryCommand(driveSubsystem, - AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)), + AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 2)), // Grab note new FollowTrajectoryCommand(driveSubsystem, List.of( AutoConstants.RED_1_COMPLEX_POSITIONS.get(0),