From c85bc462ea02a72f090a80e3087f9134880bc40c Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Sat, 14 Feb 2026 11:55:59 -0800 Subject: [PATCH] osejfoajoiero --- .../frc/robot/subsystems/turret/Turret.java | 51 +++++++++--------- .../subsystems/turret/TurretConstants.java | 11 ++-- .../robot/util/ChineseRemainderTheorem.java | 52 +++++++++++++++++++ src/main/java/frc/robot/util/Remainders.java | 25 --------- ....java => ChineseRemainderTheorumTest.java} | 6 +-- 5 files changed, 87 insertions(+), 58 deletions(-) create mode 100644 src/main/java/frc/robot/util/ChineseRemainderTheorem.java delete mode 100644 src/main/java/frc/robot/util/Remainders.java rename src/test/java/frc/robot/util/{RemainderTest.java => ChineseRemainderTheorumTest.java} (74%) 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/RemainderTest.java b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java similarity index 74% rename from src/test/java/frc/robot/util/RemainderTest.java rename to src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java index 52cbf01..932f5f5 100644 --- a/src/test/java/frc/robot/util/RemainderTest.java +++ b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java @@ -7,9 +7,9 @@ import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import frc.robot.util.Remainders.Encoder; +import frc.robot.util.ChineseRemainderTheorum.Encoder; -public class RemainderTest { +public class ChineseRemainderTheorumTest { @BeforeEach public void prepare() { @@ -25,7 +25,7 @@ public class RemainderTest { Encoder a = new Encoder(5000 % 123, 123); Encoder b = new Encoder(5000 % 321, 321); - double val = Remainders.compute(a, b, tolerance); + double val = ChineseRemainderTheorum.compute(a, b, tolerance); assertEquals(5000, val, tolerance); } } -- 2.39.5