Changes from build session 3/3/26

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 <i>aggressively</i>.
         // 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

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<PhotonVisionConfig> configs = List.of(
-        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
         new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
     );
 }

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(),