merge master

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.4368920788482864,
+        "y": 3.1622483921028106
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82409310273166,
+        "y": 7.565612949407717
+      },
+      "prevControl": {
+        "x": 1.8176568333482528,
+        "y": 6.336387277909188
+      },
+      "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,13 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 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;
@@ -14,17 +18,6 @@ import frc.robot.constants.OIConstants;
 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. 
- * 
- * 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?
- * 
- * Any changes we make should be able to be carried forward to part 3
- */
 public class RobotContainer {
     private Drivetrain drivetrain;
 
@@ -32,6 +25,8 @@ public class RobotContainer {
 
     private PoseSendable poseSendable;
 
+    private SendableChooser<Command> autoChooser;
+
     public RobotContainer() {
         drivetrain = new Drivetrain(new Pose2d());
 
@@ -39,6 +34,8 @@ public class RobotContainer {
 
         poseSendable = new PoseSendable();
 
+        autoChooser = AutoBuilder.buildAutoChooser();
+
         configureBindings();
         shuffleboardConfig();
     }
@@ -70,9 +67,19 @@ public class RobotContainer {
         tab.add("Drivetrain Pose", poseSendable)
             .withPosition(0, 0)
             .withSize(2, 3);
+
+        tab.add("Auto Selector", autoChooser)
+            .withPosition(2, 0)
+            .withSize(2, 1);
+
+        tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
+            .withPosition(2, 1)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kTextView);
     }
 
     public Command getAutonomousCommand() {
-            return null;
+        return drivetrain.zeroHeadingCommand().andThen(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,13 +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 edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
@@ -89,22 +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;
 
-        Shuffleboard.getTab("NavX")
-            .addDouble("YAW", m_gyro::getAngle)
-            .withPosition(0, 0)
-            .withSize(2, 1)
-            .withWidget(BuiltInWidgets.kTextView);
+        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
+        );
 
-        Shuffleboard.getTab("NavX")
-            .addDouble("Offset", m_gyro::getAngleAdjustment)
-            .withPosition(0, 1)
-            .withSize(2, 1)
-            .withWidget(BuiltInWidgets.kTextView);
     }
 
     @Override