General Update

src/main/java/org/trigon/commands/GearRatioCalculationCommand.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder;
 import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal;
 import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
@@ -23,14 +23,29 @@ public class GearRatioCalculationCommand extends Command {
     private final DoubleConsumer runGearRatioCalculation;
     private final String subsystemName;
     private final double backlashAccountabilityTimeSeconds;
-    private final LoggedDashboardNumber movementVoltage;
+    private final LoggedNetworkNumber movementVoltage;
 
     private double startingRotorPosition;
     private double startingEncoderPosition;
     private double gearRatio;
     private double startTime;
     private boolean hasSetStartingPositions = false;
 
+    /**
+     * Creates a new GearRatioCalculationCommand.
+     * This constructor takes a motor to run the gear ratio calculation on, and to measure the distance the rotor travels.
+     * It also takes an encoder to measure the distance traveled.
+     * This constructor will record the starting positions without any delay. This might be problematic when the subsystem has backlash.
+     * When you have backlash and want to account for it with a measuring delay, use {@link GearRatioCalculationCommand#GearRatioCalculationCommand(TalonFXMotor, CANcoderEncoder, double, SubsystemBase)}.
+     *
+     * @param motor       the motor that drives the rotor
+     * @param encoder     the encoder that measures the distance traveled
+     * @param requirement the subsystem that this command requires
+     */
+    public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, SubsystemBase requirement) {
+        this(motor, encoder, 0, requirement);
+    }
+
     /**
      * Creates a new GearRatioCalculationCommand.
      * This constructor takes a motor to run the gear ratio calculation on, and to measure the distance the rotor travels.
@@ -51,11 +66,29 @@ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder,
         );
     }
 
+    /**
+     * Creates a new GearRatioCalculationCommand.
+     * This constructor will record the starting positions without any delay. This might be problematic when the system has backlash.
+     * When you have backlash and want to account for it with a measuring delay, use {@link GearRatioCalculationCommand#GearRatioCalculationCommand(DoubleSupplier, DoubleSupplier, DoubleConsumer, double, SubsystemBase)}.
+     *
+     * @param rotorPositionSupplier   a supplier that returns the current position of the rotor. Must be in the same units as the encoder position supplier
+     * @param encoderPositionSupplier a supplier that returns the current position of the encoder. Must be in the same units as the rotor position supplier
+     * @param runGearRatioCalculation a consumer that drives the motor with a given voltage
+     * @param requirement             the subsystem that this command requires
+     */
+    public GearRatioCalculationCommand(
+            DoubleSupplier rotorPositionSupplier,
+            DoubleSupplier encoderPositionSupplier,
+            DoubleConsumer runGearRatioCalculation,
+            SubsystemBase requirement) {
+        this(rotorPositionSupplier, encoderPositionSupplier, runGearRatioCalculation, 0, requirement);
+    }
+
     /**
      * Creates a new GearRatioCalculationCommand.
      *
-     * @param rotorPositionSupplier             a supplier that returns the current position of the rotor in degrees
-     * @param encoderPositionSupplier           a supplier that returns the current position of the encoder in degrees
+     * @param rotorPositionSupplier             a supplier that returns the current position of the rotor. Must be in the same units as the encoder position supplier
+     * @param encoderPositionSupplier           a supplier that returns the current position of the encoder. Must be in the same units as the rotor position supplier
      * @param runGearRatioCalculation           a consumer that drives the motor with a given voltage
      * @param backlashAccountabilityTimeSeconds the time to wait before setting the starting positions in order to account for backlash
      * @param requirement                       the subsystem that this command requires
@@ -72,7 +105,7 @@ public GearRatioCalculationCommand(
         this.subsystemName = requirement.getName();
         this.backlashAccountabilityTimeSeconds = backlashAccountabilityTimeSeconds;
 
-        this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1);
+        this.movementVoltage = new LoggedNetworkNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1);
 
         addRequirements(requirement);
     }
@@ -88,7 +121,7 @@ public void initialize() {
     public void execute() {
         runGearRatioCalculation();
 
-        if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions)
+        if (Timer.getTimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions)
             setStartingPositions();
 
         if (hasSetStartingPositions) {

src/main/java/org/trigon/commands/NetworkTablesCommand.java

@@ -2,7 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 import java.util.Set;
 import java.util.function.BiConsumer;
@@ -15,7 +15,7 @@
  */
 public class NetworkTablesCommand extends Command {
     private final Consumer<Double[]> toRun;
-    private final LoggedDashboardNumber[] dashboardNumbers;
+    private final LoggedNetworkNumber[] dashboardNumbers;
     private final boolean runPeriodically;
 
     /**
@@ -29,9 +29,9 @@ public class NetworkTablesCommand extends Command {
     public NetworkTablesCommand(Consumer<Double[]> toRun, boolean runPeriodically, Set<Subsystem> requirements, String... keys) {
         this.toRun = toRun;
         this.runPeriodically = runPeriodically;
-        dashboardNumbers = new LoggedDashboardNumber[keys.length];
+        dashboardNumbers = new LoggedNetworkNumber[keys.length];
         for (int i = 0; i < keys.length; i++)
-            dashboardNumbers[i] = new LoggedDashboardNumber(keys[i]);
+            dashboardNumbers[i] = new LoggedNetworkNumber(keys[i]);
         addRequirements(requirements.toArray(new Subsystem[0]));
     }
 

src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java

@@ -12,7 +12,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 import java.util.Arrays;
 import java.util.function.DoubleConsumer;
@@ -23,8 +23,8 @@
  * A command for characterizing the wheel radius of each module of a swerve drive system by calculating the radius of each drive wheel based on rotational data and robot yaw during operation.
  */
 public class WheelRadiusCharacterizationCommand extends Command {
-    private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1);
-    private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1);
+    private static final LoggedNetworkNumber CHARACTERIZATION_SPEED = new LoggedNetworkNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1);
+    private static final LoggedNetworkNumber ROTATION_RATE_LIMIT = new LoggedNetworkNumber("WheelRadiusCharacterization/RotationRateLimit", 1);
 
     private final double[]
             wheelDistancesFromCenterMeters,

