characterization improvements (#46)

* update the "Swerve Drive Characterization" auto mode to use a larger range of voltages and to use the linear least-squares method (Moore-Penrose pseudoinverse) to calculate kS, kV, and kA (fixes update characterization to include kA and span a wider range of voltages #43)
* add the "Swerve Rotate Characterization" to characterize the swerve module's rotation mechanism
* add characterization constants for Nova

credit: @cyandev

.vscode/settings.json

@@ -56,8 +56,10 @@
     "Nivore",
     "odometry",
     "openloop",
+    "Penrose",
     "photonvision",
     "PIDF",
+    "pseudoinverse",
     "rapidreact",
     "REVPHJNI",
     "rezero",
@@ -72,6 +74,7 @@
     "tunables",
     "Vandermonde",
     "VBAT",
+    "wpilib",
     "WPILOG",
     "Xstance",
     "YUYV"

src/main/java/frc/lib/team254/drivers/TalonFXFactory.java

@@ -46,7 +46,7 @@ public static class Configuration {
         SensorInitializationStrategy.BootToZero;
 
     public int CONTROL_FRAME_PERIOD_MS = 20; // 10
-    public int MOTION_CONTROL_FRAME_PERIOD_MS = 100;
+    public int MOTION_CONTROL_FRAME_PERIOD_MS = 3;
 
     public int GENERAL_STATUS_FRAME_RATE_MS = 10;
     public int FEEDBACK_STATUS_FRAME_RATE_MS = 49;

src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java

@@ -34,7 +34,7 @@ public static class Configuration {
     public boolean SENSOR_PHASE = false;
 
     public int CONTROL_FRAME_PERIOD_MS = 5;
-    public int MOTION_CONTROL_FRAME_PERIOD_MS = 100;
+    public int MOTION_CONTROL_FRAME_PERIOD_MS = 3;
     public int GENERAL_STATUS_FRAME_RATE_MS = 5;
     public int FEEDBACK_STATUS_FRAME_RATE_MS = 100;
     public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000;

src/main/java/frc/lib/team3061/RobotConfig.java

@@ -65,6 +65,34 @@ public double getSwerveAngleKD() {
     return 0.0;
   }
 
+
+  /*
+   * Returns the voltage needed to overcome the swerve module's static friction. Defaults to 0.
+   * 
+   * @return the voltage needed to overcome the swerve module's static friction
+   */
+  public double getSwerveAngleKS() {
+    return 0.0;
+  }
+
+  /**
+   * Returns the voltage needed to hold (or "cruise") at a given constant velocity. Defaults to 0.
+   *
+   * @return the voltage needed to hold (or "cruise") at a given constant velocity
+   */
+  public double getSwerveAngleKV() {
+    return 0.0;
+  }
+
+  /**
+   * Returns the voltage needed to induce a given acceleration in the motor shaft. Defaults to 0.
+   *
+   * @return the voltage needed to induce a given acceleration in the motor shaft
+   */
+  public double getSwerveAngleKA() {
+    return 0.0;
+  }
+
   /**
    * Returns the proportional constant for the PID controller for the drive motor on the swerve
    * module. Defaults to 0.

src/main/java/frc/lib/team3061/swerve/SwerveModule.java

@@ -4,6 +4,8 @@
 
 package frc.lib.team3061.swerve;
 
+import static frc.robot.Constants.*;
+
 import com.ctre.phoenix6.StatusSignal;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -21,6 +23,7 @@ public class SwerveModule {
   private int moduleNumber;
   private double lastAngle;
   private double maxVelocity;
+  private double lastAngleMotorVelocity = 0.0;
 
   private static final String SUBSYSTEM_NAME = "Swerve";
   private static final boolean DEBUGGING = false;
@@ -94,12 +97,22 @@ public void setDesiredState(
    *
    * @param voltage the specified voltage for the drive motor
    */
-  public void setVoltageForCharacterization(double voltage) {
+  public void setVoltageForDriveCharacterization(double voltage) {
     io.setAnglePosition(0.0);
     lastAngle = 0.0;
     io.setDriveMotorPercentage(voltage / 12.0);
   }
 
+  /**
+   * Set the angle motor to the specified voltage. This is only used for characterization via the
+   * FeedForwardCharacterization command.
+   *
+   * @param voltage the specified voltage for the angle motor
+   */
+  public void setVoltageForRotateCharacterization(double voltage) {
+    io.setAngleMotorPercentage(voltage / 12.0);
+  }
+
   /**
    * Get the current state of this swerve module.
    *
@@ -146,6 +159,7 @@ public double getDriveCurrent() {
    * <p>This method must be invoked by the drivetrain subsystem's periodic method.
    */
   public void updateAndProcessInputs() {
+    this.lastAngleMotorVelocity = inputs.angleVelocityRevPerMin;
     io.updateInputs(inputs);
     Logger.getInstance().processInputs("Mod" + moduleNumber, inputs);
   }
@@ -168,6 +182,25 @@ public void setAngleBrakeMode(boolean enable) {
     io.setAngleBrakeMode(enable);
   }
 
+  /**
+   * Get the velocity of the angle motor in radians per second.
+   *
+   * @return the velocity of the angle motor in radians per second
+   */
+  public double getAngleMotorVelocity() {
+    return inputs.angleVelocityRevPerMin * 2.0 * Math.PI / 60.0;
+  }
+
+  /**
+   * Get the acceleration of the angle motor in radians per second^2.
+   *
+   * @return the acceleration of the angle motor in radians per second^2
+   */
+  public double getAngleMotorAcceleration() {
+    return ((inputs.angleVelocityRevPerMin - this.lastAngleMotorVelocity) / 60.0 * 2.0 * Math.PI)
+        / LOOP_PERIOD_SECS;
+  }
+
   /**
    * Returns a list of status signals for the swerve module related to odometry. This can be used to
    * synchronize the gyro and swerve modules to improve the accuracy of pose estimation.

src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java

@@ -34,6 +34,9 @@ public default void updateInputs(SwerveModuleIOInputs inputs) {}
   /** Run the drive motor at the specified percentage of full power. */
   public default void setDriveMotorPercentage(double percentage) {}
 
+  /** Run the angle motor at the specified percentage of full power. */
+  public default void setAngleMotorPercentage(double percentage) {}
+
   /** Run the drive motor at the specified velocity. */
   public default void setDriveVelocity(double velocity) {}
 

src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java

@@ -72,6 +72,7 @@ public class SwerveModuleIOTalonFXPhoenix6 implements SwerveModuleIO {
 
   private VoltageOut driveVoltageRequest;
   private VelocityVoltage driveVelocityRequest;
+  private VoltageOut angleVoltageRequest;
   private PositionVoltage anglePositionRequest;
 
   private StatusSignal<Double> anglePositionStatusSignal;
@@ -178,6 +179,8 @@ private void configAngleMotor(int angleMotorID, int canCoderID) {
     config.Slot0.kP = turnKp.get();
     config.Slot0.kI = turnKi.get();
     config.Slot0.kD = turnKd.get();
+    config.Slot0.kS = RobotConfig.getInstance().getSwerveAngleKS();
+    config.Slot0.kV = RobotConfig.getInstance().getSwerveAngleKV();
 
     config.ClosedLoopGeneral.ContinuousWrap = true;
 
@@ -202,6 +205,8 @@ private void configAngleMotor(int angleMotorID, int canCoderID) {
     this.angleVelocityStatusSignal = this.angleMotor.getVelocity();
     this.anglePositionErrorStatusSignal = this.angleMotor.getClosedLoopError();
 
+    this.angleVoltageRequest = new VoltageOut(0.0);
+    this.angleVoltageRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed();
     this.anglePositionRequest = new PositionVoltage(0.0).withSlot(0);
     this.anglePositionRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed();
   }
@@ -341,6 +346,12 @@ public void setDriveMotorPercentage(double percentage) {
     this.driveMotor.setControl(driveVoltageRequest.withOutput(percentage * 12.0));
   }
 
+  /** Run the angle motor at the specified percentage of full power. */
+  @Override
+  public void setAngleMotorPercentage(double percentage) {
+    this.angleMotor.setControl(angleVoltageRequest.withOutput(percentage * 12.0));
+  }
+
   /** Run the drive motor at the specified velocity. */
   @Override
   public void setDriveVelocity(double velocity) {
@@ -433,12 +444,13 @@ private void updateSim() {
 
     // update the simulated TalonFXs and CANcoder based on the model outputs
     double turnRPM = turnSim.getAngularVelocityRPM();
-    double angleEncoderRPS = turnRPM / 60;
+    double angleEncoderRPS = turnRPM / 60.0;
     double angleEncoderRotations = angleEncoderRPS * Constants.LOOP_PERIOD_SECS;
 
     this.angleEncoderSimState.addPosition(angleEncoderRotations);
-    this.angleMotorSimState.addRotorPosition(angleEncoderRotations);
-    this.angleMotorSimState.setRotorVelocity(angleEncoderRPS);
+    this.angleEncoderSimState.setVelocity(angleEncoderRPS);
+    this.angleMotorSimState.addRotorPosition(angleEncoderRotations / angleGearRatio);
+    this.angleMotorSimState.setRotorVelocity(angleEncoderRPS / angleGearRatio);
 
     double driveMPS = driveSim.getOutput(0);
     double driveMotorRPS = Conversions.mpsToFalconRPS(driveMPS, wheelCircumference, driveGearRatio);

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

@@ -376,17 +376,33 @@ private void configureAutoCommands() {
 
     /************ Drive Characterization ************
      *
-     * useful for characterizing the drivetrain (i.e, determining kS and kV)
+     * useful for characterizing the swerve modules for driving (i.e, determining kS and kV)
      *
      */
     autoChooser.addOption(
-        "Drive Characterization",
+        "Swerve Drive Characterization",
         new FeedForwardCharacterization(
             drivetrain,
             true,
             new FeedForwardCharacterizationData("drive"),
-            drivetrain::runCharacterizationVolts,
-            drivetrain::getCharacterizationVelocity));
+            drivetrain::runDriveCharacterizationVolts,
+            drivetrain::getDriveCharacterizationVelocity,
+            drivetrain::getDriveCharacterizationAcceleration));
+
+    /************ Swerve Rotate Characterization ************
+     *
+     * useful for characterizing the swerve modules for rotating (i.e, determining kS and kV)
+     *
+     */
+    autoChooser.addOption(
+        "Swerve Rotate Characterization",
+        new FeedForwardCharacterization(
+            drivetrain,
+            true,
+            new FeedForwardCharacterizationData("rotate"),
+            drivetrain::runRotateCharacterizationVolts,
+            drivetrain::getRotateCharacterizationVelocity,
+            drivetrain::getRotateCharacterizationAcceleration));
 
     /************ Distance Test ************
      *