added drivetrain tunable numbers

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

@@ -23,6 +23,7 @@
 import frc.robot.commands.coral.IntakeCoral;
 import frc.robot.commands.coral.ShootCoral;
 import frc.robot.commands.drivetrain.Drive;
+import frc.robot.commands.drivetrain.DriveVoltage;
 import frc.robot.commands.drivetrain.RobotSlide;
 import frc.robot.commands.elevator.*;
 import frc.robot.commands.hihi.*;
@@ -302,7 +303,14 @@ public SwerveDrivetrain getDrivetrain() {
   }
 
   public void putShuffleboardCommands() {
-
+    if (Constants.SWERVE_DEBUG) {
+      SmartShuffleboard.put("Drive", "VoltsToRunWith", 0);
+      SmartShuffleboard.putCommand(
+          "Drive",
+          "RunWithVolts",
+          new DriveVoltage(
+              drivetrain, () -> SmartShuffleboard.getDouble("Drive", "VoltsToRunWith", 0), 5));
+    }
     if (Constants.CORAL_DEBUG) {
       SmartShuffleboard.putCommand(
           "Commands", "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED));

src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java

@@ -0,0 +1,41 @@
+package frc.robot.commands.drivetrain;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.swervev3.SwerveDrivetrain;
+import frc.robot.utils.logging.commands.LoggableCommand;
+import java.util.function.DoubleSupplier;
+
+/**
+ * This class can be used to calculate kS value for {@link
+ * edu.wpi.first.math.controller.SimpleMotorFeedforward SimpleMotorFeedforward}.<br>
+ * Keep applying volts until the motors are able to move the robot at all. <br>
+ * kS represents the volts need to overcome static friction
+ */
+public class DriveVoltage extends LoggableCommand {
+  private final SwerveDrivetrain drivetrain;
+  private final DoubleSupplier volts;
+  private final double time;
+  private final Timer timer;
+
+  public DriveVoltage(SwerveDrivetrain drivetrain, DoubleSupplier volts, double time) {
+    this.drivetrain = drivetrain;
+    this.volts = volts;
+    this.time = time;
+    this.timer = new Timer();
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+  }
+
+  @Override
+  public void execute() {
+    drivetrain.applyVolts(volts.getAsDouble());
+  }
+
+  @Override
+  public boolean isFinished() {
+    return timer.hasElapsed(time);
+  }
+}

src/main/java/frc/robot/commands/drivetrain/RobotSlide.java

@@ -22,7 +22,7 @@ public RobotSlide(SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier) {
   @Override
   public void execute() {
     double horizVal = -horizSupplier.getAsDouble();
-    double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY;
+    double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 2;
     ChassisSpeeds speeds = drivetrain.createChassisSpeeds(0, str, 0, DriveMode.ROBOT_CENTRIC);
     drivetrain.drive(speeds);
   }

src/main/java/frc/robot/constants/GameConstants.java

@@ -98,9 +98,9 @@ public enum Mode {
   }
 
   // Limits
-  public static final int DRIVE_SMART_LIMIT = 38; // TODO: change later
-  public static final int DRIVE_SECONDARY_LIMIT = 48; // TODO: change later
-  public static final double DRIVE_RAMP_RATE_LIMIT = 0.1; // TODO: change later
+  public static final int DRIVE_SMART_LIMIT = 40; // TODO: change later
+  public static final int DRIVE_SECONDARY_LIMIT = 0; // TODO: change later
+  public static final double DRIVE_RAMP_RATE_LIMIT = 0; // TODO: change later
   public static final int NEO_CURRENT_LIMIT = 20;
   public static final int HIHI_CURRENT_LIMIT = 10;
 
@@ -126,11 +126,13 @@ public enum Mode {
   public static final boolean ELEVATOR_USE_MAX_MOTION = true;
 
   // Drive PID
-  public static final double DRIVE_PID_P = 1; // TODO: change later
+  public static final double DRIVE_PID_P = 2; // TODO: change later
   public static final double DRIVE_PID_I = 0; // TODO: change later
   public static final double DRIVE_PID_D = 0; // TODO: change later
-  public static final double DRIVE_PID_FF_S = 1; // TODO: change later
-  public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later
+  public static final double DRIVE_PID_FF_S = 0.19;
+  public static final double DRIVE_PID_FF_V = 3.3;
+  public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later
+  public static final double DRIVE_PID_ALLOWED_ERROR = 0;
 
   // Steer PID
   public static final double STEER_PID_P = 0.3; // TODO: change later

src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java

@@ -17,8 +17,8 @@
 import frc.robot.subsystems.swervev3.io.SwerveModule;
 import frc.robot.utils.DriveMode;
 import frc.robot.utils.logging.LoggableIO;
+import frc.robot.utils.logging.LoggedTunableNumber;
 import frc.robot.utils.logging.subsystem.LoggableSystem;
-import frc.robot.utils.shuffleboard.SmartShuffleboard;
 import org.littletonrobotics.junction.Logger;
 
 public class SwerveDrivetrain extends SubsystemBase {
@@ -42,6 +42,20 @@ public class SwerveDrivetrain extends SubsystemBase {
   private final PoseEstimator poseEstimator;
   private boolean facingTarget = false;
 
+  private LoggedTunableNumber closedLoopTunable;
+  private LoggedTunableNumber smartLimitTunable;
+  private LoggedTunableNumber secondaryLimitTunable;
+  private LoggedTunableNumber drivePTunable;
+  private LoggedTunableNumber driveITunable;
+  private LoggedTunableNumber driveDTunable;
+  private LoggedTunableNumber driveKvTunable;
+  private LoggedTunableNumber driveKsTunable;
+  private LoggedTunableNumber steerPTunable;
+  private LoggedTunableNumber steerITunable;
+  private LoggedTunableNumber steerDTunable;
+  private LoggedTunableNumber steerMaxAccelerationTunable;
+  private LoggedTunableNumber steerMaxVelocityTunable;
+
   public SwerveDrivetrain(
       SwerveModule frontLeftModule,
       SwerveModule frontRightModule,
@@ -57,6 +71,30 @@ public SwerveDrivetrain(
     this.poseEstimator =
         new PoseEstimator(
             frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro());
+    if (Constants.TUNING_MODE) {
+      closedLoopTunable =
+          new LoggedTunableNumber(
+              "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT);
+      smartLimitTunable =
+          new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT);
+      secondaryLimitTunable =
+          new LoggedTunableNumber(
+              "Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT);
+      drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P);
+      driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I);
+      driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D);
+
+      driveKvTunable = new LoggedTunableNumber("Swerve/drive/Kv", Constants.DRIVE_PID_FF_V);
+      driveKsTunable = new LoggedTunableNumber("Swerve/drive/Ks", Constants.DRIVE_PID_FF_S);
+
+      steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P);
+      steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I);
+      steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D);
+      steerMaxAccelerationTunable =
+          new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150);
+      steerMaxVelocityTunable =
+          new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150);
+    }
   }
 
   @Override
@@ -81,11 +119,84 @@ public void periodic() {
         frontRight.getLatestState(),
         backLeft.getLatestState(),
         backRight.getLatestState());
-    if (Constants.SWERVE_DEBUG) {
-      SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition());
-      SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition());
-      SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition());
-      SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition());
+    if (Constants.TUNING_MODE) {
+      LoggedTunableNumber.ifChanged(
+          hashCode(),
+          () -> {
+            updateConfig(
+                closedLoopTunable.get(),
+                secondaryLimitTunable.get(),
+                (int) smartLimitTunable.get());
+          },
+          closedLoopTunable,
+          secondaryLimitTunable,
+          smartLimitTunable);
+      LoggedTunableNumber.ifChanged(
+          hashCode(),
+          () -> {
+            frontLeft.setDrivePID(
+                drivePTunable.get(),
+                driveITunable.get(),
+                driveDTunable.get(),
+                driveKvTunable.get(),
+                driveKsTunable.get());
+            frontRight.setDrivePID(
+                drivePTunable.get(),
+                driveITunable.get(),
+                driveDTunable.get(),
+                driveKvTunable.get(),
+                driveKsTunable.get());
+            backLeft.setDrivePID(
+                drivePTunable.get(),
+                driveITunable.get(),
+                driveDTunable.get(),
+                driveKvTunable.get(),
+                driveKsTunable.get());
+            backRight.setDrivePID(
+                drivePTunable.get(),
+                driveITunable.get(),
+                driveDTunable.get(),
+                driveKvTunable.get(),
+                driveKsTunable.get());
+          },
+          drivePTunable,
+          driveITunable,
+          driveDTunable,
+          driveKvTunable,
+          driveKsTunable);
+      LoggedTunableNumber.ifChanged(
+          hashCode(),
+          () -> {
+            frontLeft.setSteerPID(
+                steerPTunable.get(),
+                steerITunable.get(),
+                steerDTunable.get(),
+                steerMaxAccelerationTunable.get(),
+                steerMaxVelocityTunable.get());
+            frontRight.setSteerPID(
+                steerPTunable.get(),
+                steerITunable.get(),
+                steerDTunable.get(),
+                steerMaxAccelerationTunable.get(),
+                steerMaxVelocityTunable.get());
+            backLeft.setSteerPID(
+                steerPTunable.get(),
+                steerITunable.get(),
+                steerDTunable.get(),
+                steerMaxAccelerationTunable.get(),
+                steerMaxVelocityTunable.get());
+            backRight.setSteerPID(
+                steerPTunable.get(),
+                steerITunable.get(),
+                steerDTunable.get(),
+                steerMaxAccelerationTunable.get(),
+                steerMaxVelocityTunable.get());
+          },
+          steerPTunable,
+          steerITunable,
+          steerDTunable,
+          steerMaxAccelerationTunable,
+          steerMaxVelocityTunable);
     }
   }
 
