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/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
+16 -34
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()
@@ -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
// );
}
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
+42 -33
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())
);
}