From 840b901506b81792634e37c336c31077413e7bed Mon Sep 17 00:00:00 2001 From: Eric Ratliff Date: Wed, 4 Feb 2026 00:17:18 -0600 Subject: [PATCH] Pot range is now dynamic --- .../ftc/teamcode/opmodes/ChuteOpMode.java | 6 ++- .../ftc/teamcode/subsystems/chute/Chute.java | 14 ++++++- .../subsystems/chute/ChuteController.java | 23 +++++++--- .../subsystems/chute/FtcPotentiometer.java | 17 ++++++-- .../subsystems/chute/MockPotentiometer.java | 25 +++++++++-- .../teamcode/subsystems/chute/ChuteTest.java | 42 +++++++++++++++++++ 6 files changed, 110 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java b/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java index 9da3c99..09476bd 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java @@ -22,6 +22,9 @@ public class ChuteOpMode extends LinearOpMode { private Chute chute; + // Pot configuration - change this if your pot has different range + private static final double POT_WRAP_AMOUNT = 2 * Math.PI; + // Position presets (in radians) private static final double HOME = 0.0; private static final double LOW = Math.PI; @@ -37,13 +40,14 @@ public class ChuteOpMode extends LinearOpMode { // Create hardware adapters FtcMotor motor = new FtcMotor(chuteMotor); - FtcPotentiometer pot = new FtcPotentiometer(chutePot); + FtcPotentiometer pot = new FtcPotentiometer(chutePot, POT_WRAP_AMOUNT); // Create chute controller with real hardware ChuteController controller = new ChuteController(motor, pot, MAX); chute = new Chute(controller, motor, pot); telemetry.addLine("Chute initialized"); + telemetry.addData("Pot wrap", "%.2f rad", POT_WRAP_AMOUNT); telemetry.addLine("Press A to home"); telemetry.update(); diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java index abb9376..71bb35b 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java @@ -9,11 +9,21 @@ public class Chute { private final MockPotentiometer pot; /** - * Create chute with mock hardware (for testing). + * Create chute with mock hardware (for testing) - default 2pi wraparound. + * @param maxExtension Maximum extension in radians */ public Chute(double maxExtension) { + this(maxExtension, 2 * Math.PI); + } + + /** + * Create chute with mock hardware (for testing) - custom wraparound. + * @param maxExtension Maximum extension in radians + * @param wrapAmount Potentiometer wrap amount in radians + */ + public Chute(double maxExtension, double wrapAmount) { this.motor = new MockMotor(); - this.pot = new MockPotentiometer(); + this.pot = new MockPotentiometer(wrapAmount); this.controller = new ChuteController(motor, pot, maxExtension); } diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteController.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteController.java index 7d51870..ad5ff06 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteController.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteController.java @@ -7,6 +7,7 @@ public class ChuteController { private final MockMotor motor; private final MockPotentiometer pot; private final double maxExtension; + private final double wrapAmount; private boolean homed = false; private boolean homing = false; @@ -32,6 +33,7 @@ public class ChuteController { this.motor = motor; this.pot = pot; this.maxExtension = maxExtension; + this.wrapAmount = pot.getWrapAmount(); } /** @@ -129,13 +131,14 @@ public class ChuteController { double delta = voltage - lastVoltage; // Detect wraps and accumulate to unwrapped voltage - if (Math.abs(delta) > Math.PI) { + // Threshold is half the wrap amount + if (Math.abs(delta) > wrapAmount / 2) { if (delta < 0) { - // Forward wrap: voltage dropped, add back 2pi - unwrappedVoltage += (delta + 2 * Math.PI); + // Forward wrap: voltage dropped, add back wrap amount + unwrappedVoltage += (delta + wrapAmount); } else { - // Backward wrap: voltage jumped, subtract 2pi - unwrappedVoltage += (delta - 2 * Math.PI); + // Backward wrap: voltage jumped, subtract wrap amount + unwrappedVoltage += (delta - wrapAmount); } } else { // Normal movement @@ -185,12 +188,20 @@ public class ChuteController { /** * Get the home voltage reference captured during homing. - * @return Home voltage in range [0, 2pi] + * @return Home voltage in range [0, wrapAmount] */ public double getHomeVoltage() { return homeVoltage; } + /** + * Get the pot wraparound amount. + * @return Wrap amount in radians + */ + public double getWrapAmount() { + return wrapAmount; + } + /** * Emergency stop - immediately stops motor and cancels movement. */ diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcPotentiometer.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcPotentiometer.java index 289162f..d050342 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcPotentiometer.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcPotentiometer.java @@ -11,11 +11,20 @@ public class FtcPotentiometer extends MockPotentiometer { private final double maxVoltage; /** - * Create potentiometer adapter. + * Create potentiometer adapter with default 2pi wraparound. * @param pot The real FTC analog input from hardwareMap */ public FtcPotentiometer(AnalogInput pot) { - super(); + this(pot, 2 * Math.PI); + } + + /** + * Create potentiometer adapter with custom wraparound. + * @param pot The real FTC analog input from hardwareMap + * @param wrapAmount Voltage value at which pot wraps (in radians) + */ + public FtcPotentiometer(AnalogInput pot, double wrapAmount) { + super(wrapAmount); this.pot = pot; this.maxVoltage = pot.getMaxVoltage(); // Usually 3.3V } @@ -25,9 +34,9 @@ public class FtcPotentiometer extends MockPotentiometer { // Read actual voltage from hardware double rawVoltage = pot.getVoltage(); - // Scale to [0, 2pi] range + // Scale to [0, wrapAmount] range // Assumes pot uses full voltage range (0 to maxVoltage) - return (rawVoltage / maxVoltage) * 2 * Math.PI; + return (rawVoltage / maxVoltage) * getWrapAmount(); } @Override diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockPotentiometer.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockPotentiometer.java index ff719fa..a86dd2e 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockPotentiometer.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockPotentiometer.java @@ -1,19 +1,32 @@ package org.firstinspires.ftc.teamcode.subsystems.chute; /** - * Simple mock potentiometer with wraparound at 2pi. + * Simple mock potentiometer with configurable wraparound. */ public class MockPotentiometer { private double position; + private final double wrapAmount; + /** + * Create potentiometer with default 2pi wraparound. + */ public MockPotentiometer() { + this(2 * Math.PI); + } + + /** + * Create potentiometer with custom wraparound amount. + * @param wrapAmount Voltage at which pot wraps back to 0 + */ + public MockPotentiometer(double wrapAmount) { this.position = 0.0; + this.wrapAmount = wrapAmount; } public double getVoltage() { - // Wrap to [0, 2pi] - double wrapped = position % (2 * Math.PI); - if (wrapped < 0) wrapped += 2 * Math.PI; + // Wrap to [0, wrapAmount] + double wrapped = position % wrapAmount; + if (wrapped < 0) wrapped += wrapAmount; return wrapped; } @@ -24,4 +37,8 @@ public class MockPotentiometer { public void updatePosition(double delta) { this.position += delta; } + + public double getWrapAmount() { + return wrapAmount; + } } \ No newline at end of file diff --git a/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java b/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java index 09cc824..8d0de7a 100644 --- a/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java +++ b/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java @@ -105,6 +105,8 @@ public class ChuteTest { chute.getMotor().setPosition(0.2); chute.getPot().setPosition(0.2); chute.home(); + + // Wait for homing to complete (200 iterations x 0.02s = 4 second timeout) for (int i = 0; i < 200; i++) { chute.update(DT); if (chute.isHomed()) break; @@ -226,4 +228,44 @@ public class ChuteTest { assertTrue(downPos < 1.5, "Should move back down"); assertTrue(downPos < upPos, "Down position should be less than up"); } + + /** + * Test custom wraparound amount (3.0 radians instead of 2pi). + * Verifies controller correctly handles non-standard pot configurations. + */ + @Test + public void testCustomWrapAmount() { + // Create chute with 3.0 radian wraparound (instead of default 2pi) + Chute chute = new Chute(20.0, 3.0); + + // Home + chute.getMotor().setPosition(0.2); + chute.getPot().setPosition(0.2); + chute.home(); + for (int i = 0; i < 200; i++) { + chute.update(DT); + if (chute.isHomed()) break; + } + + assertTrue(chute.isHomed(), "Should be homed"); + + // Move to 2.5 wraps (2.5 * 3.0 = 7.5 radians) + double target = 2.5 * 3.0; + chute.setTargetPosition(target); + + for (int i = 0; i < 2000; i++) { + chute.update(DT); + if (chute.isAtTarget()) break; + } + + // Should track through wraps correctly + assertTrue(chute.getPosition() > 2.0 * 3.0, + "Position should be > 2 wraps (6.0), got: " + chute.getPosition()); + assertTrue(Math.abs(chute.getPosition() - target) < 1.0, + "Position should be near target, expected: " + target + ", got: " + chute.getPosition()); + + // Verify pot actually wraps at 3.0 + assertEquals(3.0, chute.getPot().getWrapAmount(), EPSILON, + "Pot should wrap at 3.0 radians"); + } } \ No newline at end of file