@@ -204,4 +315,24 @@ public void setFacingTarget(boolean facingTarget) {
   public boolean isFacingTarget() {
     return facingTarget;
   }
+
+  /**
+   * @param closedLoopRampRate
+   * @param secondaryCurrentLimit
+   * @param smartCurrentLimit
+   */
+  public void updateConfig(
+      double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {
+    frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit);
+    frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit);
+    backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit);
+    backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit);
+  }
+
+  public void applyVolts(double volts) {
+    frontLeft.applyVolts(volts);
+    frontRight.applyVolts(volts);
+    backLeft.applyVolts(volts);
+    backRight.applyVolts(volts);
+  }
 }

src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java

@@ -155,4 +155,26 @@ private double getSteerPosition() {
     return AngleUtils.normalizeSwerveAngle(
         steerSystem.getInputs().getEncoderPosition() - steerOffset);
   }
+
+  public void setDrivePID(double p, double i, double d, double kV, double kS) {
+    drivePIDController.setPID(p, i, d);
+    driveFeedforward.setKv(kV);
+    driveFeedforward.setKs(kS);
+  }
+
+  public void setSteerPID(
+      double p, double i, double d, double maxAcceleration, double maxVelocity) {
+    turningPIDController.setPID(p, i, d);
+    turningPIDController.setConstraints(
+        new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity));
+  }
+
+  public void updateConfig(
+      double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {
+    driveSystem.getIO().updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit);
+  }
+
+  public void applyVolts(double volts) {
+    driveSystem.getIO().setDriveVoltage(volts);
+  }
 }

