Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add Climber stuff

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-22 14:02:31 -07:00
parent 77ca57d678
commit 119451e29e
6 changed files with 110 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+11 -49
View File
@@ -179,21 +179,6 @@ 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()));
@@ -202,21 +187,6 @@ public class RobotContainer {
// 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(() -> {
m_robotIntake.io.updateGains();
@@ -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)
@@ -303,6 +261,13 @@ public class RobotContainer {
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();
// }));
}
src/main/java/frc4388/robot/subsystems/climber/Climber.java
+33 -25
View File
@@ -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<Pose2d> m_swervePoseSupplier;
public Climber(
ClimberIO io
// Supplier<Pose2d> 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);
src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java
+15 -33
View File
@@ -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()
@@ -51,17 +47,3 @@ public class ClimberConstants {
.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
// );
src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java
+4 -14
View File
@@ -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() {}
}
src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java
+41 -32
View File
@@ -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);
}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+2 -3
View File
@@ -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())
);
}