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
false,
0.0);
}
+
SmartDashboard.putData("Turret Mech", mech);
SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate()));
SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating()));
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);
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 ---------------- */
@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();
}
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
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.
+
+
+
+*/
--- /dev/null
+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;
+ }
+
+}