src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java

@@ -12,4 +12,8 @@ public void resetEncoder() {}
 
   @Override
   public void updateInputs(MotorInputs inputs) {}
+
+  @Override
+  public void updateConfig(
+      double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {}
 }

src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java

@@ -2,7 +2,6 @@
 
 import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import frc.robot.constants.Constants;
@@ -13,14 +12,12 @@
 public class SparkMaxDriveMotorIO implements SwerveDriveMotorIO {
 
   private final SparkMax driveMotor;
-  private final SparkBaseConfig driveConfig;
   private final SparkMaxInputProvider inputProvider;
 
   public SparkMaxDriveMotorIO(
       int driveMotorIO, KinematicsConversionConfig conversionConfig, boolean driveInverted) {
     driveMotor = new SparkMax(driveMotorIO, SparkMax.MotorType.kBrushless);
     inputProvider = new SparkMaxInputProvider(driveMotor);
-    driveConfig = new SparkMaxConfig();
     setMotorConfig(driveInverted);
     setConversionFactors(conversionConfig);
   }
@@ -31,6 +28,7 @@ public void setDriveVoltage(double volts) {
   }
 
   private void setConversionFactors(KinematicsConversionConfig conversionConfig) {
+    SparkMaxConfig driveConfig = new SparkMaxConfig();
     double driveVelConvFactor =
         (2 * conversionConfig.getWheelRadius() * Math.PI)
             / (conversionConfig.getProfile().getDriveGearRatio() * 60);
@@ -43,12 +41,12 @@ private void setConversionFactors(KinematicsConversionConfig conversionConfig) {
         .velocityConversionFactor(driveVelConvFactor);
     driveMotor.configure(
         driveConfig,
-        SparkBase.ResetMode.kResetSafeParameters,
+        SparkBase.ResetMode.kNoResetSafeParameters,
         SparkBase.PersistMode.kPersistParameters);
   }
 
   private void setMotorConfig(boolean driveInverted) {
-    // driveMotor.restoreFactoryDefaults(); //TODO: idk what to do
+    SparkMaxConfig driveConfig = new SparkMaxConfig();
     driveConfig
         .inverted(driveInverted)
         .idleMode(IdleMode.kBrake)
@@ -58,7 +56,7 @@ private void setMotorConfig(boolean driveInverted) {
             Constants.DRIVE_SMART_LIMIT); // TODO: change current limiting because its different
     driveMotor.configure(
         driveConfig,
-        SparkBase.ResetMode.kResetSafeParameters,
+        SparkBase.ResetMode.kNoResetSafeParameters,
         SparkBase.PersistMode.kPersistParameters);
   }
 
@@ -71,4 +69,18 @@ public void resetEncoder() {
   public void updateInputs(MotorInputs inputs) {
     inputs.process(inputProvider);
   }
+
+  @Override
+  public void updateConfig(
+      double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {
+    SparkMaxConfig driveConfig = new SparkMaxConfig();
+    driveConfig
+        .closedLoopRampRate(closedLoopRampRate)
+        .secondaryCurrentLimit(secondaryCurrentLimit)
+        .smartCurrentLimit(smartCurrentLimit);
+    driveMotor.configure(
+        driveConfig,
+        SparkBase.ResetMode.kNoResetSafeParameters,
+        SparkBase.PersistMode.kNoPersistParameters);
+  }
 }

src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java

@@ -7,4 +7,6 @@ public interface SwerveDriveMotorIO extends LoggableIO<MotorInputs> {
   void setDriveVoltage(double volts);
 
   void resetEncoder();
+
+  void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit);
 }