use 254 cheesy poofs swerve setpoint generator

redshiftrobotics · Sep 14, 2024 · 681a861 · 681a861
1 parent 22308bc
commit 681a861
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -28,8 +28,12 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
+import frc.robot.RobotState;
 import frc.robot.utility.AllianceFlipUtil;
 import frc.robot.utility.LocalADStarAK;
+import frc.robot.utility.swerve254util.ModuleLimits;
+import frc.robot.utility.swerve254util.SwerveSetpoint;
+import frc.robot.utility.swerve254util.SwerveSetpointGenerator;
 import java.util.Arrays;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -53,6 +57,9 @@ public class Drive extends SubsystemBase {
   private Rotation2d rawGyroRotation = new Rotation2d();
   private SwerveModulePosition[] lastModulePositions;
 
+  private SwerveSetpoint currentSetpoint;
+  private final SwerveSetpointGenerator setpointGenerator;
+
   private Pose2d pose = new Pose2d();
 
   private SwerveDrivePoseEstimator poseEstimator;
@@ -91,16 +98,26 @@ public Drive(
 
     // --- Set up kinematics ---
 
-    kinematics =
-        new SwerveDriveKinematics(
-            modules().map(Module::getDistanceFromCenter).toArray(Translation2d[]::new));
+    Translation2d[] moduleTranslations =
+        modules().map(Module::getDistanceFromCenter).toArray(Translation2d[]::new);
+
+    kinematics = new SwerveDriveKinematics(moduleTranslations);
 
     // --- Set up odometry ---
 
     lastModulePositions = modules().map(Module::getPosition).toArray(SwerveModulePosition[]::new);
     poseEstimator =
         new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, pose);
 
+    // --- Swerve Setpoint Generator ---
+
+    currentSetpoint =
+        new SwerveSetpoint(
+            new ChassisSpeeds(),
+            modules().map(Module::getSpeeds).toArray(SwerveModuleState[]::new));
+
+    setpointGenerator = new SwerveSetpointGenerator(kinematics, moduleTranslations);
+
     // --- Start odometry threads ---
 
     // Start threads (does nothing if no signals have been created)
@@ -148,9 +165,7 @@ public Drive(
                 null,
                 state -> Logger.recordOutput("Drive/SysIdState", state.toString())),
             new SysIdRoutine.Mechanism(
-                (voltage) -> runCharacterization(voltage.in(Units.Volts)),
-                null,
-                this));
+                (voltage) -> runCharacterization(voltage.in(Units.Volts)), null, this));
   }
 
   // --- Robot Pose ---
@@ -324,11 +339,19 @@ public void setRobotSpeeds(ChassisSpeeds speeds, boolean fieldRelative) {
               speeds, AllianceFlipUtil.apply(getPose().getRotation()));
     }
 
-    speeds = ChassisSpeeds.discretize(speeds, Constants.LOOP_PERIOD_SECONDS);
+    ModuleLimits currentModuleLimits = RobotState.getInstance().getModuleLimits();
+    currentSetpoint =
+        setpointGenerator.generateSetpoint(
+            currentModuleLimits, currentSetpoint, speeds, Constants.LOOP_PERIOD_SECONDS);
+    SwerveDriveWheelStates cheesyWheelSpeeds =
+        new SwerveDriveWheelStates(currentSetpoint.moduleStates());
+    Logger.recordOutput("SwerveStates/254/CheesyPoofsDesiredWheelSpeeds", cheesyWheelSpeeds.states);
 
+    speeds = ChassisSpeeds.discretize(speeds, Constants.LOOP_PERIOD_SECONDS);
     SwerveDriveWheelStates wheelSpeeds = kinematics.toWheelSpeeds(speeds);
+    Logger.recordOutput("SwerveStates/254/DefaultDesiredWheelSpeeds", wheelSpeeds.states);
 
-    setWheelSpeeds(wheelSpeeds);
+    setWheelSpeeds(DriveConstants.USE_254_SWERVE_SETPOINT ? cheesyWheelSpeeds : wheelSpeeds);
   }
 
   // --- Wheel States ---

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -14,6 +14,8 @@
  */
 public class DriveConstants {
 
+  public static final boolean USE_254_SWERVE_SETPOINT = true;
+
   // --- Drive Config ---
 
   public record DriveConfig(