Merge remote-tracking branch 'origin/fix/swerve' into fix/swerve

.gitignore

@@ -160,6 +160,8 @@ gradle-app.setting
 .settings/
 bin/
 
+.factorypath
+
 # IntelliJ
 *.iml
 *.ipr

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

@@ -15,6 +15,8 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+  public static double PERIODIC_LOOP_SEC = 0.02;
+
   public static RobotType ROBOT_TYPE = RobotType.COMP;
 
   public static Mode getRobotMode() {

src/main/java/frc/robot/subsystems/swerve/Drive.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Util;
+import frc.robot.Constants;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -35,7 +36,7 @@ public enum DriveModes {
   private Rotation2d arbitraryYaw = new Rotation2d();
 
   @AutoLogOutput(key = "Swerve/YawOffset")
-  private Rotation2d gyroYawOffset = new Rotation2d(0);
+  private Rotation2d gyroYawOffset = new Rotation2d();
 
   private ChassisSpeeds teleopTargetSpeeds = new ChassisSpeeds();
   private ChassisSpeeds targetSpeeds = new ChassisSpeeds();
@@ -79,6 +80,10 @@ public void periodic() {
     }
 
     // run modules
+
+    /* use kinematics to get desired module states */
+    ChassisSpeeds discretizedSpeeds =
+        ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC);
     SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(targetSpeeds);
     SwerveDriveKinematics.desaturateWheelSpeeds(
         moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity());

src/main/java/frc/robot/subsystems/swerve/DriveConstants.java

@@ -87,6 +87,7 @@ public class DriveConstants {
       };
 
   public static final ModuleConstants MODULE_CONSTANTS =
+      // FIXME tune
       switch (getRobotType()) {
         case COMP, SIM -> new ModuleConstants(
             0.17, // steerkS