a

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 9792119b769f38b6e4636d51c6665f6722d10e68..8ac2b12ad6e86250da6ea8bebe1eb860d2bb3d54 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -91,14 +91,10 @@ public class Turret extends SubsystemBase implements TurretIO{
                }
                SmartDashboard.putData("Turret Mech", mech);
 
-               
-               // Do this for both encoders in the constructor
                double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
-
                double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
 
                double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
-
                double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
 
                int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)

diff --git a/src/main/java/frc/robot/util/ChineseRemainderTheorem.java b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java
new file mode 100644 (file)
index 0000000..571beb7
--- /dev/null
+++ b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java
@@ -0,0 +1,52 @@
+package frc.robot.util;
+
+public final class ChineseRemainderTheorem {
+
+    private ChineseRemainderTheorem() {}
+
+    /**
+     * Computes x such that:
+     *   x ≡ a (mod n1)
+     *   x ≡ b (mod n2)
+     *
+     * n1 and n2 MUST be coprime.
+     *
+     * Returns x in range [0, n1*n2)
+     */
+    public static int solve(int a, int n1, int b, int n2) {
+        if (gcd(n1, n2) != 1) {
+            throw new IllegalArgumentException("Moduli must be coprime for CRT.");
+        }
+
+        int N = n1 * n2;
+
+        int invN1modN2 = modInverse(n1, n2);
+        int invN2modN1 = modInverse(n2, n1);
+
+        int result =
+                (a * n2 * invN2modN1 +
+                 b * n1 * invN1modN2) % N;
+
+        return (result + N) % N;
+    }
+
+    private static int modInverse(int a, int mod) {
+        a = ((a % mod) + mod) % mod;
+
+        for (int x = 1; x < mod; x++) {
+            if ((a * x) % mod == 1) {
+                return x;
+            }
+        }
+        throw new IllegalStateException("No modular inverse exists.");
+    }
+
+    private static int gcd(int a, int b) {
+        while (b != 0) {
+            int t = b;
+            b = a % b;
+            a = t;
+        }
+        return Math.abs(a);
+    }
+}
\ No newline at end of file

diff --git a/src/test/java/frc/robot/util/ChineseRemainderTheorem.java b/src/test/java/frc/robot/util/ChineseRemainderTheorem.java
deleted file mode 100644 (file)
index 60b04c7..0000000
--- a/src/test/java/frc/robot/util/ChineseRemainderTheorem.java
+++ /dev/null
@@ -1,5 +0,0 @@
-package frc.robot.util;
-
-public class ChineseRemainderTheorem {
-    
-}