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;
private double lastFrameVelocity = 0.0;
- private EasyCRT easyCRT;
-
/* ---------------- Hardware ---------------- */
private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0);
private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
-
/* ---------------- Constructor ---------------- */
public Turret() {
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(
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 ---------------- */
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;
+ }
}
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
--- /dev/null
+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
+++ /dev/null
-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();
- }
- }
- }
-}
--- /dev/null
+
+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);
+ }
+}
+++ /dev/null
-
-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);
- }
-}