diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 7c55826..40d825f 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 23; - public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213"; - public static final String GIT_DATE = "2026-01-29 16:59:53 MST"; + public static final int GIT_REVISION = 24; + public static final String GIT_SHA = "5d381731689602f5fac161f1552170003ef30c14"; + public static final String GIT_DATE = "2026-01-29 18:07:19 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 17:04:31 MST"; - public static final long BUILD_UNIX_TIME = 1769731471423L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-01-29 19:26:44 MST"; + public static final long BUILD_UNIX_TIME = 1769740004409L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..f667f1b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -0,0 +1,79 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Climber extends SubsystemBase { + 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, + // } + + // 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 FieldZone { + // // The robot should aim at the hub + // InShootZone, + // // The robot should aim towards the wall + // AimAtWall, + + // } + + // // Calculate what should be done based off of the position of the robot + // // TODO: Implement field zones + // public FieldZone getTarget(Pose2d position) { + // return FieldZone.InShootZone; + // } + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Climber", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..c0b5329 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,74 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; + +public class ClimberConstants { + // Motor conversions + + public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; + + //IDs + + + // 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); + public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0); + + public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0); + + + public static final Slot0Configs CLIMBER_PID = new Slot0Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.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); + + // Motor configs + public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .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 + // ); diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..bddb5fc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,43 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Inches; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface ClimberIO { + @AutoLog + public class ClimberState { + 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); + } + + // 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 updateInputs(ClimberState state) {} +} \ 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 new file mode 100644 index 0000000..2345a08 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java @@ -0,0 +1,78 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; + +public class ClimberReal implements ClimberIO { + + + TalonFX m_climberMotor; + + public ClimberReal( + + TalonFX climberMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_climberMotor = climberMotor; + // Apply the configs + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID); + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG); + + } + + 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); + } + + @Override + public void updateInputs(ClimberState state) { + double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations); + double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + 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(); + + + } +}