Merge branch 'Wesleys-chinese-remainder-theorem' into intake-pull-request

diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java
index c9cffb0e82d7688baeb6bc40f818425b37202585,1647999d295069f6e51e8a7629c0cee0763dafec..8a806e5c6371619ab394b6e2f73699ccd68810af
--- 1/src/main/java/frc/robot/subsystems/turret/Turret.java
--- 2/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@@ -155,9 -133,40 +131,40 @@@ public class Turret extends SubsystemBa
        public void periodic() {
                updateInputs();
                Logger.processInputs("Turret", inputs);
-               
+ 
+               // 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)
+                               % TurretConstants.LEFT_ENCODER_TEETH;
+               //SmartDashboard.putNumber("Left Tooth", leftTooth);
+ 
+               int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
+                               % TurretConstants.RIGHT_ENCODER_TEETH;
+               //SmartDashboard.putNumber("Right Tooth", rightTooth);
+ 
+               int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
+               //SmartDashboard.putNumber("Turret Index", turretIndex);
+ 
+               double turretRotations = turretIndex / (double) TurretConstants.TURRET_TEETH_COUNT;
+               if(Units.rotationsToDegrees(turretRotations) > 500.0){
+                       turretRotations -= Units.degreesToRotations(846.0);
+               }
+               SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRotations));
+ 
+               double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
+ 
+               //Sets the initial motor position
+               motor.setPosition(motorRotations);
+ 
                // Position extrapolation
 -              double lookAheadSeconds = EXTRAPOLATION_TIME_CONSTANT; 
 +              double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; 
        double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);
  
                //Continuous wrap selection

diff --cc src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index ca6f24fe7d0925caa9ad66b03616649e402d1303,e56cfec97992692b05066b9d6960cbf9d899bf83..f3b6a518ad2e89f9e265f25fb9ba74ae40a3a9bc
--- 1/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
--- 2/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@@ -20,10 -19,9 +20,10 @@@ public class TurretConstants 
      public static int RIGHT_ENCODER_TEETH = 22; // read above
      public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me
  
-     public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot
-     public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot
+     public static double LEFT_ENCODER_OFFSET = 0.463379; //  rot
+     public static double RIGHT_ENCODER_OFFSET = 0.197266; //  rot
  
 +    // Turret is in center of robot, but make use of the height in shooter physics
      public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
  
      public static double CRT_TOLERANCE = 0.01;