src/main/java/org/trigon/hardware/BaseInputs.java

@@ -73,7 +73,7 @@ public double[] getThreadedSignal(String signalName) {
     }
 
     private boolean shouldPrintError() {
-        final double currentTime = Timer.getFPGATimestamp();
+        final double currentTime = Timer.getTimestamp();
         final boolean shouldPrint = currentTime - lastErrorTimestamp > 5;
         if (shouldPrint)
             lastErrorTimestamp = currentTime;

src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java

@@ -65,7 +65,7 @@ void clearLEDColors() {
     @Override
     void blink(Color firstColor, double speed) {
         final double correctedSpeed = 1 - speed;
-        final double currentTime = Timer.getFPGATimestamp();
+        final double currentTime = Timer.getTimestamp();
 
         if (currentTime - lastLEDAnimationChangeTime > correctedSpeed) {
             lastLEDAnimationChangeTime = currentTime;
@@ -89,7 +89,7 @@ void breathe(Color color, int breathingLEDs, double speed, boolean inverted, Lar
         clearLEDColors();
         final boolean correctedInverted = this.inverted != inverted;
         final double moveLEDTimeSeconds = 1 - speed;
-        final double currentTime = Timer.getFPGATimestamp();
+        final double currentTime = Timer.getTimestamp();
 
         if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
             lastLEDAnimationChangeTime = currentTime;
@@ -108,7 +108,7 @@ void colorFlow(Color color, double speed, boolean inverted) {
         clearLEDColors();
         final boolean correctedInverted = this.inverted != inverted;
         final double moveLEDTimeSeconds = 1 - speed;
-        final double currentTime = Timer.getFPGATimestamp();
+        final double currentTime = Timer.getTimestamp();
 
         if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
             lastLEDAnimationChangeTime = currentTime;
@@ -165,7 +165,7 @@ void sectionColor(Supplier<Color>[] colors) {
     @Override
     void resetLEDSettings() {
         lastBreatheLED = indexOffset;
-        lastLEDAnimationChangeTime = Timer.getFPGATimestamp();
+        lastLEDAnimationChangeTime = Timer.getTimestamp();
         rainbowFirstPixelHue = 0;
         isLEDAnimationChanged = false;
         amountOfColorFlowLEDs = 0;

src/main/java/org/trigon/hardware/rev/spark/io/RealSparkIO.java

@@ -66,7 +66,9 @@ public void configure(SparkBaseConfig configuration, SparkBase.ResetMode resetMo
 
     @Override
     public void setInverted(boolean inverted) {
-        motor.setInverted(inverted);
+        final SparkMaxConfig configuration = new SparkMaxConfig();
+        configuration.inverted(inverted);
+        motor.configure(configuration, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
     }
 
     @Override

src/main/java/org/trigon/hardware/rev/spark/io/SimulationSparkIO.java

@@ -73,7 +73,9 @@ public void setPeriodicFrameTimeout(int timeoutMs) {
 
     @Override
     public void setInverted(boolean inverted) {
-        motor.setInverted(inverted);
+        final SparkMaxConfig configuration = new SparkMaxConfig();
+        configuration.inverted(inverted);
+        motor.configure(configuration, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
     }
 
     @Override
@@ -89,8 +91,9 @@ public void updateSimulation() {
             return;
 
         updatePhysicsSimulation();
-        updateMotorSimulation();
-        updateEncoderSimulation();
+        final double physicsSimulationVelocity = getPhysicsSimulationVelocity();
+        updateMotorSimulation(physicsSimulationVelocity);
+        updateEncoderSimulation(physicsSimulationVelocity);
     }
 
     @Override
@@ -109,22 +112,27 @@ private void updatePhysicsSimulation() {
         physicsSimulation.updateMotor();
     }
 
-    private void updateMotorSimulation() {
-        motorSimulation.iterate(getPhysicsSimulationVelocityForMotorSimulation(), RobotHardwareStats.SUPPLY_VOLTAGE, RobotHardwareStats.getPeriodicTimeSeconds());
+    private void updateMotorSimulation(double physicsSimulationVelocityForSimulation) {
+        motorSimulation.iterate(physicsSimulationVelocityForSimulation, RobotHardwareStats.SUPPLY_VOLTAGE, RobotHardwareStats.getPeriodicTimeSeconds());
         motorSimulation.setMotorCurrent(physicsSimulation.getCurrent());
     }
 
-    private void updateEncoderSimulation() {
-        absoluteEncoderSimulation.iterate(getPhysicsSimulationVelocityForMotorSimulation(), RobotHardwareStats.getPeriodicTimeSeconds());
+    private void updateEncoderSimulation(double physicsSimulationVelocityForSimulation) {
+        absoluteEncoderSimulation.iterate(physicsSimulationVelocityForSimulation, RobotHardwareStats.getPeriodicTimeSeconds());
     }
 
-    private double getPhysicsSimulationVelocityForMotorSimulation() {
+    /**
+     * Gets the velocity from the physics simulation in the units set in the conversion factor. This is used in the iterate method to update the motor simulation.
+     *
+     * @return the velocity from the physics simulation
+     */
+    private double getPhysicsSimulationVelocity() {
         if (isUsingAbsoluteEncoder)
-            return getConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getSystemVelocityRotationsPerSecond());
-        return getConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getRotorVelocityRotationsPerSecond());
+            return getVelocityConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getSystemVelocityRotationsPerSecond());
+        return getVelocityConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getRotorVelocityRotationsPerSecond());
     }
 
-    private double getConversionFactor() {
-        return motor.configAccessor.encoder.getPositionConversionFactor();
+    private double getVelocityConversionFactor() {
+        return motor.configAccessor.encoder.getVelocityConversionFactor();
     }
 }