Swerve Snapping

README.md

@@ -19,4 +19,7 @@ CRESCENDO, presented by Haas, is the 2024 Season of the FIRST Robotics Competiti
 Students have 6 weeks to construct a robot to compete in the season. Once the 6 weeks are up, teams compete at regional or district-level 
 competitions (depending on region) to qualify for the FIRST Championship in Houston, Texas.
 
-Interested in learning more about CRESCENDO? Visit [FIRST's website](https://www.firstinspires.org/robotics/frc/game-and-season) for more details!
+Interested in learning more about CRESCENDO? Visit [FIRST's website](https://www.firstinspires.org/robotics/frc/game-and-season) for more details!
+
+## Controls
+![Controls](src/main/assets/controllerMap2024.png)

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
 }
 
 repositories {

src/main/java/frc/robot/Constants.java

@@ -20,14 +20,16 @@
 import frc.robot.RobotPreferences.prefDrivetrain;
 
 /*
- * | Unit Type | Preferred Unit to Use |
- * | ---------- | ------------ |
- * | Distance | Meters |
- * | Distance per Time | Meters per Second |
- * | Angle | Degrees |
- * | Angle per Time | Degrees per Second |
- * | Time | Seconds |
- * 
+ * @formatter:off
+ * | Unit Type         | Preferred Unit to Use |
+ * |-------------------|-----------------------|
+ * | Distance          | Meters                |
+ * | Distance per Time | Meters per Second     |
+ * | Angle             | Degrees               |
+ * | Angle per Time    | Degrees per Second    |
+ * | Time              | Seconds               |
+ * @formatter:on
+ *
  * If the unit does not fall under any of these types, 
  * add a JavaDoc for that variable specifying it's unit. 
  * Avoid specifying units in the variable name.
@@ -183,17 +185,36 @@ public static class constRobot {
     public static final double MAX_VOLTAGE = 12;
     public static final boolean SILENCE_JOYSTICK_WARNINGS = true;
 
+    // @formatter:off
     /**
-     * Updated by Alice to match Comp bot Feb. 15
+     * Updated by Alice to match Comp bot Feb. 19
      */
     public static final String[] PDH_DEVICES = {
-        "Swerve/FL Steer", "Swerve/FL Drive", // 00, 01
-        null, null, null, null, null, null,
-        "Swerve/FR Steer", "Swerve/FR Drive", // 08, 09
-        "Swerve/BR Drive", "Swerve/BR Steer", // 10, 11
-        null, null, null, null, null, "Swerve/BL Steer", // 17
-        "Swerve/BL Drive", "Ethernet Switch", // 18, 19
-        "Swerve CANCoders & Pigeon", "RoboRIO", "Radio Power Module", "Beelink" };
+        /*  0 */ "Swerve/FL Steer",
+        /*  1 */ "Swerve/FL Drive",
+        /*  2 */ "Shooter/Right",
+        /*  3 */ "Transfer/Feeder",
+        /*  4 */ "Shooter/Pitch",
+        /*  5 */ "Transfer/Transfer",
+        /*  6 */ "Shooter/Left",
+        /*  7 */ null,
+        /*  8 */ "Swerve/FR Steer",
+        /*  9 */ "Swerve/FR Drive",
+        /* 10 */ "Swerve/BR Drive",
+        /* 11 */ "Swerve/BR Steer",
+        /* 12 */ null,
+        /* 13 */ null,
+        /* 14 */ null,
+        /* 15 */ null,
+        /* 16 */ "Turret",
+        /* 17 */ "Swerve/BL Steer",
+        /* 18 */ "Swerve/BL Drive",
+        /* 19 */ "Ethernet Switch",
+        /* 20 */ "Swerve CANCoders & Pigeon",
+        /* 21 */ "RoboRIO",
+        /* 22 */ "Radio Power Module",
+        /* 23 */ "Beelink" };
+    // @formatter:on
   }
 
   public static class constShooter {

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import java.util.function.DoubleSupplier;
+
 import com.frcteam3255.joystick.SN_XboxController;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
@@ -16,6 +18,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.constControllers;
 import frc.robot.Constants.LockedLocation;
 import frc.robot.Constants.constLEDs;
@@ -74,14 +77,25 @@ public RobotContainer() {
     // The Left Y and X Axes are swapped because from behind the glass, the X Axis
     // is actually in front of you
     subDrivetrain
-        .setDefaultCommand(new Drive(subDrivetrain, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX,
+        .setDefaultCommand(new Drive(
+            subDrivetrain,
+            conDriver.axis_LeftY,
+            conDriver.axis_LeftX,
+            conDriver.axis_RightX,
+            conDriver.btn_LeftBumper,
+            conDriver.btn_Y,
+            conDriver.btn_B,
+            conDriver.btn_A,
+            conDriver.btn_X,
             isPracticeBot()));
 
     subTurret.setDefaultCommand(new LockTurret(subTurret, subDrivetrain));
     subPitch.setDefaultCommand(new LockPitch(subPitch, subDrivetrain));
     subVision.setDefaultCommand(new AddVisionMeasurement(subDrivetrain,
         subVision));
 
+    // View controls at:
+    // src\main\assets\controllerMap2024.png
     configureDriverBindings(conDriver);
     configureOperatorBindings(conOperator);
 
@@ -90,7 +104,6 @@ public RobotContainer() {
   }
 
   private void configureDriverBindings(SN_XboxController controller) {
-
     controller.btn_North.onTrue(Commands.runOnce(() -> subDrivetrain.resetYaw()));
     controller.btn_East.onTrue(Commands.runOnce(() -> subDrivetrain.resetYaw()));
     controller.btn_South.onTrue(Commands.runOnce(() -> subDrivetrain.resetYaw()));
@@ -101,8 +114,6 @@ private void configureDriverBindings(SN_XboxController controller) {
     controller.btn_RightBumper.whileTrue(Commands.run(() -> subDrivetrain.setDefenseMode(), subDrivetrain))
         .whileTrue(Commands.runOnce(() -> subLEDs.setLEDsToAnimation(constLEDs.DEFENSE_MODE_ANIMATION)))
         .whileFalse(Commands.runOnce(() -> subLEDs.clearAnimation()));
-    // Defaults to Field-Relative, is Robot-Relative while held
-
   }
 
   private void configureOperatorBindings(SN_XboxController controller) {

src/main/java/frc/robot/RobotPreferences.java

@@ -7,14 +7,16 @@
 import frc.robot.Constants.constDrivetrain;
 
 /*
- * | Unit Type | Preferred Unit to Use |
- * | ---------- | ------------ |
- * | Distance | Meters |
- * | Distance per Time | Meters per Second |
- * | Angle | Degrees |
- * | Angle per Time | Degrees per Second |
- * | Time | Seconds |
- * 
+ * @formatter:off
+ * | Unit Type         | Preferred Unit to Use |
+ * |-------------------|-----------------------|
+ * | Distance          | Meters                |
+ * | Distance per Time | Meters per Second     |
+ * | Angle             | Degrees               |
+ * | Angle per Time    | Degrees per Second    |
+ * | Time              | Seconds               |
+ * @formatter:on
+ *
  * If the unit does not fall under any of these types, 
  * add a JavaDoc for that variable specifying it's unit. 
  * Avoid specifying units in the variable name.
@@ -41,7 +43,7 @@ public static final class climberPref {
 
   public static final class prefDrivetrain {
     // This PID is implemented on each module, not the Drivetrain subsystem.
-    public static final SN_DoublePreference driveP = new SN_DoublePreference("driveP", 0.21);
+    public static final SN_DoublePreference driveP = new SN_DoublePreference("driveP", 0.05);
     public static final SN_DoublePreference driveI = new SN_DoublePreference("driveI", 0.0);
     public static final SN_DoublePreference driveD = new SN_DoublePreference("driveD", 0.0);
 
@@ -64,12 +66,22 @@ public static final class prefDrivetrain {
     public static final SN_DoublePreference autoSteerI = new SN_DoublePreference("autoSteerI", 0.0);
     public static final SN_DoublePreference autoSteerD = new SN_DoublePreference("autoSteerD", 0.0);
 
+    // Teleop Snapping to Rotation (Yaw)
+    public static final SN_DoublePreference yawSnapP = new SN_DoublePreference("yawSnapP", 2);
+    public static final SN_DoublePreference yawSnapI = new SN_DoublePreference("yawSnapI", 0);
+    public static final SN_DoublePreference yawSnapD = new SN_DoublePreference("yawSnapD", 0);
+
     /**
      * <b>Units:</b> Percentage from 0 to 1
      */
     public static final SN_DoublePreference minimumSteerSpeedPercent = new SN_DoublePreference(
         "minimumSteerSpeedPercent", 0.01);
 
+    /**
+     * Value to multiply with the translation velocity when slow mode is enabled
+     */
+    public static final SN_DoublePreference slowModeMultiplier = new SN_DoublePreference("slowModeMultiplier", .2);
+
     /**
      * <p>
      * Rotational speed while manually driving

src/main/java/frc/robot/commands/Drive.java

@@ -6,29 +6,48 @@
 
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.constDrivetrain;
 import frc.robot.RobotPreferences.prefDrivetrain;
 import frc.robot.subsystems.Drivetrain;
 
 public class Drive extends Command {
   Drivetrain subDrivetrain;
   DoubleSupplier xAxis, yAxis, rotationAxis;
-  boolean isOpenLoop;
+  boolean isPracticeBot, isOpenLoop;
+  Trigger slowMode, northTrigger, eastTrigger, southTrigger, westTrigger;
 
-  boolean isPracticeBot;
-  double driveSpeed;
+  double driveSpeed, xVelocity, yVelocity, rVelocity, slowMultiplier;
+  Translation2d translationVelocity;
+
+  public Drive(
+      Drivetrain subDrivetrain,
+      DoubleSupplier xAxis,
+      DoubleSupplier yAxis,
+      DoubleSupplier rotationAxis,
+      Trigger slowMode,
+      Trigger northTrigger,
+      Trigger eastTrigger,
+      Trigger southTrigger,
+      Trigger westTrigger,
+      boolean isPracticeBot) {
 
-  public Drive(Drivetrain subDrivetrain, DoubleSupplier xAxis, DoubleSupplier yAxis,
-      DoubleSupplier rotationAxis, boolean isPracticeBot) {
     this.subDrivetrain = subDrivetrain;
     this.xAxis = xAxis;
     this.yAxis = yAxis;
     this.rotationAxis = rotationAxis;
+    this.slowMode = slowMode;
     this.isPracticeBot = isPracticeBot;
 
+    this.northTrigger = northTrigger;
+    this.eastTrigger = eastTrigger;
+    this.southTrigger = southTrigger;
+    this.westTrigger = westTrigger;
+
     isOpenLoop = false;
 
     addRequirements(this.subDrivetrain);
@@ -41,12 +60,30 @@ public void initialize() {
 
   @Override
   public void execute() {
-    // Get Joystick inputs
-    double xVelocity = xAxis.getAsDouble() * driveSpeed;
-    double yVelocity = -yAxis.getAsDouble() * driveSpeed;
-    double rVelocity = -rotationAxis.getAsDouble() * Units.degreesToRadians(prefDrivetrain.turnSpeed.getValue());
+    // Get inputs
+    slowMultiplier = (slowMode.getAsBoolean()) ? prefDrivetrain.slowModeMultiplier.getValue() : 1;
+    xVelocity = xAxis.getAsDouble() * driveSpeed;
+    yVelocity = -yAxis.getAsDouble() * driveSpeed;
+    rVelocity = -rotationAxis.getAsDouble() * Units.degreesToRadians(prefDrivetrain.turnSpeed.getValue());
+
+    translationVelocity = new Translation2d(xVelocity, yVelocity).times(slowMultiplier);
+
+    // If the Driver is providing a rotation manually, don't snap
+    if (rVelocity != 0.0) {
+    } else {
+      // Otherwise, check if snapping is being requested
+      if (northTrigger.getAsBoolean()) {
+        rVelocity = subDrivetrain.getVelocityToSnap(Rotation2d.fromDegrees(180));
+      } else if (eastTrigger.getAsBoolean()) {
+        rVelocity = subDrivetrain.getVelocityToSnap(Rotation2d.fromDegrees(90));
+      } else if (southTrigger.getAsBoolean()) {
+        rVelocity = subDrivetrain.getVelocityToSnap(Rotation2d.fromDegrees(0));
+      } else if (westTrigger.getAsBoolean()) {
+        rVelocity = subDrivetrain.getVelocityToSnap(Rotation2d.fromDegrees(-90));
+      }
+    }
 
-    subDrivetrain.drive(new Translation2d(xVelocity, yVelocity), rVelocity, isOpenLoop);
+    subDrivetrain.drive(translationVelocity, rVelocity, isOpenLoop);
   }
 
   @Override

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

@@ -10,9 +10,12 @@
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
@@ -29,6 +32,7 @@
 public class Drivetrain extends SN_SuperSwerve implements Logged {
   private static TalonFXConfiguration driveConfiguration = new TalonFXConfiguration();
   private static TalonFXConfiguration steerConfiguration = new TalonFXConfiguration();
+  private static PIDController yawSnappingController;
 
   // Struct logging - Allows for logging data that SmartDashboard alone can't log,
   // but must be called on the variable's creation
@@ -93,7 +97,6 @@ public Drivetrain() {
         new ReplanningConfig(),
         constDrivetrain.AUTO_FLIP_WITH_ALLIANCE_COLOR,
         Robot.isSimulation());
-
   }
 
   @Override
@@ -110,6 +113,11 @@ public void configure() {
 
     SN_SwerveModule.driveConfiguration = driveConfiguration;
     SN_SwerveModule.steerConfiguration = steerConfiguration;
+
+    yawSnappingController = new PIDController(
+        prefDrivetrain.yawSnapP.getValue(),
+        prefDrivetrain.yawSnapI.getValue(),
+        prefDrivetrain.yawSnapD.getValue());
     super.configure();
   }
 
@@ -152,6 +160,20 @@ public void addVisionMeasurement(Pose2d estimatedPose, double timestamp) {
     swervePoseEstimator.addVisionMeasurement(estimatedPose, timestamp);
   }
 
+  /**
+   * @param desiredYaw The desired yaw to snap to
+   * @return The desired velocity needed to snap. <b>Units:</b> Radians per Second
+   */
+  public double getVelocityToSnap(Rotation2d desiredYaw) {
+    double yawSetpoint = yawSnappingController.calculate(getRotation().getRadians(), desiredYaw.getRadians());
+
+    // limit the PID output to our maximum rotational speed
+    yawSetpoint = MathUtil.clamp(yawSetpoint, -Units.degreesToRadians(prefDrivetrain.turnSpeed.getValue()),
+        Units.degreesToRadians(prefDrivetrain.turnSpeed.getValue()));
+
+    return yawSetpoint;
+  }
+
   @Override
   public void periodic() {
     super.periodic();