trying to get drive to work it's skewing a lot idk why

Author: PGgit08

README.md

@@ -7,5 +7,9 @@ A base project for future robots that has CTRE generated swerve drive code and P
 - Swerve drive code using CTRE's swerve code generator.
 - Device logging with SignalLogger and data logging with Epilogue and DogLog.
 
+# Todo
+- Add the working stuff from other SwerveBase (fault logger, photon vision code).
+- Add WPILib testing?
+
 ## 2025 Beta Known Issues
 - To run sim, do `./gradlew simulateJava` instead of using the WPILib extension.

simgui-ds.json

@@ -21,12 +21,15 @@
           "incKey": 83
         },
         {
-          "decKey": 69,
-          "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 263,
+          "incKey": 262
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -71,10 +74,7 @@
     },
     {
       "axisConfig": [
-        {
-          "decKey": 263,
-          "incKey": 262
-        },
+        {},
         {
           "decKey": 265,
           "incKey": 264

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

@@ -29,10 +29,10 @@ public static class Ports {
   public static class SwerveConstants {
     public static final Frequency odometryFrequency = Hertz.of(100);
 
-    public static final LinearVelocity translationalDeadband = MetersPerSecond.of(0.1);
-    public static final AngularVelocity rotationalDeadband = RadiansPerSecond.of(Math.PI * 0.1);
+    public static final LinearVelocity translationalDeadband = MetersPerSecond.of(0);
+    public static final AngularVelocity rotationalDeadband = RadiansPerSecond.of(0);
 
-    public static final LinearVelocity maxTranslationSpeed = TunerConstants.kSpeedAt12Volts;
+    public static final LinearVelocity maxTranslationalSpeed = TunerConstants.kSpeedAt12Volts;
     public static final AngularVelocity maxAngularSpeed = RadiansPerSecond.of(Math.PI);
   }
 }

src/main/java/frc/robot/Robot.java

@@ -11,7 +11,7 @@
 import dev.doglog.DogLogOptions;
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
-import edu.wpi.first.epilogue.NotLogged;
+import edu.wpi.first.epilogue.Logged.Strategy;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -30,7 +30,7 @@
  * the TimedRobot documentation. If you change the name of this class or the package after creating
  * this project, you must also update the Main.java file in the project.
  */
-@Logged
+@Logged(strategy = Strategy.OPT_IN)
 public class Robot extends TimedRobot {
   // controllers
   private final CommandXboxController _driverController =
@@ -40,7 +40,7 @@ public class Robot extends TimedRobot {
   @Logged(name = "Swerve")
   private CommandSwerveDrivetrain _swerve = TunerConstants.createDrivetrain();
 
-  @NotLogged private Command _autonomousCommand = Autos.none();
+  private Command _autonomousCommand = Autos.none();
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -58,10 +58,10 @@ public Robot() {
         _swerve.drive(
             InputStream.of(_driverController::getLeftY)
                 .negate()
-                .scale(SwerveConstants.maxTranslationSpeed.in(MetersPerSecond)),
+                .scale(SwerveConstants.maxTranslationalSpeed.in(MetersPerSecond)),
             InputStream.of(_driverController::getLeftX)
                 .negate()
-                .scale(SwerveConstants.maxTranslationSpeed.in(MetersPerSecond)),
+                .scale(SwerveConstants.maxTranslationalSpeed.in(MetersPerSecond)),
             InputStream.of(_driverController::getRightX)
                 .negate()
                 .scale(SwerveConstants.maxAngularSpeed.in(RadiansPerSecond))));

src/main/java/frc/robot/generated/TunerConstants.java

@@ -1,7 +1,6 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot.generated;
 
 import static edu.wpi.first.units.Units.*;

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

@@ -15,7 +15,11 @@
 import dev.doglog.DogLog;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.Logged.Strategy;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
@@ -83,6 +87,7 @@ public CommandSwerveDrivetrain(
           totalDaqs = totalDaqs == 0 ? 1 : totalDaqs;
 
           DogLog.log("Swerve/Odometry Success %", state.SuccessfulDaqs / totalDaqs * 100);
+          DogLog.log("Swerve/Odometry Period", state.OdometryPeriod);
         });
 
     if (RobotBase.isSimulation()) startSimThread();
@@ -129,6 +134,24 @@ public void drive(double velX, double velY, double velOmega) {
     _driverChassisSpeeds.vyMetersPerSecond = velY;
     _driverChassisSpeeds.omegaRadiansPerSecond = velOmega;
 
+    // go through a couple of steps to ensure that input speeds are actually achievable
+    ChassisSpeeds tempSpeeds = _driverChassisSpeeds;
+    SwerveModuleState[] tempStates;
+
+    if (_isFieldOriented)
+      tempSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(tempSpeeds, getHeading());
+
+    tempStates = getKinematics().toSwerveModuleStates(tempSpeeds);
+    SwerveDriveKinematics.desaturateWheelSpeeds(tempStates, SwerveConstants.maxTranslationalSpeed);
+    tempSpeeds = getKinematics().toChassisSpeeds(tempStates);
+
+    if (_isFieldOriented)
+      tempSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(tempSpeeds, getHeading());
+
+    velX = tempSpeeds.vxMetersPerSecond;
+    velY = tempSpeeds.vyMetersPerSecond;
+    velOmega = tempSpeeds.omegaRadiansPerSecond;
+
     if (_isFieldOriented) {
       setControl(
           _fieldCentricRequest
@@ -170,6 +193,21 @@ public void drive(
             .withWheelForceFeedforwardsY(wheelForceFeedforwardsY));
   }
 
+  /** Wrapper for getting estimated pose. */
+  public Pose2d getPose() {
+    return getState().Pose;
+  }
+
+  /** Wrapper for getting estimated heading. */
+  public Rotation2d getHeading() {
+    return getPose().getRotation();
+  }
+
+  /** Wrapper for getting current robot-relative chassis speeds. */
+  public ChassisSpeeds getChassisSpeeds() {
+    return getState().Speeds;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run