From: Wesley28w Date: Sat, 14 Feb 2026 19:55:59 +0000 (-0800) Subject: osejfoajoiero X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=c85bc462ea02a72f090a80e3087f9134880bc40c;p=FRC2026.git osejfoajoiero --- diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index b6e2ecc..48bc6ac 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; import frc.robot.constants.swerve.DriveConstants; +import frc.robot.util.ChineseRemainderTheorem; import yams.units.EasyCRT; import yams.units.EasyCRTConfig; import static edu.wpi.first.units.Units.Rotations; @@ -68,8 +69,6 @@ public class Turret extends SubsystemBase implements TurretIO{ private double lastFrameVelocity = 0.0; - private EasyCRT easyCRT; - /* ---------------- Hardware ---------------- */ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN); @@ -108,7 +107,6 @@ public class Turret extends SubsystemBase implements TurretIO{ private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); - /* ---------------- Constructor ---------------- */ public Turret() { @@ -131,27 +129,6 @@ public class Turret extends SubsystemBase implements TurretIO{ setpoint = new State(getPositionRad(), 0.0); lastGoalRad = setpoint.position; - EasyCRTConfig crt_cfg = new EasyCRTConfig(() -> Rotations.of(encoderLeft.getAbsolutePosition().getValueAsDouble()), () -> Rotations.of(encoderRight.getAbsolutePosition().getValueAsDouble())) - //.withEncoderRatios(TurretConstants.LEFT_ENCODER_RATIO, TurretConstants.RIGHT_ENCODER_RATIO) - .withCommonDriveGear(1.0, 140, 15, 22) // 1:1 from 140t to 15t (left) to 22t (right) - .withAbsoluteEncoderOffsets(Degrees.of(TurretConstants.LEFT_ENCODER_OFFSET), Degrees.of(TurretConstants.RIGHT_ENCODER_OFFSET)) - .withMechanismRange(Degrees.of(TurretConstants.MIN_ANGLE), Degrees.of(TurretConstants.MAX_ANGLE)) - .withMatchTolerance(Degrees.of(3)) // Tolerance of 3 degrees - .withAbsoluteEncoderInversions(false, false); - // shared drive gear / pinion (the tan gear/tiny first gear on motor shaft) - // turret ring / shared drive pulley (big turret gear / first black belt gear weird thingy?) - //.withCrtGearRecommendationInputs(10, 140.0/22.0); // prob need to fix - - this.easyCRT = new EasyCRT(crt_cfg); - - this.easyCRT.getAngleOptional().ifPresent(angle -> { - motor.setPosition(angle.in(Rotations) * GEAR_RATIO); - }); - - //motor.setPosition(motor.getPosition().getValueAsDouble() - Units.degreesToRotations(50.6) * GEAR_RATIO); - - motor.setPosition(0.0); // Delete this when easy crt is working - if (RobotBase.isSimulation()) { simState = motor.getSimState(); turretSim = new SingleJointedArmSim( @@ -165,11 +142,29 @@ public class Turret extends SubsystemBase implements TurretIO{ 0.0); } + double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET); + double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET); + + int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH) + % TurretConstants.LEFT_ENCODER_TEETH; + + int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH) + % TurretConstants.RIGHT_ENCODER_TEETH; + + int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH); + + double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH + * TurretConstants.RIGHT_ENCODER_TEETH; + + double turretRotations = turretIndex / (double) totalTeeth; + + double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO; + motor.setPosition(motorRotations); + SmartDashboard.putData("Turret Mech", mech); SmartDashboard.putData("Turret to 90", new InstantCommand(()-> setFieldRelativeTarget(new Rotation2d(Math.PI/2), 0.0))); SmartDashboard.putData("Turret to -90", new InstantCommand(()-> setFieldRelativeTarget(new Rotation2d(-Math.PI/2), 0.0))); - } /* ---------------- Public API ---------------- */ @@ -290,4 +285,10 @@ public class Turret extends SubsystemBase implements TurretIO{ inputs.encoderLeftRot = encoderLeft.getAbsolutePosition().getValueAsDouble(); inputs.encoderRightRot = encoderRight.getAbsolutePosition().getValueAsDouble(); } + + private double wrapUnit(double value) { + value %= 1.0; + if (value < 0) value += 1.0; + return value; + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index d5d3868..10fee61 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -15,13 +15,14 @@ public class TurretConstants { public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count public static double TURRET_GEAR_RATIO = 36.81818182; - public static double LEFT_ENCODER_RATIO = 70.0/11.0; // read right description - public static double RIGHT_ENCODER_RATIO = 28.0/3.0; // The amount of times this encoder turns for every time the turret turns - public static double ENCODER_COUNT_TOTAL = 8192.0; // how many intervals it can have, like clicks on a clock chat gpt explained to me + 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 - public static double LEFT_ENCODER_OFFSET = Units.rotationsToDegrees(9.52); // degrees 9.52 rot - public static double RIGHT_ENCODER_OFFSET = Units.rotationsToDegrees(6.53); // degrees 6.53 rot + 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 Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters + public static double CRT_TOLERANCE = 0.01; } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/ChineseRemainderTheorem.java b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java new file mode 100644 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/main/java/frc/robot/util/Remainders.java b/src/main/java/frc/robot/util/Remainders.java deleted file mode 100644 index 0086c8d..0000000 --- a/src/main/java/frc/robot/util/Remainders.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.util; - -public class Remainders { - public record Encoder(double val, double mod) { - } - - public static double compute(Encoder a, Encoder b, double tolerance) { - double testing = a.val(); - while (true) { - if (Math.abs(testing % a.mod() - a.val()) <= tolerance && - Math.abs(testing % b.mod() - b.val()) <= tolerance) - return testing; - - // alternately check positive and negative - if (testing > a.val()) { - double diff = testing - a.val(); - testing -= 2 * diff; - } else { - double diff = a.val() - testing; - testing += 2 * diff; - testing += a.mod(); - } - } - } -} diff --git a/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java new file mode 100644 index 0000000..932f5f5 --- /dev/null +++ b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java @@ -0,0 +1,31 @@ + +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); + } +} diff --git a/src/test/java/frc/robot/util/RemainderTest.java b/src/test/java/frc/robot/util/RemainderTest.java deleted file mode 100644 index 52cbf01..0000000 --- a/src/test/java/frc/robot/util/RemainderTest.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.Remainders.Encoder; - -public class RemainderTest { - - @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 = Remainders.compute(a, b, tolerance); - assertEquals(5000, val, tolerance); - } -}