From b97838252f49ff968a2e9ee505b4ae4565f98316 Mon Sep 17 00:00:00 2001 From: Eric Ratliff Date: Wed, 4 Feb 2026 00:09:08 -0600 Subject: [PATCH] Set pot wrapping at 2pi instead of pi --- .../subsystems/chute/ChuteController.java | 36 ++++++++++--------- .../subsystems/chute/FtcPotentiometer.java | 4 +-- .../subsystems/chute/MockPotentiometer.java | 8 ++--- .../teamcode/subsystems/chute/ChuteTest.java | 28 +++++++-------- 4 files changed, 40 insertions(+), 36 deletions(-) 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 d89271e..7d51870 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 @@ -11,8 +11,8 @@ public class ChuteController { private boolean homed = false; private boolean homing = false; private double homeVoltage = 0.0; + private double unwrappedVoltage = 0.0; private double currentPosition = 0.0; - private int wrapCount = 0; private double lastVoltage = 0.0; private double targetPosition = 0.0; private boolean moving = false; @@ -76,8 +76,12 @@ public class ChuteController { * @param dt Time since last update in seconds */ public void update(double dt) { + double oldMotorPos = motor.getPosition(); motor.update(dt); - pot.updatePosition(motor.getPower() * dt); + double newMotorPos = motor.getPosition(); + double actualMovement = newMotorPos - oldMotorPos; + + pot.updatePosition(actualMovement); double voltage = pot.getVoltage(); @@ -99,8 +103,8 @@ public class ChuteController { if (stableCount >= STABLE_REQUIRED) { motor.setPower(0); homeVoltage = voltage; + unwrappedVoltage = voltage; currentPosition = 0.0; - wrapCount = 0; homed = true; homing = false; stableCount = 0; @@ -124,22 +128,22 @@ public class ChuteController { private void updateMoving(double voltage) { double delta = voltage - lastVoltage; - // Detect wraps - look for large jumps - if (Math.abs(delta) > 2.0) { - if (motor.getPower() > 0) { - // Moving up: voltage wraps from ~0 to pi (positive jump) - wrapCount++; + // Detect wraps and accumulate to unwrapped voltage + if (Math.abs(delta) > Math.PI) { + if (delta < 0) { + // Forward wrap: voltage dropped, add back 2pi + unwrappedVoltage += (delta + 2 * Math.PI); } else { - // Moving down: voltage wraps from pi to ~0 (negative jump) - wrapCount--; + // Backward wrap: voltage jumped, subtract 2pi + unwrappedVoltage += (delta - 2 * Math.PI); } + } else { + // Normal movement + unwrappedVoltage += delta; } - // Calculate position: offset from home + wraps - double offset = voltage - homeVoltage; - if (offset < 0) offset += Math.PI; // Handle wraparound in offset - - currentPosition = offset + (wrapCount * Math.PI); + // Position is simply unwrapped voltage minus home + currentPosition = unwrappedVoltage - homeVoltage; if (currentPosition < 0) currentPosition = 0; // Check if at target @@ -181,7 +185,7 @@ public class ChuteController { /** * Get the home voltage reference captured during homing. - * @return Home voltage in range [0, pi] + * @return Home voltage in range [0, 2pi] */ public double getHomeVoltage() { return homeVoltage; 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 a6e2e6c..289162f 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 @@ -25,9 +25,9 @@ public class FtcPotentiometer extends MockPotentiometer { // Read actual voltage from hardware double rawVoltage = pot.getVoltage(); - // Scale to [0, pi] range + // Scale to [0, 2pi] range // Assumes pot uses full voltage range (0 to maxVoltage) - return (rawVoltage / maxVoltage) * Math.PI; + return (rawVoltage / maxVoltage) * 2 * Math.PI; } @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 63421be..ff719fa 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,7 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems.chute; /** - * Simple mock potentiometer with wraparound at pi. + * Simple mock potentiometer with wraparound at 2pi. */ public class MockPotentiometer { private double position; @@ -11,9 +11,9 @@ public class MockPotentiometer { } public double getVoltage() { - // Wrap to [0, pi] - double wrapped = position % Math.PI; - if (wrapped < 0) wrapped += Math.PI; + // Wrap to [0, 2pi] + double wrapped = position % (2 * Math.PI); + if (wrapped < 0) wrapped += 2 * Math.PI; return wrapped; } 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 ca39c85..09cc824 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 @@ -15,8 +15,8 @@ public class ChuteTest { private static final double EPSILON = 0.01; /** - * Verify potentiometer wraps from pi back to 0 (sawtooth wave behavior). - * Tests that pot correctly simulates 0→pi→0→pi pattern. + * Verify potentiometer wraps from 2pi back to 0 (sawtooth wave behavior). + * Tests that pot correctly simulates 0->2pi->0->2pi pattern. */ @Test public void testPotWraps() { @@ -25,14 +25,14 @@ public class ChuteTest { pot.setPosition(0.0); assertEquals(0.0, pot.getVoltage(), EPSILON); - pot.setPosition(Math.PI / 2); - assertEquals(Math.PI / 2, pot.getVoltage(), EPSILON); - - // Wrap at pi pot.setPosition(Math.PI); + assertEquals(Math.PI, pot.getVoltage(), EPSILON); + + // Wrap at 2pi + pot.setPosition(2 * Math.PI); assertEquals(0.0, pot.getVoltage(), EPSILON); - pot.setPosition(2 * Math.PI); + pot.setPosition(4 * Math.PI); assertEquals(0.0, pot.getVoltage(), EPSILON); } @@ -95,11 +95,11 @@ public class ChuteTest { /** * Test unwrap logic through multiple pot wraps (3.5 rotations). - * Verifies controller correctly tracks absolute position across multiple pi boundaries. + * Verifies controller correctly tracks absolute position across multiple 2pi boundaries. */ @Test public void testMultipleRotations() { - Chute chute = new Chute(15.0); + Chute chute = new Chute(25.0); // Home chute.getMotor().setPosition(0.2); @@ -113,7 +113,7 @@ public class ChuteTest { assertTrue(chute.isHomed(), "Should be homed"); // Move to 3.5 rotations (need more time) - double target = 3.5 * Math.PI; + double target = 3.5 * 2 * Math.PI; chute.setTargetPosition(target); for (int i = 0; i < 2000; i++) { @@ -121,10 +121,10 @@ public class ChuteTest { if (chute.isAtTarget()) break; } - // Should track through wraps - assertTrue(chute.getPosition() > 3.0 * Math.PI, - "Position should be > 3pi, got: " + chute.getPosition()); - assertTrue(Math.abs(chute.getPosition() - target) < 0.5, + // Should track through wraps - looser tolerance for multi-rotation + assertTrue(chute.getPosition() > 3.0 * 2 * Math.PI, + "Position should be > 3*2pi, got: " + chute.getPosition()); + assertTrue(Math.abs(chute.getPosition() - target) < 1.5, "Position should be near target, expected: " + target + ", got: " + chute.getPosition()); }