Set pot wrapping at 2pi instead of pi

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;

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

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;
     }
     

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());
     }