Move things to constants.

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 70ade0d634917d7ce7846dced5da52c335aca654..d116ab487a5a01c71e5862526f6240f744a750cf 100644 (file)
--- 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 bfd0d2ae18b0a2171530948444f315652a5dc582..9bd0ef85622c33a655d9e69d882cbb586fd50ca7 100644 (file)
--- 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 450592607d03e1a754faaab6beb976a13270ff36..ca6f24fe7d0925caa9ad66b03616649e402d1303 100644 (file)
--- 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;
+}