diff --git a/src/main/java/frc/robot/config/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/RobotConfigPhoenix.java index 794ea58f..4cf14a61 100644 --- a/src/main/java/frc/robot/config/RobotConfigPhoenix.java +++ b/src/main/java/frc/robot/config/RobotConfigPhoenix.java @@ -39,6 +39,29 @@ public RobotConfigPhoenix() { new Transform3d( new Translation3d(-Units.inchesToMeters(10.75), 0, Units.inchesToMeters(8)), new Rotation3d(0, Units.degreesToRadians(-33), Units.degreesToRadians(180))))); + cameras.add( + new VisionCamera( + "intake", + "1184", + new Transform3d( + new Translation3d(0.3048, 0, 0.22), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(0))))); + + cameras.add( + new VisionCamera( + "right", + "1196", + new Transform3d( + new Translation3d(-Units.inchesToMeters(5.25),-Units.inchesToMeters(11.25), Units.inchesToMeters(7)), + new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(-90))))); + + cameras.add( + new VisionCamera( + "left", + "1190", + new Transform3d( + new Translation3d(-Units.inchesToMeters(5.25), Units.inchesToMeters(11.25), Units.inchesToMeters(7)), + new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(90))))); VisionConstants.visionDistanceOffsetInMeters = -0.2; vision = new VisionSubsystem(cameras, AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()); diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index aa61f714..f44c745c 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -307,7 +307,7 @@ public static void setupMainControls(CommandXboxController mainController) { Map.entry( true, AutoBuilder.pathfindToPoseFlipped( - new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)), + new Pose2d(1.89, 7.75, Rotation2d.fromDegrees(-90)), new PathConstraints(3.0, 2.0, 2 * Math.PI, 3 * Math.PI))), Map.entry( false,