Merge pull request #17 from Team537/MobiliT

fix: Fix complex auto paths to function as intended
Team537 · Apr 4, 2024 · c3285fb · c3285fb
2 parents 60fc2d0 + 7cfdfb4
commit c3285fb
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 14 deletions.
diff --git 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
@@ -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),