More code and some paths to go with it

src/main/deploy/pathplanner/autos/From Subwoofer Far Edge to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7046435911053639,
+      "y": 4.3751818412727745
+    },
+    "rotation": 120.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Far Edge to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/From Subwoofer Front to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.29,
+      "y": 5.58
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Front to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7046435911053639,
+        "y": 4.3751818412727745
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4304615881959033,
+        "y": 3.1531161244140673
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82409310273166,
+        "y": 7.565612949407717
+      },
+      "prevControl": {
+        "x": 1.7961068649410026,
+        "y": 6.511464659292955
+      },
+      "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": 89.21368742867472,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 122.73522627210761,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/From Subwoofer Front to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2945574370576298,
+        "y": 5.580559282764361
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2786077412442953,
+        "y": 5.580559282764361
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8232420006879377,
+        "y": 7.533755031731888
+      },
+      "prevControl": {
+        "x": 1.8232420006879377,
+        "y": 6.694612522270728
+      },
+      "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": 90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -4,9 +4,12 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -15,15 +18,16 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.utilities.PoseSendable;
 
 /*
- * The objective for this part of testing is to see if we can get pose estimation values to work for the gyro angle
- * required for Field Oriented AND can we find a way to reset the robot's pose to properly offset using the pose estimator. 
+ * The objective for this part of testing is to bring autonomous back into the mix, by this point, we should be able 
+ * to manually set a pose to the estimator from part 2 and have field oriented be accurate. We need to be able to set the robot
+ * up for a simple auto, run a practice match, have the robot do the simple auto, and when we hit teleoperated have the robot
+ * behavior normally, field oriented style. 
  * 
  * We should try to solve in this branch:
- * - Does Field Oriented Drive work in a perfect world scenario when using the pose estimator as the rotation source?
- * - Can we reset our pose in such a way using the pose estimator that we get consistent behavior of field oriented drive
- *      regardless of what orientation the robot is booted up in?
+ * - Does Field Oriented Drive work post autonomous?
  * 
- * Any changes we make should be able to be carried forward to part 3
+ * If this works, then this is how the code in the master branch needs to be structured in order for everything to work. We'll need
+ * to test more from there to make sure the other autos still work as intended. 
  */
 public class RobotContainer {
     private Drivetrain drivetrain;
@@ -32,6 +36,8 @@ public class RobotContainer {
 
     private PoseSendable poseSendable;
 
+    private SendableChooser<Command> autoChooser;
+
     public RobotContainer() {
         drivetrain = new Drivetrain(new Pose2d());
 
@@ -39,6 +45,8 @@ public class RobotContainer {
 
         poseSendable = new PoseSendable();
 
+        autoChooser = AutoBuilder.buildAutoChooser();
+
         configureBindings();
         shuffleboardConfig();
     }
@@ -73,6 +81,6 @@ public class RobotContainer {
     }
 
     public Command getAutonomousCommand() {
-            return null;
+        return autoChooser.getSelected();
     }
 }

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

@@ -0,0 +1,34 @@
+package frc.robot.constants;
+
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+
+public final class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+    public static final double kPXController = 1.05;
+    public static final double kPYController = 1.05;
+    public static final double kPThetaController = 0.95; // needs to be separate from heading control
+
+    public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
+    public static final double kDHeadingController = 0.0025;
+
+    public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig(
+        new PIDConstants(kPXController), 
+        new PIDConstants(kPThetaController), 
+        kMaxSpeedMetersPerSecond, 
+        Units.inchesToMeters(Math.sqrt(Math.pow(14-1.75, 2) + Math.pow(14-1.75, 2))), 
+        new ReplanningConfig()
+    );
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+}

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

@@ -11,11 +11,15 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
@@ -87,10 +91,28 @@ public class Drivetrain extends SubsystemBase {
             },
             initialPose
         );
+
         m_currentRotation = 0.0;
         m_currentTranslationDir = 0.0;
         m_currentTranslationMag = 0.0;
         m_prevTime =  WPIUtilJNI.now() * 1e-6;
+
+        AutoBuilder.configureHolonomic(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPFConfig, 
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
+
     }
 
     @Override