From 4858699becdc8b1bd03ca81967910840930f657d Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Fri, 27 Mar 2026 08:26:40 -0700 Subject: [PATCH] Constant Changes, remove CRT --- .../frc/robot/constants/VisionConstants.java | 8 ++-- .../frc/robot/subsystems/turret/Turret.java | 37 ------------------- .../subsystems/turret/TurretConstants.java | 16 ++------ 3 files changed, 7 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 6a48023..f235169 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -157,7 +157,7 @@ public class VisionConstants { new Transform3d( new Translation3d(Units.inchesToMeters(-8.47), Units.inchesToMeters(11.54), - Units.inchesToMeters(19.7)), + Units.inchesToMeters(18.7)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(55.0)))), new Pair( @@ -165,7 +165,7 @@ public class VisionConstants { new Transform3d( new Translation3d(Units.inchesToMeters(-8.47), Units.inchesToMeters(-11.54), - Units.inchesToMeters(19.7)), + Units.inchesToMeters(18.7)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-55.0)))), new Pair( @@ -173,7 +173,7 @@ public class VisionConstants { new Transform3d( new Translation3d(Units.inchesToMeters(-10.91), Units.inchesToMeters(12), - Units.inchesToMeters(19.66)), + Units.inchesToMeters(18.66)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(145.0)))), new Pair( @@ -181,7 +181,7 @@ public class VisionConstants { new Transform3d( new Translation3d(Units.inchesToMeters(-10.91), Units.inchesToMeters(-12), - Units.inchesToMeters(19.66)), + Units.inchesToMeters(18.66)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-145.0)))) )); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index f77aa82..0b6f00d 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -115,23 +115,6 @@ public class Turret extends SubsystemBase implements TurretIO{ turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); SmartDashboard.putData("Turret Test Positions", turretTestChooser); - - double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble(); - double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET); - - double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble(); - double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET); - - crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT); - - double turretRot = crt.solve(leftAbs, rightAbs); - - double motorRotations = turretRot * TurretConstants.GEAR_RATIO; - - //Sets the initial motor position - inputs.positionDeg = turretRot; - motor.setPosition(motorRotations); - //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); motor.setPosition(0.0); @@ -245,19 +228,6 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble()); - - double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble(); - double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET); - - double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble(); - double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET); - - crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT); - - double turretRot = crt.solve(leftAbs, rightAbs); - - SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot)); - SmartDashboard.putBoolean("Turret Calibrated", !calibrating); SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint()); } @@ -324,11 +294,4 @@ public class Turret extends SubsystemBase implements TurretIO{ calibrating = false; setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0); } - - // in rotations - public Pair getEncoderPositions() { - return new Pair( - encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET, - encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET); - } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 69486c3..5dac346 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -4,8 +4,8 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; public class TurretConstants { - public static double MAX_ANGLE = 200; // Deg - public static double MIN_ANGLE = -200; // Deg + public static double MAX_ANGLE = 220; // Deg + public static double MIN_ANGLE = -220; // Deg public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop @@ -17,21 +17,11 @@ public class TurretConstants { public static double TURRET_WIDTH = Units.inchesToMeters(6.4); public static double TURRET_RADIUS = TURRET_WIDTH / 2; - 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 int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me - - // TODO: set these properly - public static double LEFT_ENCODER_OFFSET = 0.364502; // rot - public static double RIGHT_ENCODER_OFFSET = 0.718506; // rot + public static double GEAR_RATIO = 28; // Turret is in center of robot, but make use of the height in shooter physics public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters - public static double CRT_TOLERANCE = 0.01; - public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; public static final double FEEDFORWARD_KV = 0.185; -- 2.39.5