From 6272b55062130c3822a97f644a4b08c9bb4fb39a Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Fri, 20 Feb 2026 10:36:49 -0800 Subject: [PATCH] Move things to constants. --- .../java/frc/robot/subsystems/hood/Hood.java | 28 ++++------- .../frc/robot/subsystems/turret/Turret.java | 49 +++++++------------ .../subsystems/turret/TurretConstants.java | 8 ++- 3 files changed, 33 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 70ade0d..d116ab4 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -21,14 +21,6 @@ import frc.robot.constants.IdConstants; public class Hood extends SubsystemBase implements HoodIO{ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE); - private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE); - - private double MAX_VEL_RAD_PER_SEC = HoodConstants.MAX_VELOCITY; - private double MAX_ACCEL_RAD_PER_SEC2 = HoodConstants.MAX_ACCELERATION; - - private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; - private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); @@ -52,12 +44,12 @@ public class Hood extends SubsystemBase implements HoodIO{ config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot var mm = config.MotionMagic; - mm.MotionMagicCruiseVelocity = Units.radiansToRotations(MAX_VEL_RAD_PER_SEC) * GEAR_RATIO; - mm.MotionMagicAcceleration = Units.radiansToRotations(MAX_ACCEL_RAD_PER_SEC2) * GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION) * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed motor.getConfigurator().apply(config); - motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); + motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); @@ -85,7 +77,7 @@ public class Hood extends SubsystemBase implements HoodIO{ * @return Position of turret in degrees */ public double getPositionDeg(){ - return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; } @Override @@ -108,10 +100,10 @@ public class Hood extends SubsystemBase implements HoodIO{ setpointRad = lastFilteredRad; // Tells the Kraken to get to this position using 1000Hz profile - double motorGoalRotations = Units.radiansToRotations(setpointRad) * GEAR_RATIO; + double motorGoalRotations = Units.radiansToRotations(setpointRad) * HoodConstants.HOOD_GEAR_RATIO; //Clamp the setpoint rotations - motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(MIN_ANGLE_RAD) * GEAR_RATIO, Units.radiansToRotations(MAX_ANGLE_RAD) * GEAR_RATIO); + motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MIN_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MAX_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO); // Multiply goal velocity by kV double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV; @@ -122,15 +114,15 @@ public class Hood extends SubsystemBase implements HoodIO{ .withFeedForward(velocityCompensation)); Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / GEAR_RATIO); - Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()) / GEAR_RATIO); + Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()) / HoodConstants.HOOD_GEAR_RATIO); } @Override public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO; + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; + inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index bfd0d2a..9bd0ef8 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -25,21 +25,6 @@ import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; public class Turret extends SubsystemBase implements TurretIO{ - - /* ---------------- Constants ---------------- */ - - private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE); - private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE); - - private static final double MAX_VEL_RAD_PER_SEC = TurretConstants.MAX_VELOCITY; - private static final double MAX_ACCEL_RAD_PER_SEC2 = TurretConstants.MAX_ACCELERATION; - - private static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; - - private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO; - - private double FEEDFORWARD_KV = 0.185; - // Super low magnitude filter for the position to make it less jittery private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); @@ -84,8 +69,8 @@ public class Turret extends SubsystemBase implements TurretIO{ config.Slot0.kD = 0.15; // The "Braking" term to stop overshoot var mm = config.MotionMagic; - mm.MotionMagicCruiseVelocity = Units.radiansToRotations(MAX_VEL_RAD_PER_SEC) * GEAR_RATIO; - mm.MotionMagicAcceleration = Units.radiansToRotations(MAX_ACCEL_RAD_PER_SEC2) * GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) * TurretConstants.GEAR_RATIO; // Lowered for belt safety mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed motor.getConfigurator().apply(config); @@ -95,11 +80,11 @@ public class Turret extends SubsystemBase implements TurretIO{ simState = motor.getSimState(); turretSim = new SingleJointedArmSim( edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1), - GEAR_RATIO, + TurretConstants.GEAR_RATIO, 0.01, 0.15, - MIN_ANGLE_RAD, - MAX_ANGLE_RAD, + Units.degreesToRadians(TurretConstants.MIN_ANGLE), + Units.degreesToRadians(TurretConstants.MAX_ANGLE), false, 0.0); } @@ -122,7 +107,7 @@ public class Turret extends SubsystemBase implements TurretIO{ // double turretRotations = turretIndex / (double) totalTeeth; - // double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO; + // double motorRotations = turretRotations * TurretConstants.TURRET_TurretConstant.GEAR_RATIO; // motor.setPosition(motorRotations); motor.setPosition(0.0); //TODO: remove after chinese remainder theorem works @@ -154,14 +139,14 @@ public class Turret extends SubsystemBase implements TurretIO{ * @return Posiiton of the turret in radians */ public double getPositionRad() { - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; } /** * @return Posiiton of the turret in degrees */ public double getPositionDeg() { - return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; } /* ---------------- Periodic ---------------- */ @@ -172,7 +157,7 @@ public class Turret extends SubsystemBase implements TurretIO{ Logger.processInputs("Turret", inputs); // Position extrapolation - double lookAheadSeconds = EXTRAPOLATION_TIME_CONSTANT; + double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds); //Continuous wrap selection @@ -181,7 +166,7 @@ public class Turret extends SubsystemBase implements TurretIO{ for (int i = -2; i <= 2; i++) { double candidate = futureRobotAngle + 2.0 * Math.PI * i; - if (candidate < MIN_ANGLE_RAD || candidate > MAX_ANGLE_RAD) + if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE) || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE)) continue; if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) { @@ -205,13 +190,13 @@ public class Turret extends SubsystemBase implements TurretIO{ best = lastFilteredRad; // Tells the Kraken to get to this position using 1000Hz profile - double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO; + double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO; // Clamp position setpoint to min and max angles - motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO); + motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(180) * TurretConstants.GEAR_RATIO); // Multiply goal velocity by kV - double robotTurnCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV; + double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV; // Sets motor control with feedforward motor.setControl(mmVoltageRequest @@ -237,16 +222,16 @@ public class Turret extends SubsystemBase implements TurretIO{ turretSim.update(Constants.LOOP_TIME); simState.setRawRotorPosition( - Units.radiansToRotations(turretSim.getAngleRads()) * GEAR_RATIO); + Units.radiansToRotations(turretSim.getAngleRads()) * TurretConstants.GEAR_RATIO); simState.setRotorVelocity( - Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * GEAR_RATIO); + Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * TurretConstants.GEAR_RATIO); } @Override public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / 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(); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 4505926..ca6f24f 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -15,7 +15,7 @@ public class TurretConstants { public static double TURRET_RADIUS = TURRET_WIDTH / 2; public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count - public static double TURRET_GEAR_RATIO = 36.81818182; + public static double GEAR_RATIO = 36.81818182; 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 @@ -27,4 +27,8 @@ public class TurretConstants { 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 + + public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; + + public static final double FEEDFORWARD_KV = 0.185; +} -- 2.39.5