nexus-workshops/ftc-chute-controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Set pot wrapping at 2pi instead of pi

Browse Source
This commit is contained in:
Eric Ratliff
2026-02-04 00:09:08 -06:00
parent a69c2bf718
commit b97838252f
4 changed files with 40 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteController.java
View File

@@ -11,8 +11,8 @@ public class ChuteController {
private boolean homed = false; private boolean homed = false;
private boolean homing = false; private boolean homing = false;
private double homeVoltage = 0.0; private double homeVoltage = 0.0;
private double unwrappedVoltage = 0.0;
private double currentPosition = 0.0; private double currentPosition = 0.0;
private int wrapCount = 0;
private double lastVoltage = 0.0; private double lastVoltage = 0.0;
private double targetPosition = 0.0; private double targetPosition = 0.0;
private boolean moving = false; private boolean moving = false;
@@ -76,8 +76,12 @@ public class ChuteController {
* @param dt Time since last update in seconds * @param dt Time since last update in seconds
*/ */
public void update(double dt) { public void update(double dt) {
double oldMotorPos = motor.getPosition();
motor.update(dt); motor.update(dt);
pot.updatePosition(motor.getPower() * dt); double newMotorPos = motor.getPosition();
double actualMovement = newMotorPos - oldMotorPos;
pot.updatePosition(actualMovement);
double voltage = pot.getVoltage(); double voltage = pot.getVoltage();
@@ -99,8 +103,8 @@ public class ChuteController {
if (stableCount >= STABLE_REQUIRED) { if (stableCount >= STABLE_REQUIRED) {
motor.setPower(0); motor.setPower(0);
homeVoltage = voltage; homeVoltage = voltage;
unwrappedVoltage = voltage;
currentPosition = 0.0; currentPosition = 0.0;
wrapCount = 0;
homed = true; homed = true;
homing = false; homing = false;
stableCount = 0; stableCount = 0;
@@ -124,22 +128,22 @@ public class ChuteController {
private void updateMoving(double voltage) { private void updateMoving(double voltage) {
double delta = voltage - lastVoltage; double delta = voltage - lastVoltage;
// Detect wraps - look for large jumps // Detect wraps and accumulate to unwrapped voltage
if (Math.abs(delta) > 2.0) { if (Math.abs(delta) > Math.PI) {
if (motor.getPower() > 0) { if (delta < 0) {
// Moving up: voltage wraps from ~0 to pi (positive jump) // Forward wrap: voltage dropped, add back 2pi
wrapCount++; unwrappedVoltage += (delta + 2 * Math.PI);
} else { } else {
// Moving down: voltage wraps from pi to ~0 (negative jump) // Backward wrap: voltage jumped, subtract 2pi
wrapCount--; unwrappedVoltage += (delta - 2 * Math.PI);
} }
} else {
// Normal movement
unwrappedVoltage += delta;
} }
// Calculate position: offset from home + wraps // Position is simply unwrapped voltage minus home
double offset = voltage - homeVoltage; currentPosition = unwrappedVoltage - homeVoltage;
if (offset < 0) offset += Math.PI; // Handle wraparound in offset
currentPosition = offset + (wrapCount * Math.PI);
if (currentPosition < 0) currentPosition = 0; if (currentPosition < 0) currentPosition = 0;
// Check if at target // Check if at target
@@ -181,7 +185,7 @@ public class ChuteController {
/** /**
* Get the home voltage reference captured during homing. * 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() { public double getHomeVoltage() {
return homeVoltage; return homeVoltage;

4
src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcPotentiometer.java
View File

@@ -25,9 +25,9 @@ public class FtcPotentiometer extends MockPotentiometer {
// Read actual voltage from hardware // Read actual voltage from hardware
double rawVoltage = pot.getVoltage(); double rawVoltage = pot.getVoltage();
// Scale to [0, pi] range // Scale to [0, 2pi] range
// Assumes pot uses full voltage range (0 to maxVoltage) // Assumes pot uses full voltage range (0 to maxVoltage)
return (rawVoltage / maxVoltage) * Math.PI; return (rawVoltage / maxVoltage) * 2 * Math.PI;
} }
@Override @Override

8
src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockPotentiometer.java
View File

@@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.subsystems.chute; package org.firstinspires.ftc.teamcode.subsystems.chute;
/** /**
* Simple mock potentiometer with wraparound at pi. * Simple mock potentiometer with wraparound at 2pi.
*/ */
public class MockPotentiometer { public class MockPotentiometer {
private double position; private double position;
@@ -11,9 +11,9 @@ public class MockPotentiometer {
} }
public double getVoltage() { public double getVoltage() {
// Wrap to [0, pi] // Wrap to [0, 2pi]
double wrapped = position % Math.PI; double wrapped = position % (2 * Math.PI);
if (wrapped < 0) wrapped += Math.PI; if (wrapped < 0) wrapped += 2 * Math.PI;
return wrapped; return wrapped;
} }

28
src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java
View File

@@ -15,8 +15,8 @@ public class ChuteTest {
private static final double EPSILON = 0.01; private static final double EPSILON = 0.01;
/** /**
* Verify potentiometer wraps from pi back to 0 (sawtooth wave behavior). * Verify potentiometer wraps from 2pi back to 0 (sawtooth wave behavior).
* Tests that pot correctly simulates 0→pi→0→pi pattern. * Tests that pot correctly simulates 0->2pi->0->2pi pattern.
*/ */
@Test @Test
public void testPotWraps() { public void testPotWraps() {
@@ -25,14 +25,14 @@ public class ChuteTest {
pot.setPosition(0.0); pot.setPosition(0.0);
assertEquals(0.0, pot.getVoltage(), EPSILON); 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); pot.setPosition(Math.PI);
assertEquals(Math.PI, pot.getVoltage(), EPSILON);
// Wrap at 2pi
pot.setPosition(2 * Math.PI);
assertEquals(0.0, pot.getVoltage(), EPSILON); assertEquals(0.0, pot.getVoltage(), EPSILON);
pot.setPosition(2 * Math.PI); pot.setPosition(4 * Math.PI);
assertEquals(0.0, pot.getVoltage(), EPSILON); assertEquals(0.0, pot.getVoltage(), EPSILON);
} }
@@ -95,11 +95,11 @@ public class ChuteTest {
/** /**
* Test unwrap logic through multiple pot wraps (3.5 rotations). * 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 @Test
public void testMultipleRotations() { public void testMultipleRotations() {
Chute chute = new Chute(15.0); Chute chute = new Chute(25.0);
// Home // Home
chute.getMotor().setPosition(0.2); chute.getMotor().setPosition(0.2);
@@ -113,7 +113,7 @@ public class ChuteTest {
assertTrue(chute.isHomed(), "Should be homed"); assertTrue(chute.isHomed(), "Should be homed");
// Move to 3.5 rotations (need more time) // Move to 3.5 rotations (need more time)
double target = 3.5 * Math.PI; double target = 3.5 * 2 * Math.PI;
chute.setTargetPosition(target); chute.setTargetPosition(target);
for (int i = 0; i < 2000; i++) { for (int i = 0; i < 2000; i++) {
@@ -121,10 +121,10 @@ public class ChuteTest {
if (chute.isAtTarget()) break; if (chute.isAtTarget()) break;
} }
// Should track through wraps // Should track through wraps - looser tolerance for multi-rotation
assertTrue(chute.getPosition() > 3.0 * Math.PI, assertTrue(chute.getPosition() > 3.0 * 2 * Math.PI,
"Position should be > 3pi, got: " + chute.getPosition()); "Position should be > 3*2pi, got: " + chute.getPosition());
assertTrue(Math.abs(chute.getPosition() - target) < 0.5, assertTrue(Math.abs(chute.getPosition() - target) < 1.5,
"Position should be near target, expected: " + target + ", got: " + chute.getPosition()); "Position should be near target, expected: " + target + ", got: " + chute.getPosition());
} }