From 48c34a9c05154972e9e367df07c74f4b55a32893 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 10 Dec 2024 22:00:40 +0200 Subject: [PATCH 1/7] updated akit stuff, and added more gear ratio calc constructors --- .../commands/GearRatioCalculationCommand.java | 68 +++++++++++++++++-- .../trigon/commands/NetworkTablesCommand.java | 8 +-- .../WheelRadiusCharacterizationCommand.java | 6 +- .../java/org/trigon/hardware/BaseInputs.java | 2 +- .../misc/leds/AddressableLEDStrip.java | 8 +-- 5 files changed, 75 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 3222ed96..35d01bd0 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,33 @@ 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. + * + * @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. + * It also takes an encoder to measure the distance traveled. + * + * @param motor the motor that drives the rotor + * @param encoder the encoder that measures the distance traveled + * @param isFOCEnabled whether FOC is enabled on the motor or not + * @param requirement the subsystem that this command requires + */ + public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, boolean isFOCEnabled, SubsystemBase requirement) { + this(motor, encoder, isFOCEnabled, 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. @@ -42,15 +69,46 @@ public class GearRatioCalculationCommand extends Command { * @param requirement the subsystem that this command requires */ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) { + this(motor, encoder, false, backlashAccountabilityTimeSeconds, 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. + * It also takes an encoder to measure the distance traveled. + * + * @param motor the motor that drives the rotor + * @param encoder the encoder that measures the distance traveled + * @param isFOCEnabled whether FOC is enabled on the motor or not + * @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 + */ + public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, boolean isFOCEnabled, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) { this( () -> motor.getSignal(TalonFXSignal.ROTOR_POSITION), () -> encoder.getSignal(CANcoderSignal.POSITION), - (voltage) -> motor.setControl(new VoltageOut(voltage)), + (voltage) -> motor.setControl(new VoltageOut(voltage).withEnableFOC(isFOCEnabled)), backlashAccountabilityTimeSeconds, 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 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. * @@ -72,7 +130,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 +146,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; From 187b5c75f5846767f5aad3d15a0d5026a97bfe75 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 10 Dec 2024 22:05:09 +0200 Subject: [PATCH 2/7] Realized some of these constructors are really unnecacery --- .../commands/GearRatioCalculationCommand.java | 34 ++----------------- 1 file changed, 2 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 35d01bd0..f590abc3 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -44,20 +44,6 @@ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, 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. - * It also takes an encoder to measure the distance traveled. - * - * @param motor the motor that drives the rotor - * @param encoder the encoder that measures the distance traveled - * @param isFOCEnabled whether FOC is enabled on the motor or not - * @param requirement the subsystem that this command requires - */ - public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, boolean isFOCEnabled, SubsystemBase requirement) { - this(motor, encoder, isFOCEnabled, 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. @@ -69,28 +55,12 @@ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, * @param requirement the subsystem that this command requires */ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) { - this(motor, encoder, false, backlashAccountabilityTimeSeconds, 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. - * It also takes an encoder to measure the distance traveled. - * - * @param motor the motor that drives the rotor - * @param encoder the encoder that measures the distance traveled - * @param isFOCEnabled whether FOC is enabled on the motor or not - * @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 - */ - public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, boolean isFOCEnabled, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) { this( () -> motor.getSignal(TalonFXSignal.ROTOR_POSITION), () -> encoder.getSignal(CANcoderSignal.POSITION), - (voltage) -> motor.setControl(new VoltageOut(voltage).withEnableFOC(isFOCEnabled)), + (voltage) -> motor.setControl(new VoltageOut(voltage)), backlashAccountabilityTimeSeconds, - requirement - ); + requirement); } /** From 43a5e987ebe7bbeb05fe340f0f7df6cf230b7c83 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 10 Dec 2024 22:06:13 +0200 Subject: [PATCH 3/7] this bugged me --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index f590abc3..f7a58e0b 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -60,7 +60,8 @@ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, () -> encoder.getSignal(CANcoderSignal.POSITION), (voltage) -> motor.setControl(new VoltageOut(voltage)), backlashAccountabilityTimeSeconds, - requirement); + requirement + ); } /** From 88112a7a3ced7c71015da5225b7184067e81a17a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 15 Dec 2024 18:03:56 +0200 Subject: [PATCH 4/7] made spark sim more efficiant, and removed usage of deprecated methods --- .../hardware/rev/spark/io/RealSparkIO.java | 4 +++- .../rev/spark/io/SimulationSparkIO.java | 17 ++++++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) 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..5550f432 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 physicsSimulationVelocityForSimulation = getPhysicsSimulationVelocityForMotorSimulation(); + updateMotorSimulation(physicsSimulationVelocityForSimulation); + updateEncoderSimulation(physicsSimulationVelocityForSimulation); } @Override @@ -109,13 +112,13 @@ 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() { From c769e887b07635cd738d3784da7dd3a006d68337 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Wed, 18 Dec 2024 10:00:38 +0200 Subject: [PATCH 5/7] added jdocs, and corrected cf in spark sim --- .../commands/GearRatioCalculationCommand.java | 4 ++++ .../hardware/rev/spark/io/SimulationSparkIO.java | 13 +++++++++---- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index f7a58e0b..13d64029 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -35,6 +35,8 @@ public class GearRatioCalculationCommand extends Command { * 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 the other constructor. * * @param motor the motor that drives the rotor * @param encoder the encoder that measures the distance traveled @@ -66,6 +68,8 @@ 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 the other constructor. * * @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 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 5550f432..b5b1baa8 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 @@ -121,13 +121,18 @@ private void updateEncoderSimulation(double physicsSimulationVelocityForSimulati absoluteEncoderSimulation.iterate(physicsSimulationVelocityForSimulation, RobotHardwareStats.getPeriodicTimeSeconds()); } + /** + * 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 getPhysicsSimulationVelocityForMotorSimulation() { 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 From 56ac5a9cb9db526e3d2033ef45d7dc3fb3e71118 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 19 Dec 2024 21:22:31 +0200 Subject: [PATCH 6/7] jdoc changes --- .../trigon/commands/GearRatioCalculationCommand.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 13d64029..d9f28bf5 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -36,7 +36,7 @@ public class GearRatioCalculationCommand extends Command { * 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 the other constructor. + * 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 @@ -69,10 +69,10 @@ 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 the other constructor. + * 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 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 requirement the subsystem that this command requires */ @@ -87,8 +87,8 @@ public GearRatioCalculationCommand( /** * 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 From 900cc73ab70324e127e7258cb3045cd614d6ac18 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 24 Dec 2024 13:08:53 +0200 Subject: [PATCH 7/7] name change --- .../trigon/hardware/rev/spark/io/SimulationSparkIO.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 b5b1baa8..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 @@ -91,9 +91,9 @@ public void updateSimulation() { return; updatePhysicsSimulation(); - final double physicsSimulationVelocityForSimulation = getPhysicsSimulationVelocityForMotorSimulation(); - updateMotorSimulation(physicsSimulationVelocityForSimulation); - updateEncoderSimulation(physicsSimulationVelocityForSimulation); + final double physicsSimulationVelocity = getPhysicsSimulationVelocity(); + updateMotorSimulation(physicsSimulationVelocity); + updateEncoderSimulation(physicsSimulationVelocity); } @Override @@ -126,7 +126,7 @@ private void updateEncoderSimulation(double physicsSimulationVelocityForSimulati * * @return the velocity from the physics simulation */ - private double getPhysicsSimulationVelocityForMotorSimulation() { + private double getPhysicsSimulationVelocity() { if (isUsingAbsoluteEncoder) return getVelocityConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getSystemVelocityRotationsPerSecond()); return getVelocityConversionFactor() * Conversions.perMinuteToPerSecond(physicsSimulation.getRotorVelocityRotationsPerSecond());