Move thing to periodic to test.

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index fac3856aa1b069608f1d0af2e624caa921cce7f9..8f549255a8a3c41c3863a2140f6971b852d4c719 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -13,7 +13,6 @@ import com.ctre.phoenix6.sim.TalonFXSimState;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
@@ -106,34 +105,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                                        false,
                                        0.0);
                }
-               // 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 totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
-        * TurretConstants.RIGHT_ENCODER_TEETH;
-
-               double turretRotations = turretIndex / (double) 140.0;
-
-               double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
-               motor.setPosition(motorRotations);
-
                SmartDashboard.putData("Turret Mech", mech);
 
        }
@@ -162,6 +133,36 @@ public class Turret extends SubsystemBase implements TurretIO{
        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 totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
+        * TurretConstants.RIGHT_ENCODER_TEETH;
+
+               double turretRotations = turretIndex / (double) 140.0;
+               SmartDashboard.putNumber("CRT thing out", Units.rotationsToDegrees(turretRotations));
+
+               double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
+               // motor.setPosition(motorRotations);
+
                
                // Position extrapolation
                double lookAheadSeconds = EXTRAPOLATION_TIME_CONSTANT; 

diff --git a/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java
deleted file mode 100644 (file)
index 932f5f5..0000000
--- a/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java
+++ /dev/null
@@ -1,31 +0,0 @@
-
-package frc.robot.util;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import frc.robot.util.ChineseRemainderTheorum.Encoder;
-
-public class ChineseRemainderTheorumTest {
-
-       @BeforeEach
-       public void prepare() {
-       }
-
-       @AfterEach
-       public void cleanup() {
-       }
-
-       @Test
-       public void test() {
-               double tolerance = 0.01;
-
-               Encoder a = new Encoder(5000 % 123, 123);
-               Encoder b = new Encoder(5000 % 321, 321);
-               double val = ChineseRemainderTheorum.compute(a, b, tolerance);
-               assertEquals(5000, val, tolerance);
-       }
-}