From 6d53d84979f5d6c9de548c16971f75014c24251f Mon Sep 17 00:00:00 2001 From: iefomit Date: Thu, 16 Apr 2026 17:28:32 -0700 Subject: [PATCH] remove unused things --- src/main/java/frc/robot/RobotContainer.java | 10 +- .../robot/commands/gpm/AutoShootCommand.java | 6 +- .../robot/commands/gpm/ClimbDriveCommand.java | 23 -- .../frc/robot/commands/gpm/LockedShoot.java | 35 --- .../robot/commands/gpm/Superstructure.java | 3 - .../frc/robot/constants/FieldConstants.java | 1 - .../controls/PS5ControllerDriverConfig.java | 6 +- .../controls/PS5XboxModeDriverConfig.java | 29 +-- .../subsystems/Climb/ClimbConstants.java | 42 ---- .../robot/subsystems/Climb/LinearClimb.java | 238 ------------------ .../robot/subsystems/Climb/LinearClimbIO.java | 14 -- .../frc/robot/subsystems/Intake/Intake.java | 6 - .../frc/robot/subsystems/turret/Turret.java | 7 - 13 files changed, 4 insertions(+), 416 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/Climb/LinearClimb.java delete mode 100644 src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2240aac..6d17086 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,6 @@ import frc.robot.commands.LogCommand; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.commands.gpm.BrownOutControl; -import frc.robot.commands.gpm.ClimbDriveCommand; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.LockedShoot; import frc.robot.commands.gpm.RunSpindexer; @@ -38,7 +37,6 @@ import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; -import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.LED.LED; import frc.robot.subsystems.drivetrain.Drivetrain; @@ -76,7 +74,6 @@ public class RobotContainer { // Controllers are defined here private BaseDriverConfig driver = null; private Operator operator = null; - private LinearClimb linearClimb = null; private LED led = null; // TODO: move to correct robot and put the correct port? @@ -146,7 +143,7 @@ public class RobotContainer { case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb); + driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer); operator = new Operator(drive); // Detected objects need access to the drivetrain @@ -269,11 +266,6 @@ public class RobotContainer { hood.forceHoodDown(false); })); } - - if (linearClimb != null && drive != null) { - NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive)); - } - } public void addAuto(String name) { diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 91113e8..02abe4c 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -24,7 +24,6 @@ import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.turret.TurretConstants; import frc.robot.util.ShooterPhysics; -import frc.robot.util.ShooterPhysics.Constraints; import frc.robot.util.ShooterPhysics.TurretState; public class AutoShootCommand extends Command { @@ -33,10 +32,7 @@ public class AutoShootCommand extends Command { private Hood hood; private Shooter shooter; private Spindexer spindexer; - - //TODO: find maximum interpolation - private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); - + private double turretSetpoint; private double hoodSetpoint; diff --git a/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java deleted file mode 100644 index 6819c09..0000000 --- a/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.drive_comm.DriveToPose; -import frc.robot.constants.FieldConstants; -import frc.robot.subsystems.Climb.LinearClimb; -import frc.robot.subsystems.drivetrain.Drivetrain; - -public class ClimbDriveCommand extends SequentialCommandGroup{ - - public ClimbDriveCommand(LinearClimb climb, Drivetrain drive){ - addCommands( - new ParallelCommandGroup( - new InstantCommand(() -> climb.climbPosition()), - new DriveToPose(drive, () -> FieldConstants.getClimbLocation()) - ) - ); - } - - -} diff --git a/src/main/java/frc/robot/commands/gpm/LockedShoot.java b/src/main/java/frc/robot/commands/gpm/LockedShoot.java index 0f886a4..3f7a0f5 100644 --- a/src/main/java/frc/robot/commands/gpm/LockedShoot.java +++ b/src/main/java/frc/robot/commands/gpm/LockedShoot.java @@ -1,6 +1,5 @@ package frc.robot.commands.gpm; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -9,7 +8,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; @@ -31,17 +29,11 @@ public class LockedShoot extends Command { private Hood hood; private Shooter shooter; - private double turretSetpoint; - private double hoodSetpoint; - private Pose2d drivepose; - private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - private Rotation2d lastTurretAngle; private Rotation2d targetAngle; - private double turretVelocity; private double lastHoodAngle; private double hoodAngle; @@ -55,10 +47,6 @@ public class LockedShoot extends Command { private PhaseManager phaseManager = new PhaseManager(); - private double hoodOffset = 0.0; - - private double turretOffset = 0.0; - private double distanceFromTarget = 0.0; private double TOFAdjustment = 0.85; @@ -138,7 +126,6 @@ public class LockedShoot extends Command { // Pitch is in radians hoodAngle = goalState.pitch(); - hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); lastHoodAngle = hoodAngle; @@ -172,25 +159,6 @@ public class LockedShoot extends Command { //spindexer.stopSpindexer(); } - // shoot higher - public void bumpUpHoodOffset() { - hoodOffset += 1.0; // 1 degree - } - - // shoot lower - public void bumpDownHoodOffset() { - hoodOffset -= 1.0; // 1 degree - } - - // aim more left - public void bumpUpTurretOffset() { - turretOffset += 2.5; // 2.5 degree - } - // aim more right - public void bumpDownTurretOffset() { - turretOffset -= 2.5; // 2.5 degree - } - @Override public void execute() { updateDrivePose(); @@ -215,9 +183,6 @@ public class LockedShoot extends Command { hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); } - double x = drivepose.getX(); // compared as meters - double y = drivepose.getY(); - // if (FieldConstants.underTrench(x, y)) { // System.out.println("Hood forced down"); // } else { diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 4853269..0928289 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -272,9 +272,6 @@ public class Superstructure extends Command { hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); } - double x = drivepose.getX(); // compared as meters - double y = drivepose.getY(); - // if (FieldConstants.underTrench(x, y)) { // System.out.println("Hood forced down"); // } else { diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index d0339b1..399345d 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -173,7 +173,6 @@ public class FieldConstants { public static FieldZone getZone(Translation2d drivepose) { double x = drivepose.getX(); - double y = drivepose.getY(); if ((x < FIELD_LENGTH / 2 - Units.inchesToMeters(120.0) && x > (BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) // blue alliance line diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 9b57820..6449efe 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -14,7 +14,6 @@ import frc.robot.commands.gpm.RunSpindexer; import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; import frc.robot.constants.swerve.DriveConstants; -import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; @@ -40,7 +39,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private Hood hood; private Intake intake; private Spindexer spindexer; - private LinearClimb climb; public PS5ControllerDriverConfig( Drivetrain drive, @@ -48,15 +46,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { Turret turret, Hood hood, Intake intake, - Spindexer spindexer, - LinearClimb climb) { + Spindexer spindexer) { super(drive); this.shooter = shooter; this.turret = turret; this.hood = hood; this.intake = intake; this.spindexer = spindexer; - this.climb = climb; } public void configureControls() { diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java index 3cdc02b..e0cc573 100644 --- a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -12,10 +12,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; import frc.robot.commands.gpm.AutoShootCommand; -import frc.robot.commands.gpm.ClimbDriveCommand; import frc.robot.commands.gpm.ReverseMotors; import frc.robot.constants.Constants; -import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.Intake.Intake; @@ -50,7 +48,6 @@ public class PS5XboxModeDriverConfig extends BaseDriverConfig { private Hood hood; private Intake intake; private Spindexer spindexer; - private LinearClimb climb; // PS5 button aliases private final Button CROSS = Button.A; @@ -82,15 +79,13 @@ public class PS5XboxModeDriverConfig extends BaseDriverConfig { Turret turret, Hood hood, Intake intake, - Spindexer spindexer, - LinearClimb climb) { + Spindexer spindexer) { super(drive); this.shooter = shooter; this.turret = turret; this.hood = hood; this.intake = intake; this.spindexer = spindexer; - this.climb = climb; } public void configureControls() { @@ -183,28 +178,6 @@ public class PS5XboxModeDriverConfig extends BaseDriverConfig { })); } - // Climb - if (climb != null) { - // Calibration - controller.get(OPTIONS).onTrue(new InstantCommand(() -> { - climb.hardstopCalibration(); - })).onFalse(new InstantCommand(() -> { - climb.stopCalibrating(); - })); - - // Climb retract - controller.get(CROSS).onTrue(new InstantCommand(() -> { - climb.retract(); - })); - - // Drive to climb position and rumble - controller.get(TRIANGLE).onTrue(new SequentialCommandGroup( - new ClimbDriveCommand(climb, getDrivetrain()), - new InstantCommand(() -> this.startRumble()), - new WaitCommand(1), - new InstantCommand(() -> this.endRumble()))); - } - // Hood if (hood != null) { controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java deleted file mode 100644 index 169c32c..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.subsystems.Climb; - - -import edu.wpi.first.math.util.Units; - -public class ClimbConstants { - - // CHANGE LATER - // gear ratio for converting motor rotations to linear distance - public final static double CLIMB_GEAR_RATIO = 45.0; - // TODO: Get actual winch bobbin radius measurement - /** Winch spool radius in meters */ - public final static double WHEEL_RADIUS = Units.inchesToMeters(0.5); - /** climber stowed ? position in meters */ - public final static double BOTTOM_POSITION = Units.inchesToMeters(8); - /** Calibration position: lower than BOTTOM_POSITION to reduce motor strain */ - // public final static double CALIBRATION_POSITION = Units.inchesToMeters(8.5); - /** position that should put the robot off the ground? in meters. 6 inches */ - public final static double CLIMB_POSITION = Units.inchesToMeters(6); - public final static double UP_POSITION = 0.0; - - // current limits (in amps) - // CALIBRATION: Low current while finding hardstop to prevent damage - // NORMAL: Moderate current for PID-controlled movement - // CLIMB: High current for full-power climbing - public final static double CALIBRATION_CURRENT = 20.0; - public final static double CLIMB_CURRENT = 42.0; - public final static double CALIBRATION_CURRENT_THRESHOLD = 18.0; - - // PID Constants - // TODO: what are the units? Inches? Meters? - public final static double PID_P = 0.1; - public final static double PID_I = 0.0; - public final static double PID_D = 0.0; - public final static double PID_TOLERANCE = 0.2; - - // Motor Limits - public final static double CALIBRATION_POWER = 0.15; - - // Calibration - public final static int CALIBRATION_COUNTER_LIMIT = 250; -} diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java deleted file mode 100644 index 80389af..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ /dev/null @@ -1,238 +0,0 @@ -package frc.robot.subsystems.Climb; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; - -/** - * Climber subsystem - */ -public class LinearClimb extends SubsystemBase implements LinearClimbIO { - /** climber motor */ - private final TalonFX motor; - /** whether the subsysgtem is calibrating */ - private boolean calibrating = false; - - /** should the subsystem perform calibration automatically */ - private boolean calibrateOnStartUp = false; - - private double MAX_POWER = 0.2; - - private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); - - // logging information - private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged(); - - /** This PID controller uses motor rotations */ - private final PIDController pid = new PIDController( - ClimbConstants.PID_P, - ClimbConstants.PID_I, - ClimbConstants.PID_D); - - public LinearClimb() { - motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB); - pid.setTolerance(ClimbConstants.PID_TOLERANCE); - - motor.setNeutralMode(NeutralModeValue.Brake); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); - - setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Calibrate", new InstantCommand(() -> hardstopCalibration())); - SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0 - SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8 - SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6 - } - - // starting position - motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - - // calibrate on startup to find hardstop - if (calibrateOnStartUp) { - hardstopCalibration(); - } - } - - /** - * set the setpoint for the pid - * - * @param setpoint in rotations - */ - public void setSetpoint(double setpoint) { - pid.setSetpoint(setpoint); - } - - /** - * @return when the position is within 0.2 rotations - */ - public boolean atSetPoint() { - return pid.atSetpoint(); - } - - /** - * Returns the current position of the climb motor. - * - * @return Position in motor rotations. Positive values move the climb mechanism - * UP (toward the hardstop). Higher values = higher physical position. - * Use {@link #getAsMeters()} for linear distance in meters. - */ - public double getPosition() { - return motor.getPosition().getValueAsDouble(); - } - - /** - * Returns the climb position converted to linear distance in meters. - * This is useful for debugging and logging. - * - * @return Linear position in meters, calculated as: - * rotations * gearRatio * 2 * PI * radius - */ - public double getAsMeters() { - return inputs.positionMeters; - } - - /** - * goes to the up position - */ - public void goUp() { // 0 - MAX_POWER = 0.8; - setSetpoint(metersToRotations(ClimbConstants.UP_POSITION)); - } - - /** - * goes to the down position - */ - public void retract() { // 8 - MAX_POWER = 0.2; - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - } - - /** - * goes to the climb position - */ - public void climbPosition() { // 6 - MAX_POWER = 0.8; - setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION)); - } - - @Override - public void periodic() { - double power = pid.calculate(motor.getPosition().getValueAsDouble()); - // if it is not calibrating, do normal stuff - if (!calibrating) { - power = MathUtil.clamp(power, -MAX_POWER, MAX_POWER); - } else { - power = ClimbConstants.CALIBRATION_POWER; - boolean atHardStop = Math - .abs(motor.getStatorCurrent().getValueAsDouble()) >= ClimbConstants.CALIBRATION_CURRENT_THRESHOLD; - if (calibrationDebouncer.calculate(atHardStop)) { - stopCalibrating(); - } - } - - // motor.set(power); // during calibration we have 20ms of high power before we stop calibration\ - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Climb Power from PID", power); - SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint()); - SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble()); - SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters); - SmartDashboard.putBoolean("Climb Calibrated", !calibrating); - SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint()); - } - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) - * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO); - } - updateInputs(); - Logger.processInputs("LinearClimb", inputs); - } - - /** - * converts motor rotations to meters - * - * @param motorRotations - * @return - */ - public double rotationsToMeters(double motorRotations) { - double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS; - double meters = motorRotations / ClimbConstants.CLIMB_GEAR_RATIO * circ; - return meters; - } - - /** - * converts meters to motor rotations - * - * @param meters - * @return - */ - public double metersToRotations(double meters) { - double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS; - double motorRotations = meters / circ * ClimbConstants.CLIMB_GEAR_RATIO; - return motorRotations; - } - - /** - * sets supply and stator current limits - * - * @param limit the current limit for stator and supply current - */ - public void setCurrentLimits(double limit) { - TalonFXConfiguration config = new TalonFXConfiguration(); - - config.CurrentLimits = new CurrentLimitsConfigs(); - - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = limit; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = limit; - - motor.getConfigurator().apply(config); - } - - /** - * Sets the motor to a slow speed until it hits the hard stop - */ - public void hardstopCalibration() { - calibrating = true; - setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT); - } - - /** - * stops calibration and sets current limits to normal. - */ - public void stopCalibrating() { - motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - calibrating = false; - setCurrentLimits(ClimbConstants.CLIMB_CURRENT); - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - } - - @Override - public void updateInputs() { - inputs.positionMeters = Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) - * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO; - inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.power = pid.calculate(motor.getPosition().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java deleted file mode 100644 index a3ed4e9..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java +++ /dev/null @@ -1,14 +0,0 @@ -package frc.robot.subsystems.Climb; - -import org.littletonrobotics.junction.AutoLog; - -public interface LinearClimbIO { - @AutoLog - public static class LinearClimbIOInputs{ - public double positionMeters = 0.0; - public double motorCurrent = 0.0; - public double power = 0.0; - } - - public void updateInputs(); -} diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index d373bb9..65e9be5 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -11,8 +11,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; @@ -66,7 +64,6 @@ public class Intake extends SubsystemBase implements IntakeIO{ private double setpointInches = 0.0; private boolean calibrating = false; - private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); @@ -205,9 +202,6 @@ public class Intake extends SubsystemBase implements IntakeIO{ leftMotor.set(-0.1); rightMotor.set(-0.1); boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD; - // if(calibrationDebouncer.calculate(atHardStop)){ - // stopCalibrating(); - // } } updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 6b05330..550c956 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -281,13 +281,6 @@ public class Turret extends SubsystemBase implements TurretIO{ motor.getConfigurator().apply(limits); } - // Also ignore this for now - private double wrapUnit(double value) { - value %= 1.0; - if (value < 0) value += 1.0; - return value; - } - private void calibrate(){ setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT); calibrating = true; -- 2.39.5