From cdae7a25ac662921189737c70343c2bdd2a22701 Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Thu, 26 Feb 2026 16:31:58 -0800 Subject: [PATCH] I made CRT work horray --- .../frc/robot/subsystems/turret/Turret.java | 27 +++---- .../subsystems/turret/TurretConstants.java | 22 +++++- .../robot/util/ChineseRemainderTheorem.java | 3 +- src/main/java/frc/robot/util/ModifiedCRT.java | 73 +++++++++++++++++++ 4 files changed, 105 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/util/ModifiedCRT.java diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3afbfab..057224c 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; import frc.robot.util.ChineseRemainderTheorem; +import frc.robot.util.ModifiedCRT; public class Turret extends SubsystemBase implements TurretIO{ // Super low magnitude filter for the position to make it less jittery @@ -98,6 +99,7 @@ public class Turret extends SubsystemBase implements TurretIO{ false, 0.0); } + SmartDashboard.putData("Turret Mech", mech); SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); @@ -108,24 +110,14 @@ public class Turret extends SubsystemBase implements TurretIO{ 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); + ModifiedCRT crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT); - int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH); + double turretRot = crt.solve(leftAbs, rightAbs); //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)); + SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot)); - double motorRotations = turretRotations * TurretConstants.GEAR_RATIO; + double motorRotations = turretRot * TurretConstants.GEAR_RATIO; //Sets the initial motor position motor.setPosition(motorRotations); @@ -135,7 +127,6 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);})); SmartDashboard.putData("Turn to 200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0);})); SmartDashboard.putData("Turn to -200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0);})); - } /* ---------------- Public API ---------------- */ @@ -263,11 +254,11 @@ public class Turret extends SubsystemBase implements TurretIO{ @Override public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; + // inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO; inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.encoderLeftRot = encoderLeft.getAbsolutePosition().getValueAsDouble(); - inputs.encoderRightRot = encoderRight.getAbsolutePosition().getValueAsDouble(); + inputs.encoderLeftRot = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble()); + inputs.encoderRightRot = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble()); inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 9b3dd6b..70058a9 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -16,7 +16,7 @@ public class TurretConstants { public static double TURRET_WIDTH = Units.inchesToMeters(6.4); public static double TURRET_RADIUS = TURRET_WIDTH / 2; - public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count + public static int TURRET_TEETH_COUNT = 140; // the turret teeth count 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 @@ -39,3 +39,23 @@ public class TurretConstants { public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A } +/* +turret is 140 +left encoder is 15 +right encoder is 22 + +turret cannot go beyond 15 * 22 teeth (330) (2.3 rotations) +2.3 * 360 = 828 deg (range of values we can have in degrees) + +picking 532 deg + +e_1_val = 532 * (140/15) % 360 = 285.33_ +e_2_val = 532 * (140/22) % 360 = 145.45_45 + +(n + (E/360))Gr = A +n = number of possible countable rot +array only needs to be 0 - less than the teeth count of the other gear. + + + +*/ diff --git a/src/main/java/frc/robot/util/ChineseRemainderTheorem.java b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java index 571beb7..fd8aef2 100644 --- a/src/main/java/frc/robot/util/ChineseRemainderTheorem.java +++ b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java @@ -13,7 +13,8 @@ public final class ChineseRemainderTheorem { * * Returns x in range [0, n1*n2) */ - public static int solve(int a, int n1, int b, int 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."); } diff --git a/src/main/java/frc/robot/util/ModifiedCRT.java b/src/main/java/frc/robot/util/ModifiedCRT.java new file mode 100644 index 0000000..e0897c3 --- /dev/null +++ b/src/main/java/frc/robot/util/ModifiedCRT.java @@ -0,0 +1,73 @@ +package frc.robot.util; + +public class ModifiedCRT { + private int gearOne; + private int gearTwo; + private int turretGear; + + public ModifiedCRT(int gearOne, int gearTwo, int turretGear) { + this.gearOne = gearOne; + this.gearTwo = gearTwo; + this.turretGear = turretGear; + } + + public double bruteForce(double encoderLeftRot, double encoderRightRot) { + double[] encoderLeft = new double[gearOne]; + double[] encoderRight = new double[gearTwo]; + + // Adds all possible positons for encoder left + for (int n=0; n < gearOne; n++) { + encoderLeft[n] = (n+encoderLeftRot) * (gearOne/turretGear); + } + // Gets all possible encoder two positions + for (int n=0; n < gearTwo; n++) { + encoderRight[n] = (n+encoderRightRot) * (gearTwo/turretGear); + } + + for (double a: encoderLeft) { + for (double b: encoderRight) { + if (a==b) { + return a; + } + } + } + return 0.0; + } + + private long modInverse(long a, long m) { + long m0 = m, t, q; + long x0 = 0, x1 = 1; + if (m==1) return 0; + while (a > 1) { + q = a / m; + t = m; + m = a % m; + a = t; + + t = x0; + x0 = x1 - q * x0; + x1 = t; + } + + if (x1 < 0) { + x1 =+ m0; + } + return x1; + } + + public double solve(double encoderLeftRot, double encoderRightRot) { + double r1 = encoderLeftRot * gearOne; + double r2 = encoderRightRot * gearTwo; + + long m1 = gearOne; + long m2 = gearTwo; + + long inv = modInverse(m1 % m2, m2); + + double x = r1 + m1 * (((r2-r1) * inv) % m2); + double combined = x % (m1 * m2); + + return combined / turretGear; + } + +} -- 2.39.5