From 918876923f928724417c43c884c68467097bd39c Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Wed, 4 Mar 2026 10:05:46 -0500 Subject: [PATCH] Changes from build session 3/3/26 --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++-------- .../frc/robot/constants/PhotonConstants.java | 15 +++++++++++++-- .../java/frc/robot/subsystems/Drivetrain.java | 7 +++++++ 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8cf350..8d87f65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,7 +42,7 @@ import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { - //private PhotonVision vision; + private PhotonVision vision; private Drivetrain drivetrain; private Hood hood; private Shooter shooter; @@ -59,7 +59,7 @@ public class RobotContainer { private Timer shiftTimer; public RobotContainer() { - //vision = new PhotonVision(); + vision = new PhotonVision(); drivetrain = new Drivetrain(null); hood = new Hood(); shooter = new Shooter(); @@ -68,14 +68,14 @@ public class RobotContainer { spindexer = new Spindexer(); //climber = new Climber(); - /* + vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( "Vision/" + vp.cameraName() + "/Pose", vp.visualPose() ); - });*/ + }); driver = new CommandXboxController(OIConstants.kDriverControllerPort); secondary = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -108,7 +108,7 @@ public class RobotContainer { // This should just work, if it doesn't it's likely modules aren't assigned the right IDs // after the electronics rebuild. For testing normal operation nothing about the Drivetrain // class should need to change - drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true)); + //drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true)); drivetrain.setDefaultCommand( drivetrain.drive( driver::getLeftY, @@ -117,6 +117,8 @@ public class RobotContainer { () -> true ) ); + + driver.y().whileTrue(drivetrain.zeroHeading()); /* // This needs to be tested after a prolonged amount of driving around aggressively. // Do things like going over the bump repeatedly, spin around a bunch, etc. @@ -168,8 +170,7 @@ public class RobotContainer { // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the // constants file for the subsystem having the problem driver.rightBumper().whileTrue( - //intakeRoller.runIn().alongWith(spindexer.spinToShooter()) - spindexer.spinToShooter() + spindexer.spinToShooter() ); // While holding the left bumper of the driver controller, the intake rollers @@ -178,7 +179,8 @@ public class RobotContainer { // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the // constants file for the subsystem having the problem driver.leftBumper().whileTrue( - intakeRoller.runOut().alongWith(spindexer.spinToIntake()) + intakeRoller.runIn() + //intakeRoller.runOut().alongWith(spindexer.spinToIntake()) ); // While holding D-Pad up on the secondary controller, the shooter should spin diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java index 78239a6..ae6384e 100644 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -2,7 +2,9 @@ package frc.robot.constants; import java.util.List; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import frc.robot.utilities.PhotonVisionConfig; public class PhotonConstants { @@ -11,7 +13,16 @@ public class PhotonConstants { // TODO Need actual values for all of this public static final Transform3d kCamera1RobotToCam = new Transform3d(); - public static final Transform3d kCamera2RobotToCam = new Transform3d(); + public static final Transform3d kCamera2RobotToCam = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(24), + Units.degreesToRadians(-10) + ) + ); public static final double kCamera1HeightMeters = 0; public static final double kCamera1PitchRadians = 0; @@ -21,7 +32,7 @@ public class PhotonConstants { // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM public static final List configs = List.of( - new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), + //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians) ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 8727052..53b99f3 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -323,6 +323,13 @@ public class Drivetrain extends SubsystemBase { }); } + public Command zeroHeading() { + return run(() -> { + gyro.reset(); + estimator.resetRotation(new Rotation2d(0)); + }); + } + public void consumeVisualPose(VisualPose pose) { estimator.addVisionMeasurement( pose.visualPose(),