From 119451e29e54822903ac2139f8263f821c5f3f15 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 22 Feb 2026 14:02:31 -0700 Subject: [PATCH] Add Climber stuff --- .../java/frc4388/robot/RobotContainer.java | 64 ++++------------ .../robot/subsystems/climber/Climber.java | 58 +++++++------- .../subsystems/climber/ClimberConstants.java | 50 ++++--------- .../robot/subsystems/climber/ClimberIO.java | 18 +---- .../robot/subsystems/climber/ClimberReal.java | 75 +++++++++++-------- .../robot/subsystems/intake/IntakeReal.java | 5 +- 6 files changed, 110 insertions(+), 160 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cd4ec83..a0cc59b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,43 +179,13 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new RotTo45(m_robotSwerveDrive)); - - // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - // .onTrue(RobotShoot) - // .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); - - // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - // .onTrue(RobotIntake) - // .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); - - - - // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> { @@ -237,9 +207,6 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> { m_robotSwerveDrive.softStop(); })); - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Extended); - // })) // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) @@ -252,15 +219,6 @@ public class RobotContainer { ); }, m_robotSwerveDrive) ); - // .onTrue(new InstantCommand(() -> { - // // When Right trigger is pressed, - // m_robotShooter.setShooterReady(); - // })) - // .onFalse(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Retracted); - // m_robotShooter.setShooterNotReady(); - // })); - // D-PAD fine alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) @@ -302,7 +260,14 @@ public class RobotContainer { })).onFalse(new InstantCommand(() -> { m_robotShooter.setShooterNOTShoot(); })); - + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extending); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { @@ -312,13 +277,10 @@ public class RobotContainer { m_robotIntake.setMode(IntakeMode.Idle); })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extending); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> { + // m_robotClimber.toggleDeployed(); + // })); } diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java index 3ddb883..2e3ff14 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -1,50 +1,58 @@ package frc4388.robot.subsystems.climber; +import static edu.wpi.first.units.Units.Inches; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { - ClimberIO io; + public ClimberIO io; ClimberStateAutoLogged state = new ClimberStateAutoLogged(); - // Supplier m_swervePoseSupplier; - public Climber( ClimberIO io - // Supplier swervePoseSupplier ) { this.io = io; - // this.m_swervePoseSupplier = swervePoseSupplier; } - // public enum ClimberMode { - // Up, - // Down, - // } + ClimberMode mode = ClimberMode.Retracting; - // public void setMode(IntakeMode mode) { - // switch (mode) { - // case Up: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); - // io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); - // break; - // case Down: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); - // io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); - // break; - // } - // } + public enum ClimberMode { + Extended, + Retracting, + } + public void deploy() { + mode = ClimberMode.Extended; + } + public void retract() { + mode = ClimberMode.Retracting; + } + + public void toggleDeployed() { + switch (mode) { + case Extended: + mode = ClimberMode.Retracting; + break; + case Retracting: + mode = ClimberMode.Extended; + break; + } + } @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter - + switch (mode) { + case Extended: + io.setClimberDistance(state, Inches.of(ClimberConstants.CLIMBER_RETRACT_SPEED.get())); + break; + case Retracting: + io.setPercentOutput(state, ClimberConstants.CLIMBER_RETRACT_SPEED.get()); + break; + } Logger.processInputs("Climber", state); diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java index 131ea38..fc45dba 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java @@ -8,36 +8,32 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Distance; +import frc4388.utility.configurable.ConfigurableDouble; +import frc4388.utility.status.CanDevice; public class ClimberConstants { // Motor conversions - - public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; + public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; // dimentionless + public static final double CLIMBER_SPOOL_DIAMETER = 1.; // in + public static final double CLIMBER_REVS_PER_INCH = CLIMBER_MOTOR_GEAR_RATIO / (Math.PI * CLIMBER_SPOOL_DIAMETER); // in //IDs + public static final CanDevice CLIMBER_MOTOR = new CanDevice("Climber Motor", 30); + public static final int CLIMBER_LIMIT_SWITCH_CHANNEL = 8; - - // public static final CanDevice CLIMBER_ID = new CanDevice("SHOOTER 1", 20); - - // Limits - - // 0 is the forward angle on the robot. - // negative is left, positive is right - // public static final Angle L1 = Degrees.of(-180); - // public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + // Constants public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0); - - public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0); - + public static final ConfigurableDouble CLIMBER_LIMIT_UPPER = new ConfigurableDouble("Climber Height in", 6.); + public static final ConfigurableDouble CLIMBER_RETRACT_SPEED = new ConfigurableDouble("Climber retract %", 0.3); public static final Slot0Configs CLIMBER_PID = new Slot0Configs() - .withKP(2.0) + .withKP(0.3) .withKI(0.0) - .withKD(10.0); + .withKD(0.0); - // 0 is paralell to the ground, 90 is directly up - // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + public static final ConfigurableDouble CLIMBER_kP = new ConfigurableDouble("Climber kP", 0.3); + public static final ConfigurableDouble CLIMBER_kI = new ConfigurableDouble("Climber kI", 0.); + public static final ConfigurableDouble CLIMBER_kD = new ConfigurableDouble("Climber kD", 0.); // Motor configs public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration() @@ -50,18 +46,4 @@ public class ClimberConstants { .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); -} - - // public static final class IDs { - // public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - // } - // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java index f401d05..574601e 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java @@ -14,23 +14,13 @@ public interface ClimberIO { Distance climberDistance = Inches.of(0); Distance climberTargetDistance = Inches.of(0); Current climberMotorCurrent = Amps.of(0); - - // Distance shooterPitch = Rotations.of(0); - // Angle shooterTargetPitch = Rotations.of(0); - // Current pitchMotorCurrent = Amps.of(0); - - // AngularVelocity rollerVelocity = RotationsPerSecond.of(0); - // AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); - // Current rollerMotorCurrent = Amps.of(0); - - // LinearVelocity feederVelocity = InchesPerSecond.of(0); - // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - // Current feederMotorCurrent = Amps.of(0); + boolean climberLimit = false; } - // public default void setShooterAngle(ShooterState state, Angle angle) {} - // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setClimberDistance(ClimberState state, Distance distance) {} + public default void setPercentOutput(ClimberState state, double percent) {} public default void updateInputs(ClimberState state) {} + + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java index 9069754..6fcc040 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java @@ -2,68 +2,77 @@ package frc4388.robot.subsystems.climber; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.DigitalInput; public class ClimberReal implements ClimberIO { - - TalonFX m_climberMotor; + DigitalInput m_lowerLimit; + + PositionDutyCycle climberPosition = new PositionDutyCycle(0); + DutyCycleOut climberPercentOutout = new DutyCycleOut(0); public ClimberReal( - - TalonFX climberMotor + TalonFX climberMotor, + DigitalInput lowerLimit ) { - // m_angleMotor = angleMotor; - // m_pitchMotor = pitchMotor; m_climberMotor = climberMotor; + m_lowerLimit = lowerLimit; + // Apply the configs m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID); m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG); + climberPosition.Slot = 0; } - private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){ - if(distance.gt(climberLimitUpper)) { - return climberLimitUpper; - }else if(distance.lt(climberLimitLower)) { - return climberLimitLower; - }else{ - return distance; - } - } - - @Override public void setClimberDistance(ClimberState state, Distance distance) { state.climberTargetDistance = distance; - // Assume that the angle is always accurate, because I think we will use a shaft encoder - // Assume that 0 degrees = forwards. Might need an offset here - - Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER); - // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; - PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance); - m_climberMotor.setControl(posRequest); + + // length * (motor rot / length) = motor rot + double angle = distance.in(Inches) * ClimberConstants.CLIMBER_REVS_PER_INCH; + + m_climberMotor.setControl( + climberPosition.withPosition(Rotations.of(angle)) + .withLimitReverseMotion(m_lowerLimit.get()) + ); + } + + @Override + public void setPercentOutput(ClimberState state, double percent) { + // var d = new DutyCycleOut(0); + m_climberMotor.setControl( + climberPercentOutout.withOutput(percent) + .withLimitReverseMotion(m_lowerLimit.get()) + ); } @Override public void updateInputs(ClimberState state) { + // motor rot / (motor rot / length) = length + double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations); - double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + double linearInches = motorRotations / ClimberConstants.CLIMBER_REVS_PER_INCH; state.climberDistance = Inches.of(linearInches); + state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue(); - - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); - - // state.armAngle = m_armMotor.getPosition().getValue(); - // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - + state.climberLimit = m_lowerLimit.get(); } + + @Override + public void updateGains() { + ClimberConstants.CLIMBER_PID.kP = ClimberConstants.CLIMBER_kP.get(); + ClimberConstants.CLIMBER_PID.kI = ClimberConstants.CLIMBER_kI.get(); + ClimberConstants.CLIMBER_PID.kD = ClimberConstants.CLIMBER_kD.get(); + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID); + } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index e096062..035eca4 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -19,7 +19,7 @@ public class IntakeReal implements IntakeIO { DigitalInput m_armLimitSwitch; PositionDutyCycle armPosition = new PositionDutyCycle(0); - // VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0); + DutyCycleOut armPercentOutput = new DutyCycleOut(0); public IntakeReal( DigitalInput armLimitSwitch, @@ -94,9 +94,8 @@ public class IntakeReal implements IntakeIO { @Override public void armOutput(double percentOutput){ - var d = new DutyCycleOut(0); m_armMotor.setControl( - d.withOutput(percentOutput) + armPercentOutput.withOutput(percentOutput) .withLimitReverseMotion(!m_armLimitSwitch.get()) ); }