fully implement swerve system

src/main/java/frc/robot/swerve/SwerveSubsystem.java

@@ -11,6 +11,9 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.config.RobotConfig;
+import frc.robot.fms.FmsSubsystem;
+import frc.robot.util.ControllerHelpers;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
 import java.util.List;
@@ -76,6 +79,12 @@ public class SwerveSubsystem extends StateMachine<SwerveState> {
   private ChassisSpeeds robotRelativeSpeeds;
   private ChassisSpeeds fieldRelativeSpeeds;
   private boolean slowEnoughToFeed;
+  private double goalSnapAngle = 0;
+
+  /** The latest requested teleop speeds. */
+  private ChassisSpeeds teleopSpeeds = new ChassisSpeeds();
+
+  private ChassisSpeeds autoSpeeds = new ChassisSpeeds();
 
   public ChassisSpeeds getRobotRelativeSpeeds() {
     return robotRelativeSpeeds;
@@ -97,11 +106,65 @@ public List<SwerveModulePosition> getModulePositions() {
     return modulePositions;
   }
 
+  public void setSnapToAngle(double angle) {
+    goalSnapAngle = angle;
+  }
+
   public SwerveSubsystem(CommandXboxController driveController) {
     super(SubsystemPriority.SWERVE, SwerveState.TELEOP);
     this.controller = driveController;
   }
 
+  public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
+    autoSpeeds = speeds;
+    sendSwerveRequest();
+  }
+
+  public void driveTeleop(double x, double y, double theta) {
+    double leftY =
+        -1.0
+            * ControllerHelpers.getExponent(
+                ControllerHelpers.getDeadbanded(controller.getLeftY(), leftYDeadband), 1.5);
+    double leftX =
+        ControllerHelpers.getExponent(
+            ControllerHelpers.getDeadbanded(controller.getLeftX(), leftXDeadband), 1.5);
+    double rightX =
+        ControllerHelpers.getExponent(
+            ControllerHelpers.getDeadbanded(controller.getRightX(), rightXDeadband), 2);
+
+    if (RobotConfig.get().swerve().invertRotation()) {
+      rightX *= -1.0;
+    }
+
+    if (RobotConfig.get().swerve().invertX()) {
+      leftX *= -1.0;
+    }
+
+    if (RobotConfig.get().swerve().invertY()) {
+      leftY *= -1.0;
+    }
+
+    if (FmsSubsystem.isRedAlliance()) {
+      leftX *= -1.0;
+      leftY *= -1.0;
+    }
+
+    teleopSpeeds =
+        new ChassisSpeeds(
+            -1.0 * leftY * MaxSpeed,
+            leftX * MaxSpeed,
+            rightX * TELEOP_MAX_ANGULAR_RATE.getRadians());
+
+    sendSwerveRequest();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    super.robotPeriodic();
+    // Once per loop send a swerve request to ensure data is fresh
+    sendSwerveRequest();
+  }
+
   @Override
   protected void collectInputs() {
     modulePositions = calculateModulePositions();
@@ -141,4 +204,37 @@ private boolean calculateMovingSlowEnoughForFloorShot(ChassisSpeeds speeds) {
 
     return linearSpeed < MAX_FLOOR_SPEED_SHOOTING;
   }
+
+  private void sendSwerveRequest() {
+    switch (getState()) {
+      case TELEOP ->
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(teleopSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+      case TELEOP_SNAPS ->
+          drivetrain.setControl(
+              driveToAngle
+                  .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                  .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+      case AUTO ->
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(autoSpeeds.vxMetersPerSecond)
+                  .withVelocityY(autoSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(autoSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.Velocity));
+      case AUTO_SNAPS ->
+          drivetrain.setControl(
+              driveToAngle
+                  .withVelocityX(autoSpeeds.vxMetersPerSecond)
+                  .withVelocityY(autoSpeeds.vyMetersPerSecond)
+                  .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
+                  .withDriveRequestType(DriveRequestType.Velocity));
+    }
+  }
 }