after GSD

src/main/java/frc/robot/RobotContainer.java

@@ -266,6 +266,8 @@ public class RobotContainer {
         () -> MathUtil.clamp(operator.getRightTriggerAxis()-operator.getLeftTriggerAxis(), -0.75, 0.75)
         ));
 
+        driver.povDown().whileTrue(indexer.shootNote(() -> -1.0));
+
         operator.start().onTrue(shooter.zeroEncoder());
         
   }
@@ -276,6 +278,8 @@ public class RobotContainer {
       .withPosition(0, 0)
       .withSize(2, 1)
       .withWidget(BuiltInWidgets.kComboBoxChooser);
+    
+    
 
     //Always select the auto tab on startup
     Shuffleboard.selectTab(kAutoTabName);

src/main/java/frc/robot/constants/AutoConstants.java

@@ -24,7 +24,7 @@ public final class AutoConstants {
         new PIDConstants(kPXController), 
         new PIDConstants(kPThetaController), 
         kMaxSpeedMetersPerSecond, 
-        Units.inchesToMeters(Math.sqrt(Math.pow(28-17.5, 2)+ Math.pow(28-1.75, 2))), 
+        Units.inchesToMeters(Math.sqrt(Math.pow(14-17.5, 2)+ Math.pow(14-1.75, 2))), 
         new ReplanningConfig()
     );
 

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -63,6 +63,8 @@ public class Drivetrain extends SubsystemBase {
 
     private double m_prevTime;
 
+    private double gyroOffset;
+
     /** Creates a new DriveSubsystem. */
     public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) {
         m_frontLeft = new MAXSwerveModule(
@@ -132,6 +134,9 @@ public class Drivetrain extends SubsystemBase {
             }, 
             this
         );
+
+        gyroOffset = DrivetrainConstants.kRobotStartOffset;
+
     }
 
     @Override
@@ -156,6 +161,14 @@ public class Drivetrain extends SubsystemBase {
         }
     }
 
+    public double getRobotStartOffset() {
+        return gyroOffset;
+    }
+
+    public void setRobotStartOffset(double offset) {
+        gyroOffset = offset;
+    }
+
     public boolean isFieldRelativeControl() {
         return fieldRelativeControl;
     }
@@ -275,7 +288,7 @@ public class Drivetrain extends SubsystemBase {
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative.getAsBoolean()
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -413,8 +426,8 @@ public class Drivetrain extends SubsystemBase {
         return runOnce(this::zeroHeading);
     }
 
-    public void offsetZero(double angle){
-        m_gyro.setAngleAdjustment(angle);
+    public void offsetZero(Pose2d angle){
+        resetOdometry(angle);
     }
 
     /**
@@ -427,7 +440,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public double getGyroAngle(){
-        return m_gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0) + DrivetrainConstants.kRobotStartOffset;
+        return m_gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
     /**