diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b3d9491..34267af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.SpikeController; -import frc.robot.commands.EmptyHopper; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.findexer.Findexer; @@ -41,6 +40,12 @@ public class RobotContainer { private final SwerveRequest.FieldCentric driveCmd = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + + private final SwerveRequest.FieldCentricFacingAngle snapCmd = new SwerveRequest.FieldCentricFacingAngle() + .withDeadband(MaxSpeed * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withHeadingPID(6, 0, 0.2); + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); @@ -52,10 +57,10 @@ public class RobotContainer { public static CommandSwerveDrivetrain drive; private final Vision vision; private final Turret turret; - private final Intake intake; + private final Intake intake; private final Trigger trigger; private final Shooter shooter; - private final Targeting targeting; + private final Targeting targeting; private final Findexer findexer; public RobotContainer() { @@ -72,8 +77,6 @@ public RobotContainer() { SmartDashboard.putData("Auto Path", autoChooser); configureBindings(); - - NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, targeting, 15)); } public static CommandSwerveDrivetrain getDrive() { @@ -91,24 +94,55 @@ private void setupSwerveBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. drive.setDefaultCommand( - // Drivetrain will execute this command periodically - drive.applyRequest(() -> driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive - // forward with - // negative Y - // (forward) - .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise - // with negative X (left) - )); - + drive.applyRequest(() -> { + + double speedMultiplier = driverController.rightBumper().getAsBoolean() ? 0.3 : 1.0; + + double vx = -driverController.getLeftY() * MaxSpeed * speedMultiplier; + double vy = -driverController.getLeftX() * MaxSpeed * speedMultiplier; + + // Snap angles + if (driverController.y().getAsBoolean()) { // Up + return snapCmd + .withVelocityX(vx) + .withVelocityY(vy) + .withTargetDirection(Rotation2d.fromDegrees(0)); + } + else if (driverController.b().getAsBoolean()) { // Right + return snapCmd + .withVelocityX(vx) + .withVelocityY(vy) + .withTargetDirection(Rotation2d.fromDegrees(270)); + } + else if (driverController.a().getAsBoolean()) { // Down + return snapCmd + .withVelocityX(vx) + .withVelocityY(vy) + .withTargetDirection(Rotation2d.fromDegrees(180)); + } + else if (driverController.x().getAsBoolean()) { // Left + return snapCmd + .withVelocityX(vx) + .withVelocityY(vy) + .withTargetDirection(Rotation2d.fromDegrees(90)); + } + + double angularMultiplier = driverController.rightBumper().getAsBoolean() ? 0.3 : 1.0; + + return driveCmd + .withVelocityX(vx) + .withVelocityY(vy) + .withRotationalRate(-driverController.getRightX() * MaxAngularRate * angularMultiplier); + }) + ); // Idle while the robot is disabled. This ensures the configured // neutral mode is applied to the drive motors while disabled. final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue( drive.applyRequest(() -> idle).ignoringDisable(true)); - driverController.a().whileTrue(drive.applyRequest(() -> brake)); - driverController.b().whileTrue(drive.applyRequest(() -> point + driverController.leftTrigger().whileTrue(drive.applyRequest(() -> brake)); + driverController.rightTrigger().whileTrue(drive.applyRequest(() -> point .withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())))); // Run SysId routines when holding back/start and X/Y. @@ -133,9 +167,8 @@ private void setupSwerveBindings() { private void setupIntakeBindings() { // toggle intake on A press - operatorController.rightBumper().onTrue(intake.runOnce(intake::toggleIntake)); - - operatorController.a().onTrue(intake.runOnce(intake::switchDirection)); + driverController.leftBumper().onTrue(intake.runOnce(intake::toggle)); + driverController.povUp().onTrue(intake.runOnce(intake::switchDirection)); } private void setupTargetingBindings() { @@ -146,10 +179,10 @@ private void setupTargetingBindings() { private void setupShooterBindings() { // toggle shooter on right trigger hold - driverController.rightTrigger() + operatorController.rightTrigger() .onTrue(shooter.runOnce(() -> shooter.setDriverRequestingShooting(true))) .onFalse(shooter.runOnce(() -> shooter.setDriverRequestingShooting(false))); - driverController.leftTrigger() + operatorController.leftTrigger() .onTrue(shooter.runOnce(() -> shooter.setRequestingWithForce(true))) .onFalse(shooter.runOnce(() -> shooter.setRequestingWithForce(false))); @@ -162,8 +195,8 @@ private void setupShooterBindings() { operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); - operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2))); - operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2))); + operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(1))); + operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-1))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java deleted file mode 100644 index cd72c64..0000000 --- a/src/main/java/frc/robot/commands/EmptyHopper.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.targeting.Targeting; - -public class EmptyHopper extends Command { - private Shooter shooter; - private Targeting targeting; - - private double scoringTime; - private Timer scoringTimer = new Timer(); - - public EmptyHopper(Shooter shooter, Targeting targeting, double forTime) { - this.shooter = shooter; - this.targeting = targeting; - - this.scoringTime = forTime; - - this.shooter.setDriverRequestingShooting(true); - - this.targeting.setTargetingHub(); - } - - @Override - public void initialize() { - this.scoringTimer.restart(); - this.targeting.setTargetingHub(); - } - - @Override - public void execute() { - this.targeting.setTargetingHub(); - } - - @Override - public boolean isFinished() { - return this.scoringTimer.hasElapsed(scoringTime); - } - - @Override - public void end(boolean interrupted) { - // TODO Auto-generated method stub - this.shooter.setDriverRequestingShooting(false); - } -} diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 6100bea..ec45c91 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -4,7 +4,8 @@ import frc.robot.subsystems.trigger.Trigger; public class Findexer extends SpikeSystem { - private static final double FEEDING_RPS = -30.0 * 9; // feeding velocity in rotations per second + // multiplied by the gear ratio of the findexer (36:1) + private static final double FEEDING_RPS = -3*36; // feeding velocity in rotations per second private final Trigger trigger; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index fa22074..2e333d2 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -91,6 +91,10 @@ private void doDeployedState() { // } // Update the intakeIO on speed + if (forward == false) { + intakeIO.setIntakeSpeed(-70); + return; + } intakeIO.setIntakeSpeed(70); } @@ -128,6 +132,10 @@ public void switchDirection() { forward = !forward; } + public void toggle() { + running = !running; + } + // Disables the intake public void disable() { running = false; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 0d5535d..f49be54 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -70,6 +70,11 @@ public void updateInputs(IntakeIOInputs inputs) { // double speed - Speed to set the motor to in Rotations Per Second @Override public void setIntakeSpeed(double speed) { + if (speed == 0) { + intakeMotor.stopMotor(); + return; + } + // intakeMotor.set(speed); this.velocityControl.withVelocity(speed); intakeMotor.setControl(this.velocityControl); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 8d55030..8e24c9c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -93,10 +93,10 @@ public ShooterIOTalonFX() { // flywheel configs (units in AMPS) var flywheelSlot0 = new Slot0Configs(); - flywheelSlot0.kP = 20; // amps / rps of error + flywheelSlot0.kP = 14; // amps / rps of error flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; - flywheelSlot0.kS = 0.0; // amps needed to overcome static friction + flywheelSlot0.kS = 15.0; // amps needed to overcome static friction flywheelSlot0.kV = 0.0; // not used for torque control this.flywheelMotor.getConfigurator().apply(flywheelSlot0); @@ -174,16 +174,13 @@ public void updateInputs(ShooterIOInputs inputs) { public void setFlywheelVelocity(double rps) { this.flywheelRPSSetPoint = rps; - // boolean firstCommand = Double.isNaN(lastAppliedFlywheelRPSSetPoint); - // boolean meaningfulChange = firstCommand - // || (Math.abs(rps - lastAppliedFlywheelRPSSetPoint) >= FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS); - - // if (!meaningfulChange) { - // return; - // } + if (rps == 0) { + flywheelMotor.stopMotor(); + } else { + this.flywheelControl.withVelocity(rps); + flywheelMotor.setControl(this.flywheelControl); + } - this.flywheelControl.withVelocity(rps); - flywheelMotor.setControl(this.flywheelControl); this.lastAppliedFlywheelRPSSetPoint = rps; } /** diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index c3930d2..670fe07 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -5,39 +5,27 @@ public class ShotData { public static final InterpolatingDoubleTreeMap distanceToRPM = new InterpolatingDoubleTreeMap(); public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); - + public static final InterpolatingDoubleTreeMap distanceToTOFConstant = new InterpolatingDoubleTreeMap(); //TOF = Time of Flight + static { - distanceToRPM.put(3.14, 2200.0); - distanceToHoodAngle.put(3.14, 30.0); - - distanceToRPM.put(4.7, 2550.0); - distanceToHoodAngle.put(4.7, 36.0); - - distanceToRPM.put(1.5, 1900.0); - distanceToHoodAngle.put(1.5, 15.0); - - distanceToRPM.put(2.5, 2300.0); - distanceToHoodAngle.put(2.5, 25.0); - - // distanceToRPM.put(2.96, 2300.0); - // distanceToHoodAngle.put(2.96, 30.0); - - // distanceToRPM.put(2.7, 1900.0); - // distanceToHoodAngle.put(2.7, 30.0); + distanceToRPM.put(1.0, 1800.0); + distanceToHoodAngle.put(1.0, 15.0); + distanceToTOFConstant.put(1.0, 0.8); + distanceToTOFConstant.put(4.0, 0.9 ); - // distanceToRPM.put(3.0, 2000.0); - // distanceToHoodAngle.put(3.0, 30.0); + distanceToRPM.put(2.0, 2050.0); + distanceToHoodAngle.put(2.0, 23.0); - // distanceToRPM.put(3.6, 2100.0); - // distanceToHoodAngle.put(3.6, 30.0); + distanceToRPM.put(3.0, 2200.0); + distanceToHoodAngle.put(3.0, 30.0); - // distanceToRPM.put(4.0, 2250.0); - // distanceToHoodAngle.put(4.0, 30.0); + distanceToRPM.put(4.0, 2500.0); + distanceToHoodAngle.put(4.0, 32.0); - // distanceToRPM.put(5.5, 2400.0); - // distanceToHoodAngle.put(5.5, 40.0); + distanceToRPM.put(5.3, 2650.0); + distanceToHoodAngle.put(5.3, 35.0); - distanceToRPM.put(7.5, 3100.0); - distanceToHoodAngle.put(7.5, 40.0); + distanceToRPM.put(17.069, 5000.0); + distanceToHoodAngle.put(17.069, 45.0); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 61e5da2..68455dc 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -32,7 +33,6 @@ public class Targeting extends SubsystemBase { private boolean overrideRedAlliance = false; private boolean overrideBlueAlliance = false; - public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); @@ -47,46 +47,63 @@ public static Translation2d differenceBetweenRobotAndTarget() { // calculate field-relative angle of the turret based on the turret motor position and the robot's heading // get pose of robo Pose2d robotPose = RobotContainer.getDrive().getPose(); + Translation2d robotPos = robotPose.getTranslation(); - // translate robot-center pose to the turret pivot location on the field - Translation2d turretPivot = Turret.TURRET_OFFSET_FROM_CENTER + Translation2d goalPose = targetPos; + + Translation2d turretPivotNow = Turret.TURRET_OFFSET_FROM_CENTER .rotateBy(robotPose.getRotation()) .plus(robotPose.getTranslation()); - // vector from the turret pivot directly to the goal - Translation2d goalPose = targetPos; - Translation2d toGoal = goalPose.minus(turretPivot); + double staticDistance = goalPose.getDistance(turretPivotNow); + double tof = ShotData.distanceToTOFConstant.get(staticDistance); + // translate robot-center pose to the turret pivot location on the field + + ChassisSpeeds robotRelSpeeds = RobotContainer.getDrive().getState().Speeds; + + ChassisSpeeds speeds = + ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelSpeeds.vxMetersPerSecond, + robotRelSpeeds.vyMetersPerSecond, + robotRelSpeeds.omegaRadiansPerSecond, + robotPose.getRotation() + ); + + double vx = speeds.vxMetersPerSecond; + double vy = speeds.vyMetersPerSecond; + + Translation2d predictedRobotPos = robotPos.plus( + new Translation2d(vx * tof, vy * tof) + ); + + Rotation2d predictedHeading = + robotPose.getRotation().plus( + Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * tof) + ); + + Translation2d predictedTurretPivot = Turret.TURRET_OFFSET_FROM_CENTER + .rotateBy(predictedHeading) + .plus(predictedRobotPos); + + Translation2d toGoalComp = goalPose.minus(predictedTurretPivot); - Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); - Logger.recordOutput("Targeting/TargetPose", new Pose2d(goalPose, new Rotation2d())); - return toGoal; + Logger.recordOutput("Targeting/StaticDistance", staticDistance); + Logger.recordOutput("Targeting/PredictedRobotPos", new Pose2d(predictedRobotPos, predictedHeading)); + Logger.recordOutput("Targeting/PredictedTurretPivot", new Pose2d(predictedTurretPivot, predictedHeading)); + Logger.recordOutput("Targeting/ToGoalCompensation", toGoalComp); + Logger.recordOutput("Targeting/TimeOfFlight", tof); + + return toGoalComp; } - + /** * Periodically calculate the shot data given the target position and robot movement */ @Override public void periodic() { - Pose2d robotPose = drive.getPose(); - - // compute the turret pivot location in field coordinates so ShotCompensation - Translation2d turretPivot = Turret.TURRET_OFFSET_FROM_CENTER - .rotateBy(robotPose.getRotation()) - .plus(robotPose.getTranslation()); - Pose2d turretPivotPose = new Pose2d(turretPivot, robotPose.getRotation()); - - // shotData = ShotCompensation.compensateForMovement( - // turretPivotPose, - // drive.getState().Speeds, - // new Pose2d(targetPos, new Rotation2d()), - // NOMINAL_SHOT_TIME_S - // ); - if (DriverStation.isAutonomous()) { setTargetingHub(); } - - Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); } /** @@ -139,6 +156,7 @@ public void setTargetingShuttleRight() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); } else { @@ -151,6 +169,7 @@ public void setTargetingShuttleLeft() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, 0 + shuttlingYOffset); } else { diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index c830fe2..922d90b 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -29,8 +29,9 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); + public static final double TURRET_CENTER_OFFSET_DEG = -88.1; // subtracted from robot relative heading - public static final double TURRET_ROBOT_OFFSET_DEG = 51.8; // subtracted from robot relative heading to get turret relative heading + public static final double TURRET_ROBOT_OFFSET_DEG = 47.8; // subtracted from robot relative heading to get turret relative heading public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, -0.2); // distance from the center of the robot to the center of the turret, in meters diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 4b3dc67..f69b271 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -104,7 +104,7 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) // absolute robot-relative target, in motor rotations double targetRobotRelativeDeg = fieldRelativeAngleDegrees - currentRobotHeading; - this.processedTargetTurretDegreesFieldRelative = wrap180(targetRobotRelativeDeg + turretTrimDegrees); + this.processedTargetTurretDegreesFieldRelative = wrap180(targetRobotRelativeDeg); setTurretAngleRobotRelativeDegrees(targetRobotRelativeDeg); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index f19331b..403fcae 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Pose2d; public class Vision extends SpikeSystem { + private static final double AMBIGUITY_THRESHOLD = 0.2; private VisionIO visionIO; private final CommandSwerveDrivetrain drive; @@ -29,23 +30,31 @@ public void onPeriodic() { continue; } double avgDist = 0; + boolean usePose = true; // calculate the average distance to the targets // used to calculate the standard deviations of the vision measurement if (!pose.targetsUsed.isEmpty()) { double totalDist = 0; for (var target : pose.targetsUsed) { + if (target.poseAmbiguity > AMBIGUITY_THRESHOLD) { + usePose = false; + break; + } + totalDist += target.getBestCameraToTarget().getTranslation().getNorm(); } avgDist = totalDist / pose.targetsUsed.size(); } - drive.addVisionMeasurement( - pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) - ); + if (usePose) { + drive.addVisionMeasurement( + pose.estimatedPose.toPose2d(), + pose.timestampSeconds, + CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) + ); + } } } diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index f696f6e..9c55703 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -43,9 +43,9 @@ public static List getCameras() { new Camera( "right", new Transform3d( - Inches.of(-0.95), + Inches.of(-1.5), Inches.of(-13.75), - Inches.of(8.475), + Inches.of(8.625), new Rotation3d( Degrees.of(0), Degrees.of(-65), // negative pitch = tilted upward @@ -54,5 +54,21 @@ public static List getCameras() { ) ) ); + + registerCamera( + new Camera( + "left", + new Transform3d( + Inches.of(-2.5), + Inches.of(-13.75), + Inches.of(8.5625), + new Rotation3d( + Degrees.of(0), + Degrees.of(-65), // negative pitch = tilted upward + Degrees.of(90) + ) + ) + ) + ); } }