diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a90ad0a3..561e1eb0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -351,7 +351,9 @@ public static enum AlgaeScoreTarget { new ServoIOReal(1)); private final ClimberSubsystem climber = - new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); + new ClimberSubsystem( + ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim(), + new ServoIOReal(2)); private final Superstructure superstructure = new Superstructure( diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 5ff570e5..5b57e01b 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -4,25 +4,35 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.servo.ServoIO; +import frc.robot.subsystems.servo.ServoIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { // TODO update public static final double CLIMBER_ARM_LENGTH_METERS = Units.inchesToMeters(11.0); public static final double CLIMBER_DRUM_RADIUS_METERS = Units.inchesToMeters(1.0); - public static final double CLIMB_GEAR_RATIO = 125.0; + public static final double CLIMB_GEAR_RATIO = 25.0; public static final double CLIMB_EXTENDED_POSITION = 3.5; + public static final Rotation2d UNLOCK_POSITION = Rotation2d.fromDegrees(-90.0); + public static final Rotation2d LOCK_POSITION = Rotation2d.fromDegrees(0.0); + private final ClimberIO io; private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); - public ClimberSubsystem(ClimberIO io) { + private final ServoIO servo; + private final ServoIOInputsAutoLogged servoInputs = new ServoIOInputsAutoLogged(); + + public ClimberSubsystem(ClimberIO io, ServoIO servo) { this.io = io; + this.servo = servo; SmartDashboard.putData( "rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); @@ -32,6 +42,7 @@ public ClimberSubsystem(ClimberIO io) { @Override public void periodic() { io.updateInputs(inputs); + // no servo inputs to update Logger.processInputs("Climber", inputs); } @@ -46,4 +57,12 @@ public Command resetClimber() { public Command zeroClimber() { return Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true); } + + public Command unlock() { + return this.runOnce(() -> servo.setPosition(UNLOCK_POSITION)); + } + + public Command lock() { + return this.runOnce(() -> servo.setPosition(LOCK_POSITION)); + } }