From 00f6bdb051b027ee922effa6f3029ffc719dda73 Mon Sep 17 00:00:00 2001 From: truher Date: Sat, 14 Feb 2026 15:33:57 -0800 Subject: [PATCH 1/3] tuning --- .../wpi/DutyCycleRotaryPositionSensor.java | 11 ++- .../lib/util/Rotation2dInterpolator.java | 11 +++ studies/camera_delay/.Glass/glass.json | 78 +++++++++++++++++++ .../main/java/org/team100/frc2026/Robot.java | 12 ++- 4 files changed, 107 insertions(+), 5 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java create mode 100644 studies/camera_delay/.Glass/glass.json diff --git a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java index d2be49a..8402b90 100644 --- a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java +++ b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java @@ -23,7 +23,8 @@ * counter. * * Note that for the first few seconds after the sensor is constructed on the - * RoboRIO, the duty cycle input produces garbage. So the Robot class should sleep awhile. + * RoboRIO, the duty cycle input produces garbage. So the Robot class should + * sleep awhile. * * Relies on Memo and Takt, so you must put Memo.resetAll() and Takt.update() in * Robot.robotPeriodic(). @@ -59,6 +60,14 @@ protected DutyCycleRotaryPositionSensor( m_log_frequency = log.intLogger(Level.TRACE, "frequency"); m_log_connected = log.booleanLogger(Level.TRACE, "connected"); log.intLogger(Level.COMP, "channel").log(() -> channel.channel); + waitForDutyCycleBug(); + } + + void waitForDutyCycleBug() { + try { + Thread.sleep(3000); + } catch (InterruptedException e) { + } } @Override diff --git a/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java b/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java new file mode 100644 index 0000000..fb4b284 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java @@ -0,0 +1,11 @@ +package org.team100.lib.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.Interpolator; + +public class Rotation2dInterpolator implements Interpolator { + @Override + public Rotation2d interpolate(Rotation2d startValue, Rotation2d endValue, double t) { + return startValue.interpolate(endValue, t); + } +} diff --git a/studies/camera_delay/.Glass/glass.json b/studies/camera_delay/.Glass/glass.json new file mode 100644 index 0000000..d4b2f0d --- /dev/null +++ b/studies/camera_delay/.Glass/glass.json @@ -0,0 +1,78 @@ +{ + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "retained": { + "log": { + "NeoVortexCANSparkMotor": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Log Level": "String Chooser", + "field": "Field2d" + } + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "100" + }, + "Plots": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 272, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:log/AS5048RotaryPositionSensor/frequency" + }, + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:log/AS5048RotaryPositionSensor/duty cycle" + } + ] + }, + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 272 + } + ] + } + } +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java b/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java index d184868..cb63156 100644 --- a/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java +++ b/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java @@ -16,6 +16,7 @@ import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; import org.team100.lib.util.CanId; import org.team100.lib.util.RoboRioChannel; +import org.team100.lib.util.Rotation2dInterpolator; import org.team100.lib.util.TimeInterpolatableBuffer100; import edu.wpi.first.math.geometry.Rotation2d; @@ -47,10 +48,10 @@ public Robot() { log, new CanId(1), MotorPhase.FORWARD, - 1, // current limit + 20, // current limit NeoVortexCANSparkMotor.ff(log), NeoVortexCANSparkMotor.friction(log), - PIDConstants.makeVelocityPID(log, 0.0002)); + PIDConstants.makeVelocityPID(log, 0.02)); RoboRioChannel sensorChannel = new RoboRioChannel(1); m_sensor = new AS5048RotaryPositionSensor( @@ -59,8 +60,8 @@ public Robot() { 0.0, // offset EncoderDrive.DIRECT); - m_sensorBuffer = new TimeInterpolatableBuffer100<>(2, 0, Rotation2d.kZero); - m_cameraBuffer = new TimeInterpolatableBuffer100<>(2, 0, Rotation2d.kZero); + m_sensorBuffer = new TimeInterpolatableBuffer100<>(new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); + m_cameraBuffer = new TimeInterpolatableBuffer100<>(new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); // Update the buffer with the roll component, and accept the supplied timestamp // as true. @@ -104,6 +105,9 @@ public void robotPeriodic() { double laggedCamera = m_cameraBuffer.get(past).getRadians(); m_logCamera.log(() -> laggedCamera); m_logDiff.log(() -> laggedSensor - laggedCamera); + + m_motor.periodic(); + m_sensor.periodic(); } @Override From 49d0d010969971506a641ccfe2fc2a94fef3c0ca Mon Sep 17 00:00:00 2001 From: truher Date: Sat, 14 Feb 2026 15:34:31 -0800 Subject: [PATCH 2/3] tuning --- studies/camera_delay/.Glass/glass.json | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/studies/camera_delay/.Glass/glass.json b/studies/camera_delay/.Glass/glass.json index d4b2f0d..30f89ea 100644 --- a/studies/camera_delay/.Glass/glass.json +++ b/studies/camera_delay/.Glass/glass.json @@ -5,9 +5,15 @@ }, "retained": { "log": { + "Friction": { + "open": true + }, "NeoVortexCANSparkMotor": { "open": true }, + "SimpleDynamics": { + "open": true + }, "open": true } }, From 3e2979952e57525b03607d3c409aedda1b9ffaa5 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sat, 14 Feb 2026 22:28:46 -0800 Subject: [PATCH 3/3] make camera tester work better --- .../wpi/DutyCycleRotaryPositionSensor.java | 8 - .../lib/util/Rotation2dInterpolator.java | 11 ++ .../lib/util/TimeInterpolatableBuffer100.java | 12 ++ .../team100/lib/geometry/Rotation2dTest.java | 29 ++++ studies/camera_delay/.vscode/settings.json | 6 +- studies/camera_delay/simgui-ds.json | 10 ++ .../main/java/org/team100/frc2026/Robot.java | 160 +++++++++++++----- .../team100/frc2026/SimulatedGroundTruth.java | 67 -------- .../java/org/team100/frc2026/Simulator.java | 36 ---- .../absolute/wpi/SimulatedAS5048.java | 21 ++- .../frc2026/SimulatedGroundTruthTest.java | 37 ---- 11 files changed, 202 insertions(+), 195 deletions(-) create mode 100644 lib/src/test/java/org/team100/lib/geometry/Rotation2dTest.java delete mode 100644 studies/camera_delay/src/main/java/org/team100/frc2026/SimulatedGroundTruth.java delete mode 100644 studies/camera_delay/src/main/java/org/team100/frc2026/Simulator.java delete mode 100644 studies/camera_delay/src/test/java/org/team100/frc2026/SimulatedGroundTruthTest.java diff --git a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java index 8402b90..345b5f8 100644 --- a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java +++ b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java @@ -60,14 +60,6 @@ protected DutyCycleRotaryPositionSensor( m_log_frequency = log.intLogger(Level.TRACE, "frequency"); m_log_connected = log.booleanLogger(Level.TRACE, "connected"); log.intLogger(Level.COMP, "channel").log(() -> channel.channel); - waitForDutyCycleBug(); - } - - void waitForDutyCycleBug() { - try { - Thread.sleep(3000); - } catch (InterruptedException e) { - } } @Override diff --git a/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java b/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java index fb4b284..19cef9c 100644 --- a/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java +++ b/lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java @@ -3,6 +3,17 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.Interpolator; +/** + * For use with TimeInterpolatableBuffer (or something similar). + * + * Note that the buffer class short-cuts exact matches. + * + * This exposes an inconsistency (a bug IMHO) in the Rotation2d class, + * wherein the interpolated value wraps, and the uninterpolated value does not. + * + * So you'll want to wrap the buffer output, e.g. by adding kZero (which wraps + * the value!) + */ public class Rotation2dInterpolator implements Interpolator { @Override public Rotation2d interpolate(Rotation2d startValue, Rotation2d endValue, double t) { diff --git a/lib/src/main/java/org/team100/lib/util/TimeInterpolatableBuffer100.java b/lib/src/main/java/org/team100/lib/util/TimeInterpolatableBuffer100.java index 05c2f01..7d7d311 100644 --- a/lib/src/main/java/org/team100/lib/util/TimeInterpolatableBuffer100.java +++ b/lib/src/main/java/org/team100/lib/util/TimeInterpolatableBuffer100.java @@ -65,6 +65,8 @@ public void put(double timeS, T value) { if (oldestAgeS < m_historyS) break; m_pastSnapshots.remove(oldestTimeS); + if (DEBUG) + System.out.printf("removed %f\n", oldestTimeS); } m_pastSnapshots.put(timeS, value); } finally { @@ -131,6 +133,9 @@ public T get(double timeSeconds) { double timeSpan = topBound.getKey() - bottomBound.getKey(); double timeFraction = timeSinceBottom / timeSpan; if (DEBUG) { + System.out.printf("timeSeconds %f bottom %f top %f\n", + timeSeconds, bottomBound.getKey(), topBound.getKey()); + System.out.printf("interpolate %f\n", timeFraction); } return m_interpolator.interpolate( @@ -164,4 +169,11 @@ public int size() { public double lastKey() { return m_pastSnapshots.lastKey(); } + + /** Print the entire buffer */ + public void dump() { + for (var x : m_pastSnapshots.entrySet()) { + System.out.printf("%s: %s\n", x.getKey(), x.getValue()); + } + } } \ No newline at end of file diff --git a/lib/src/test/java/org/team100/lib/geometry/Rotation2dTest.java b/lib/src/test/java/org/team100/lib/geometry/Rotation2dTest.java new file mode 100644 index 0000000..0d0a38c --- /dev/null +++ b/lib/src/test/java/org/team100/lib/geometry/Rotation2dTest.java @@ -0,0 +1,29 @@ +package org.team100.lib.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class Rotation2dTest { + + // this fails + // see https://github.com/wpilibsuite/allwpilib/issues/8617 + // @Test + void testInterpolate() { + Rotation2d x = new Rotation2d(100); + Rotation2d y = x.interpolate(x, 0); + assertEquals(x.getRadians(), y.getRadians(), 1e-9); + } + + // this fails + // see https://github.com/wpilibsuite/allwpilib/issues/8617 + // @Test + void testPlus() { + Rotation2d x = new Rotation2d(100); + Rotation2d y = x.plus(Rotation2d.kZero); + assertEquals(x.getRadians(), y.getRadians(), 1e-9); + } + +} diff --git a/studies/camera_delay/.vscode/settings.json b/studies/camera_delay/.vscode/settings.json index 4250c83..693dead 100644 --- a/studies/camera_delay/.vscode/settings.json +++ b/studies/camera_delay/.vscode/settings.json @@ -57,5 +57,9 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false + "java.dependency.enableDependencyCheckup": false, + "files.autoSave": "afterDelay", + "editor.minimap.enabled": false, + "editor.inlayHints.enabled": "off", + "workbench.editor.revealIfOpen": true } diff --git a/studies/camera_delay/simgui-ds.json b/studies/camera_delay/simgui-ds.json index 4919a24..9d31d15 100644 --- a/studies/camera_delay/simgui-ds.json +++ b/studies/camera_delay/simgui-ds.json @@ -1,9 +1,19 @@ { + "FMS": { + "window": { + "visible": false + } + }, "Joysticks": { "window": { "visible": false } }, + "System Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java b/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java index cb63156..fb24e96 100644 --- a/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java +++ b/studies/camera_delay/src/main/java/org/team100/frc2026/Robot.java @@ -6,52 +6,69 @@ import org.team100.lib.framework.TimedRobot100; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.DoubleLogger; +import org.team100.lib.logging.LoggerFactory.Rotation2dLogger; import org.team100.lib.logging.Logging; import org.team100.lib.motor.BareMotor; import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.rev.NeoVortexCANSparkMotor; +import org.team100.lib.motor.sim.SimulatedBareMotor; import org.team100.lib.network.RawTags; import org.team100.lib.sensor.position.absolute.EncoderDrive; import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; +import org.team100.lib.sensor.position.absolute.wpi.SimulatedAS5048; import org.team100.lib.util.CanId; import org.team100.lib.util.RoboRioChannel; import org.team100.lib.util.Rotation2dInterpolator; import org.team100.lib.util.TimeInterpolatableBuffer100; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; public class Robot extends TimedRobot100 { + private static final boolean DEBUG = false; /** * The position sensor is assumed to have a fixed delay of 600 us. */ private static final double SENSOR_DELAY_S = 0.0006; + private static final double MOTOR_SPEED_RAD_S = 1.0; + private final BareMotor m_motor; private final AS5048RotaryPositionSensor m_sensor; private final RawTags m_rawTags; + private final TimeInterpolatableBuffer100 m_motorBuffer; private final TimeInterpolatableBuffer100 m_sensorBuffer; private final TimeInterpolatableBuffer100 m_cameraBuffer; - private final DoubleLogger m_logSensor; - private final DoubleLogger m_logCamera; - private final DoubleLogger m_logDiff; + private final Rotation2dLogger m_logMotor; + private final Rotation2dLogger m_logSensor; + private final Rotation2dLogger m_logCamera; + private final Rotation2dLogger m_logSensorMinusCamera; + private final Rotation2dLogger m_logMotorMinusCamera; + private final Rotation2dLogger m_logMotorMinusSensor; + + private final SimulatedAS5048 m_simSensor; + private final SimulatedCamera m_simCamera; - private final Simulator m_sim; + private double m_positionRad; public Robot() { Logging logging = Logging.instance(); LoggerFactory log = logging.rootLogger; - m_motor = NeoVortexCANSparkMotor.get( - log, - new CanId(1), - MotorPhase.FORWARD, - 20, // current limit - NeoVortexCANSparkMotor.ff(log), - NeoVortexCANSparkMotor.friction(log), - PIDConstants.makeVelocityPID(log, 0.02)); + if (RobotBase.isReal()) { + m_motor = NeoVortexCANSparkMotor.get( + log, + new CanId(1), + MotorPhase.FORWARD, + 20, // current limit + NeoVortexCANSparkMotor.ff(log), + NeoVortexCANSparkMotor.friction(log), + PIDConstants.makePositionPID(log, 1)); + } else { + m_motor = new SimulatedBareMotor(log, 600); + } RoboRioChannel sensorChannel = new RoboRioChannel(1); m_sensor = new AS5048RotaryPositionSensor( @@ -60,51 +77,107 @@ public Robot() { 0.0, // offset EncoderDrive.DIRECT); - m_sensorBuffer = new TimeInterpolatableBuffer100<>(new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); - m_cameraBuffer = new TimeInterpolatableBuffer100<>(new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); - - // Update the buffer with the roll component, and accept the supplied timestamp - // as true. - m_rawTags = new RawTags(log, new Roll((r, t) -> m_cameraBuffer.put(t, r))); - - m_logSensor = log.doubleLogger(Level.TRACE, "lagged sensor"); - m_logCamera = log.doubleLogger(Level.TRACE, "lagged camera"); - m_logDiff = log.doubleLogger(Level.TRACE, "lagged difference"); + m_motorBuffer = new TimeInterpolatableBuffer100<>( + new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); + m_sensorBuffer = new TimeInterpolatableBuffer100<>( + new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); + m_cameraBuffer = new TimeInterpolatableBuffer100<>( + new Rotation2dInterpolator(), 2, 0, Rotation2d.kZero); + // Update the buffer with the roll component, and accept the + // supplied timestamp as true. + m_rawTags = new RawTags( + log, + new Roll((r, t) -> m_cameraBuffer.put(t, r))); + + m_logMotor = log.rotation2dLogger(Level.TRACE, "lagged motor"); + m_logSensor = log.rotation2dLogger(Level.TRACE, "lagged sensor"); + m_logCamera = log.rotation2dLogger(Level.TRACE, "lagged camera"); + m_logSensorMinusCamera = log.rotation2dLogger(Level.TRACE, "sensor minus camera"); + m_logMotorMinusCamera = log.rotation2dLogger(Level.TRACE, "motor minus camera"); + m_logMotorMinusSensor = log.rotation2dLogger(Level.TRACE, "motor minus sensor"); if (RobotBase.isSimulation()) { - m_sim = new Simulator(log, m_sensor); + // these extra additions are to wrap the result. + m_simSensor = new SimulatedAS5048( + (x) -> m_motorBuffer.get(x).plus(Rotation2d.kZero), + m_sensor); + m_simCamera = new SimulatedCamera( + (x) -> m_motorBuffer.get(x).plus(Rotation2d.kZero)); } else { - m_sim = null; + m_simSensor = null; + m_simCamera = null; + } + + waitForDutyCycleBug(); + } + + static void waitForDutyCycleBug() { + try { + if (RobotBase.isReal()) + Thread.sleep(3000); + } catch (InterruptedException e) { } } @Override public void robotPeriodic() { - // Update the clock. + // Refreshing the cache twice here. :-( + Takt.update(); + if (DEBUG) + System.out.printf("takt %f\n", Takt.actual()); - // Sim runs first so that it sees the Takt time. - if (m_sim != null) { - m_sim.run(); + // This updates the motor output to match the input. + Cache.refresh(); + + // the motor state is updated in the cache refresh + Rotation2d newValue = new Rotation2d(MathUtil.angleModulus(m_motor.getUnwrappedPositionRad())); + m_motorBuffer.put(Takt.actual(), newValue); + if (DEBUG) + System.out.printf("motor value %s\n", newValue); + + if (m_simSensor != null) { + m_simSensor.run(); + } + if (m_simCamera != null) { + m_simCamera.run(); } - // Updates the sensor. + // This updates the real AS5048 to read the value written by the simulated one. Cache.refresh(); // Read the sensor and update the sensor buffer. - m_sensorBuffer.put(Takt.actual() - SENSOR_DELAY_S, new Rotation2d( - m_sensor.getWrappedPositionRad())); + // The value here is the same one that was just written by the sim + Rotation2d sensorValue = new Rotation2d(m_sensor.getWrappedPositionRad()); + m_sensorBuffer.put(Takt.actual() - SENSOR_DELAY_S, sensorValue); + if (DEBUG) + System.out.printf("sensor value %s\n", sensorValue); // Read the camera and update the camera buffer. m_rawTags.update(); // Sample the buffers from 1 sec ago and log. double past = Takt.actual() - 1.0; - double laggedSensor = m_sensorBuffer.get(past).getRadians(); + + // the extra addition wraps the result + Rotation2d laggedMotor = m_motorBuffer.get(past).plus(Rotation2d.kZero); + if (DEBUG) + System.out.printf("lagged motor %s\n", laggedMotor); + m_logMotor.log(() -> laggedMotor); + + // the extra addition wraps the result + Rotation2d laggedSensor = m_sensorBuffer.get(past).plus(Rotation2d.kZero); + if (DEBUG) + System.out.printf("lagged sensor %s\n", laggedSensor); m_logSensor.log(() -> laggedSensor); - double laggedCamera = m_cameraBuffer.get(past).getRadians(); + + // the extra addition wraps the result + Rotation2d laggedCamera = m_cameraBuffer.get(past).plus(Rotation2d.kZero); m_logCamera.log(() -> laggedCamera); - m_logDiff.log(() -> laggedSensor - laggedCamera); + + m_logSensorMinusCamera.log(() -> laggedSensor.minus(laggedCamera)); + m_logMotorMinusCamera.log(() -> laggedMotor.minus(laggedCamera)); + m_logMotorMinusSensor.log(() -> laggedMotor.minus(laggedSensor)); m_motor.periodic(); m_sensor.periodic(); @@ -112,18 +185,21 @@ public void robotPeriodic() { @Override public void teleopInit() { - m_motor.setVelocity(1, 0, 0); - if (m_sim != null) { - m_sim.start(); - } + m_positionRad = 0; + m_motor.setUnwrappedEncoderPositionRad(m_positionRad); + } + + @Override + public void teleopPeriodic() { + // this comes before robotPeriodic. + double motorPositionIncrementRad = MOTOR_SPEED_RAD_S * TimedRobot100.LOOP_PERIOD_S; + m_positionRad += motorPositionIncrementRad; + m_motor.setUnwrappedPosition(m_positionRad, 0, 0, 0); } @Override public void teleopExit() { m_motor.stop(); - if (m_sim != null) { - m_sim.stop(); - } } } diff --git a/studies/camera_delay/src/main/java/org/team100/frc2026/SimulatedGroundTruth.java b/studies/camera_delay/src/main/java/org/team100/frc2026/SimulatedGroundTruth.java deleted file mode 100644 index a923492..0000000 --- a/studies/camera_delay/src/main/java/org/team100/frc2026/SimulatedGroundTruth.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.team100.frc2026; - -import java.util.function.DoubleFunction; - -import org.team100.lib.coherence.Takt; -import org.team100.lib.logging.Level; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.DoubleLogger; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** The actual position for simulation */ -public class SimulatedGroundTruth implements Runnable, DoubleFunction { - private static final double SPEED_RAD_S = 1.0; - /** When the disc started moving, or null if stopped */ - - private final DoubleLogger m_log_truth; - private Double t0; - /** Position at t0 */ - private Rotation2d x0; - - public SimulatedGroundTruth(LoggerFactory parent) { - LoggerFactory log = parent.type(this); - m_log_truth = log.doubleLogger(Level.TRACE, "lagged truth"); - t0 = null; - x0 = Rotation2d.kZero; - } - - /** Start spinning at the current position at time t. */ - public void start(double t) { - if (t0 != null) { - // already running - return; - } - // current x0 is the starting x0 - t0 = t; - } - - /** Stop spinning at the position at time t. */ - public void stop(double t) { - if (t0 == null) { - // already stopped - return; - } - // x0 is where it is right now - x0 = apply(t); - t0 = null; - } - - @Override - public void run() { - m_log_truth.log(() -> apply(Takt.actual() - 1.0).getRadians()); - } - - @Override - public Rotation2d apply(double t) { - if (t0 == null) { - // disc is stopped - return x0; - } - double dt = t - t0; - double dx = SPEED_RAD_S * dt; - Rotation2d dr = new Rotation2d(dx); - return x0.plus(dr); - } - -} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2026/Simulator.java b/studies/camera_delay/src/main/java/org/team100/frc2026/Simulator.java deleted file mode 100644 index 9e9d23b..0000000 --- a/studies/camera_delay/src/main/java/org/team100/frc2026/Simulator.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.team100.frc2026; - -import org.team100.lib.coherence.Takt; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; -import org.team100.lib.sensor.position.absolute.wpi.SimulatedAS5048; - -/** Container for simulation stuff */ -public class Simulator implements Runnable { - - private final SimulatedGroundTruth m_truth; - private final SimulatedAS5048 m_sensor; - private final SimulatedCamera m_camera; - - public Simulator(LoggerFactory parent, AS5048RotaryPositionSensor sensor) { - m_truth = new SimulatedGroundTruth(parent); - m_sensor = new SimulatedAS5048(m_truth, sensor); - m_camera = new SimulatedCamera(m_truth); - } - - public void start() { - m_truth.start(Takt.actual()); - } - - public void stop() { - m_truth.stop(Takt.actual()); - } - - @Override - public void run() { - m_truth.run(); - m_sensor.run(); - m_camera.run(); - } - -} diff --git a/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java b/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java index 30b2fc1..ef430a3 100644 --- a/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java +++ b/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java @@ -14,6 +14,8 @@ * For simulation of position through the real AS5048 code. */ public class SimulatedAS5048 implements Runnable { + private static final boolean DEBUG = false; + /** * The position sensor is assumed to have a fixed delay of 600 us. */ @@ -26,10 +28,14 @@ public class SimulatedAS5048 implements Runnable { private static final double SENSOR_RANGE = SENSOR_MAX - SENSOR_MIN; private final DoubleFunction m_truth; + private final AS5048RotaryPositionSensor m_sensor; private final DutyCycleSim m_sim; - public SimulatedAS5048(DoubleFunction truth, AS5048RotaryPositionSensor sensor) { + public SimulatedAS5048( + DoubleFunction truth, + AS5048RotaryPositionSensor sensor) { m_truth = truth; + m_sensor = sensor; m_sim = new DutyCycleSim(sensor.getDutyCycle()); m_sim.setInitialized(true); m_sim.setFrequency(1000); @@ -40,9 +46,16 @@ public SimulatedAS5048(DoubleFunction truth, AS5048RotaryPositionSen public void run() { // Sample the ground-truth value in the past. Rotation2d pastValue = m_truth.apply(Takt.actual() - DELAY_S); - // Set the HAL to the duty cycle for this rotation. - m_sim.setOutput(getDutyCycle(pastValue)); + double dutyCycle = getDutyCycle(pastValue); + m_sim.setOutput(dutyCycle); + // this updates immediately (the numbers below match) + if (DEBUG) { + System.out.printf("SimulatedAS5048 pastValue %s\n", pastValue); + double sensorDutyCycle = m_sensor.getDutyCycle().getOutput(); + System.out.printf("desired duty %f actual %f\n", + dutyCycle, sensorDutyCycle); + } } /** Match the real sensor squashing. */ @@ -52,7 +65,7 @@ static double getDutyCycle(Rotation2d r) { // This value is between 0 and 1. double rot01 = (rotations + 1) % 1; - + // Squash the value into the real sensor range. return rot01 * SENSOR_RANGE + SENSOR_MIN; } diff --git a/studies/camera_delay/src/test/java/org/team100/frc2026/SimulatedGroundTruthTest.java b/studies/camera_delay/src/test/java/org/team100/frc2026/SimulatedGroundTruthTest.java deleted file mode 100644 index e3de0fe..0000000 --- a/studies/camera_delay/src/test/java/org/team100/frc2026/SimulatedGroundTruthTest.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.team100.frc2026; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.TestLoggerFactory; -import org.team100.lib.logging.primitive.TestPrimitiveLogger; - -public class SimulatedGroundTruthTest { - private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); - - @Test - void testStartStop() { - SimulatedGroundTruth t = new SimulatedGroundTruth(log); - - // Initial position is zero. - assertEquals(0.00, t.apply(0.00).getRadians(), 0.001); - - // Position does not change with time. - assertEquals(0.00, t.apply(0.02).getRadians(), 0.001); - - // Start movement at t=0.02. - t.start(0.02); - - // Now position changes with time. - assertEquals(0.02, t.apply(0.04).getRadians(), 0.001); - assertEquals(0.04, t.apply(0.06).getRadians(), 0.001); - - // Stop movement at t=0.06 (position as above, 0.04). - t.stop(0.06); - - // Position does not change with time. - assertEquals(0.04, t.apply(0.08).getRadians(), 0.001); - } - -}