diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c41d82c..713e7a6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,7 +25,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.PubSub; import edu.wpi.first.wpilibj.XboxController; import frc.robot.util.FFConstants; @@ -262,7 +261,6 @@ public static final class Module { // Hope this works??? This should be tuned using SYSID or Power, I, and Damping method public static final FFConstants REAL_FF = new FFConstants(0, 0); - // TODO: Test 0.0020645, 0.0, 0.0 (YAGSL configs) public static final PIDConstants REAL_DRIVE_PID = new PIDConstants(0.0020645, 0.0, 0.0); public static final PIDConstants REAL_TURN_PID = new PIDConstants(7.0, 0.0, 0.00007); @@ -281,14 +279,8 @@ public static final class Sim { public static final double TURN_MOI = 0.004; } - // TODO: Put constants from those abstractions in here - public static final class SparkMax {} - - public static final class TalonFX {} - public static final class Hybrid { - // TODO: Uh do these work? public static final double DRIVE_GEAR_RATIO = 5.357; public static final double TURN_GEAR_RATIO = 21.4286; @@ -302,11 +294,6 @@ public static final class Hybrid { public static final int SPARK_AVG_DEPTH = 2; public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; - // CAN/Device IDS and offsets (May be Wrong, guessed which ids are correct off vibes) - - // TODO: Confirm module IDs are associated with correct module and tune module offsets if - // last years dont work - // Front Left Module public static final int DRIVE0_ID = 2; public static final int TURN0_ID = 1; @@ -369,15 +356,23 @@ public static final class Climber { public static final int LEFT_ID = 33; public static final int RIGHT_ID = 34; + public static final int CURRENT_FILTER_TAPS = 5; + + public static final double ZEROING_SPEED = -0.25; + + // Sim Configs + public static final int NUM_MOTORS = 2; + public static final double GEARING = 0.01; + public static final double JKG_M_SQUARED = 1; + // IN ROTATIONS (climber rotates a lot ig) - // TODO: TUNE public static final double ERROR_OF_MARGIN = 1; + public static final double IDLE = 10; + public static final double CLIMBING = 150; - // TODO: Check if these are still right public static final double LEFT_CURRENT_LIMIT = 13; public static final double RIGHT_CURRENT_LIMIT = 13; - // TODO: TUNE public static final PIDConstants REAL_PID = new PIDConstants(0.05, 0, 0); public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); } diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climbers/ClimberIOSim.java index 279ad12..468a185 100644 --- a/src/main/java/frc/robot/subsystems/climbers/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climbers/ClimberIOSim.java @@ -17,15 +17,15 @@ public class ClimberIOSim implements ClimberIO { public ClimberIOSim() { // Lol I'm not gona tune the sim - climberSim = new DCMotorSim(DCMotor.getNEO(2), 1, 1); + climberSim = new DCMotorSim(DCMotor.getNEO(Constants.Climber.NUM_MOTORS), Constants.Climber.GEARING, Constants.Climber.JKG_M_SQUARED); controller = new PIDController(0, 0, 0); climberSetpoint = 0; appliedVolts = 0; } public void updateInput(ClimberIOInputs inputs) { - inputs.leftClimberSpeed = climberSim.getAngularVelocityRPM() / 60; - inputs.rightClimberSpeed = climberSim.getAngularVelocityRPM() / 60; + inputs.leftClimberSpeed = climberSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.rightClimberSpeed = climberSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; inputs.leftClimberPosition = climberSim.getAngularPositionRotations(); inputs.rightClimberPosition = climberSim.getAngularPositionRotations(); inputs.leftClimberSetpoint = climberSetpoint; diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climbers/ClimberIOSparkMax.java index ce252ad..a62dab8 100644 --- a/src/main/java/frc/robot/subsystems/climbers/ClimberIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climbers/ClimberIOSparkMax.java @@ -46,8 +46,8 @@ public ClimberIOSparkMax() { climberController = new PIDController(0, 0, 0); - leftFilter = LinearFilter.movingAverage(5); - rightFilter = LinearFilter.movingAverage(5); + leftFilter = LinearFilter.movingAverage(Constants.Climber.CURRENT_FILTER_TAPS); + rightFilter = LinearFilter.movingAverage(Constants.Climber.CURRENT_FILTER_TAPS); leftClimberSetpoint = 0; rightClimberSetpoint = 0; @@ -64,8 +64,8 @@ public void updateInputs(ClimberIOInputs inputs) { inputs.rightClimberPosition = rightClimberEncoder.getPosition(); inputs.leftClimberSetpoint = leftClimberSetpoint; inputs.rightClimberSetpoint = rightClimberSetpoint; - inputs.leftClimberSpeed = leftClimberEncoder.getVelocity() / 60; - inputs.rightClimberSpeed = rightClimberEncoder.getVelocity() / 60; + inputs.leftClimberSpeed = leftClimberEncoder.getVelocity() / Constants.RPM_TO_RPS_CF; + inputs.rightClimberSpeed = rightClimberEncoder.getVelocity() / Constants.RPM_TO_RPS_CF; } public void updateOutputs(ClimberIOOutputs outputs) { @@ -108,8 +108,8 @@ public void setSetpoints(double leftSetpoint, double rightSetpoint) { } public void zeroClimbers() { - double leftZeroingSpeed = -0.25; - double rightZeroingSpeed = -0.25; + double leftZeroingSpeed = Constants.Climber.ZEROING_SPEED; + double rightZeroingSpeed = Constants.Climber.ZEROING_SPEED; // Not voltage sensing if ( @@ -118,7 +118,7 @@ public void zeroClimbers() { leftClimberZeroed ) { if (!leftClimberZeroed) leftClimberMotor.getEncoder().setPosition(0); - leftClimberSetpoint = 10; + leftClimberSetpoint = Constants.Climber.IDLE; leftZeroingSpeed = climberController.calculate( leftClimberEncoder.getPosition(), leftClimberSetpoint @@ -132,7 +132,7 @@ public void zeroClimbers() { rightClimberZeroed ) { if (!rightClimberZeroed) rightClimberMotor.getEncoder().setPosition(0); - rightClimberSetpoint = 10; + rightClimberSetpoint = Constants.Climber.IDLE; rightZeroingSpeed = climberController.calculate( rightClimberEncoder.getPosition(), rightClimberSetpoint diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimberStates.java b/src/main/java/frc/robot/subsystems/climbers/ClimberStates.java index 1af8445..bd58bea 100644 --- a/src/main/java/frc/robot/subsystems/climbers/ClimberStates.java +++ b/src/main/java/frc/robot/subsystems/climbers/ClimberStates.java @@ -1,10 +1,11 @@ package frc.robot.subsystems.climbers; +import frc.robot.Constants; import frc.robot.subsystems.SubsystemStates; public enum ClimberStates implements SubsystemStates { - OFF(10, 10, "Off"), - CLIMBING(150, 150, "Climbing"); + OFF(Constants.Climber.IDLE, Constants.Climber.IDLE, "Off"), + CLIMBING(Constants.Climber.CLIMBING, Constants.Climber.CLIMBING, "Climbing"); private double leftSetpoint; private double rightSetpoint;