diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bdbc81c..a6026980 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,42 +126,43 @@ public RobotContainer(RobotRunType runtimeType) { for (int i = 0; i < 7; i++) { numNoteChooser.addOption(String.valueOf(i), i); } - cameras = - /* - * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right - */ - new PhotonCameraWrapper[] { - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontLeftFacingCamera.KCAMERA_TO_ROBOT), - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT), - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.BackLeftFacingCamera.CAMERA_NAME, - Constants.CameraConstants.BackLeftFacingCamera.CAMERA_IP), - Constants.CameraConstants.BackLeftFacingCamera.KCAMERA_TO_ROBOT)}; - // new PhotonCameraWrapper( - // new PhotonReal(Constants.CameraConstants.BackRightFacingCamera.CAMERA_NAME), - // Constants.CameraConstants.BackRightFacingCamera.KCAMERA_TO_ROBOT)}; switch (runtimeType) { case kReal: + cameras = + /* + * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right + */ + new PhotonCameraWrapper[] {new PhotonCameraWrapper( + new PhotonReal(Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_NAME, + Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_IP), + Constants.CameraConstants.FrontLeftFacingCamera.KCAMERA_TO_ROBOT), + new PhotonCameraWrapper( + new PhotonReal( + Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, + Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), + Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT), + new PhotonCameraWrapper( + new PhotonReal( + Constants.CameraConstants.BackLeftFacingCamera.CAMERA_NAME, + Constants.CameraConstants.BackLeftFacingCamera.CAMERA_IP), + Constants.CameraConstants.BackLeftFacingCamera.KCAMERA_TO_ROBOT)}; + // new PhotonCameraWrapper( + // new PhotonReal(Constants.CameraConstants.BackRightFacingCamera.CAMERA_NAME), + // Constants.CameraConstants.BackRightFacingCamera.KCAMERA_TO_ROBOT)}; shooter = new Shooter(new ShooterVortex()); intake = new Intake(new IntakeIOFalcon()); s_Swerve = new Swerve(new SwerveReal(), cameras); elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator); break; case kSimulation: - s_Swerve = new Swerve(new SwerveIO() {}, cameras); + s_Swerve = new Swerve(new SwerveIO() {}, new PhotonCameraWrapper[] {}); shooter = new Shooter(new ShooterIO() {}); intake = new Intake(new IntakeIO() {}); elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); break; default: - s_Swerve = new Swerve(new SwerveIO() {}, cameras); + s_Swerve = new Swerve(new SwerveIO() {}, new PhotonCameraWrapper[] {}); shooter = new Shooter(new ShooterIO() {}); intake = new Intake(new IntakeIO() {}); elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); diff --git a/src/main/java/frc/robot/autos/P8765.java b/src/main/java/frc/robot/autos/P8765.java index 22195ad1..4b637cca 100644 --- a/src/main/java/frc/robot/autos/P8765.java +++ b/src/main/java/frc/robot/autos/P8765.java @@ -3,10 +3,10 @@ import java.util.function.BooleanSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory.State; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -124,10 +124,9 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Sho Command wait = Commands.waitSeconds(.01); Command resetPosition = Commands.runOnce(() -> { - Pose2d initialState = - FieldConstants.allianceFlip(path0.getPreviewStartingHolonomicPose() - .plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180)))); - swerveDrive.resetOdometry(initialState); + State initalState = path0.getTrajectory(null, null).getInitialState(); + Pose2d initialPose = FieldConstants.allianceFlip(initalState.getTargetHolonomicPose()); + swerveDrive.resetOdometry(initialPose); }); Command followPaths = part0.andThen(Commands.either(