after GSD

src/main/deploy/pathplanner/autos/Just Backup.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9847983228729987,
+      "y": 1.9646179463299098
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Just Backup"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto

@@ -2,15 +2,40 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2,
+      "x": 0.4936438362905214,
-      "y": 2
+      "y": 6.981410202136645
     },
-    "rotation": 0
+    "rotation": 180.0
   },
   "command": {
     "type": "sequential",
     "data": {
-      "commands": []
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Subwoofer"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "L Sub to Center"
+          }
+        }
+      ]
     }
   },
   "folder": null,

src/main/deploy/pathplanner/paths/Just Backup.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9847983228729987,
+        "y": 1.9646179463299098
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9847983228729973,
+        "y": 1.9646179463299098
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6453682536994707,
+        "y": 1.9646179463299098
+      },
+      "prevControl": {
+        "x": 1.6453682536994707,
+        "y": 1.9646179463299098
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 1.3971810272964678,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/L Sub to Center.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2888463383764373,
+        "y": 6.642279723305886
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2888463383764366,
+        "y": 6.642279723305886
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.159911049166827,
+        "y": 6.864468657712245
+      },
+      "prevControl": {
+        "x": 2.159911049166827,
+        "y": 6.864468657712245
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 21.801409486351982,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Subwoofer.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.4936438362905214,
+        "y": 6.981410202136645
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.3941696612245824,
+        "y": 6.981410202136645
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.148516485067158,
+        "y": 6.560420642208806
+      },
+      "prevControl": {
+        "x": 1.5812001994374358,
+        "y": 6.864468657712245
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -146.30993247402017,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -178.6677801461302,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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());
         
   }
@@ -277,6 +279,8 @@ public class RobotContainer {
       .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){
+    public void offsetZero(Pose2d angle){
-        m_gyro.setAngleAdjustment(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);
     }
 
     /**