diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 3222ed96..d9f28bf5 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/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,7 +23,7 @@ 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; @@ -31,6 +31,21 @@ public class GearRatioCalculationCommand extends Command { 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) { diff --git a/src/main/java/org/trigon/commands/NetworkTablesCommand.java b/src/main/java/org/trigon/commands/NetworkTablesCommand.java index 4e9ffc72..1c48652f 100644 --- a/src/main/java/org/trigon/commands/NetworkTablesCommand.java +++ b/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 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 toRun, boolean runPeriodically, Set 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])); } diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 07a7cc52..6ad1890e 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/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, diff --git a/src/main/java/org/trigon/hardware/BaseInputs.java b/src/main/java/org/trigon/hardware/BaseInputs.java index edf8a9ab..6f05d373 100644 --- a/src/main/java/org/trigon/hardware/BaseInputs.java +++ b/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; diff --git a/src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java b/src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java index 17b485f6..c9c329b5 100644 --- a/src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java +++ b/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[] colors) { @Override void resetLEDSettings() { lastBreatheLED = indexOffset; - lastLEDAnimationChangeTime = Timer.getFPGATimestamp(); + lastLEDAnimationChangeTime = Timer.getTimestamp(); rainbowFirstPixelHue = 0; isLEDAnimationChanged = false; amountOfColorFlowLEDs = 0; diff --git a/src/main/java/org/trigon/hardware/rev/spark/io/RealSparkIO.java b/src/main/java/org/trigon/hardware/rev/spark/io/RealSparkIO.java index 1797f2ce..ec157185 100644 --- a/src/main/java/org/trigon/hardware/rev/spark/io/RealSparkIO.java +++ b/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 diff --git a/src/main/java/org/trigon/hardware/rev/spark/io/SimulationSparkIO.java b/src/main/java/org/trigon/hardware/rev/spark/io/SimulationSparkIO.java index 8f1ea862..b1bb907d 100644 --- a/src/main/java/org/trigon/hardware/rev/spark/io/SimulationSparkIO.java +++ b/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(); } } \ No newline at end of file