a

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8a806e5c6371619ab394b6e2f73699ccd68810af..15d7c1f6e7aba21df7696ed170f6879c34f24142 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -90,6 +90,38 @@ 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)
+                               % 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.GEAR_RATIO;
+
+               //Sets the initial motor position
+               motor.setPosition(motorRotations);
+
        }
 
        /* ---------------- Public API ---------------- */
@@ -132,37 +164,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                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 = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; 
        double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index f3b6a518ad2e89f9e265f25fb9ba74ae40a3a9bc..44425c0dc31a5c53143c2528a832e3bffbebf216 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -15,7 +15,7 @@ public class TurretConstants {
     public static double TURRET_RADIUS = TURRET_WIDTH / 2;
 
     public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count
-    public static double TURRET_GEAR_RATIO = 25.454545454;
+    public static double GEAR_RATIO = 25.454545454;
     public static int LEFT_ENCODER_TEETH = 15; // gear teeth
     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