diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52f6933..59313e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc.robot.constants.OIConstants; import frc.robot.constants.ShooterConstants; import frc.robot.constants.IntakePivotConstants.IntakePivotPosition; import frc.robot.constants.ShooterConstants.ShooterSpeeds; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Hood; import frc.robot.subsystems.IntakePivot; @@ -48,6 +49,7 @@ public class RobotContainer { private IntakePivot intakePivot; private IntakeRoller intakeRoller; private Spindexer spindexer; + private Climber climber; private CommandXboxController driver; private CommandXboxController secondary; @@ -64,6 +66,7 @@ public class RobotContainer { intakePivot = new IntakePivot(); intakeRoller = new IntakeRoller(); spindexer = new Spindexer(); + climber = new Climber(); vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { @@ -143,6 +146,17 @@ public class RobotContainer { intakeRoller.setDefaultCommand(intakeRoller.stop()); hood.setDefaultCommand(hood.stop()); spindexer.setDefaultCommand(spindexer.stop()); + climber.setDefaultCommand(climber.stop()); + + // While holding POV up of the driver controller, the climber + // should move such that its motor moves the climber down with the left + // driver controller trigger axis, and up with the right driver controller + // trigger axis. + // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted + // from the constants file for the subsystem having the problem. + driver.povUp().whileTrue(climber.manualSpeed(() -> { + return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis(); + })); // While holding the right bumper of the driver controller, the intake rollers // and the spindexer and feeder should move such that all motors are moving in such a way @@ -209,6 +223,7 @@ public class RobotContainer { secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp)); // TODO Some means of testing hood PIDF + // TODO Some means of testing climber PIDF } private void configureBindings() { diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java new file mode 100644 index 0000000..1de632c --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -0,0 +1,61 @@ +package frc.robot.constants; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +public class ClimberConstants { + // TODO Real values + public enum ClimberPositions { + kStow(0), + kClimbOffGround(0), + kUp(0); + + private double positionMeters; + + private ClimberPositions(double positionMeters) { + this.positionMeters = positionMeters; + } + + public double getPositionMeters() { + return positionMeters; + } + } + + public static final int kMotorCANID = 0; + + public static final double kConversionFactor = 0; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kV = 0; + public static final double kA = 0; + + public static final boolean kMotorInverted = false; + + public static final int kCurrentLimit = 40; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig(); + + static { + kMotorConfig + .inverted(kMotorInverted) + .smartCurrentLimit(kCurrentLimit) + .idleMode(kIdleMode); + kMotorConfig.encoder + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); + kMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kP, kI, kD) + .outputRange(-1, 1) + .feedForward + .sva(kS, kV, kA); + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..a9b95e3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ClimberConstants; +import frc.robot.constants.ClimberConstants.ClimberPositions; + +public class Climber extends SubsystemBase { + private SparkMax motor; + + private RelativeEncoder encoder; + + private SparkClosedLoopController controller; + + private ClimberPositions targetPosition; + + public Climber() { + motor = new SparkMax(ClimberConstants.kMotorCANID, MotorType.kBrushless); + + motor.configure( + ClimberConstants.kMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + encoder = motor.getEncoder(); + + controller = motor.getClosedLoopController(); + + targetPosition = null; + } + + @Override + public void periodic() { + Logger.recordOutput("Climber/TargetPositionMeters", targetPosition == null ? -1 : targetPosition.getPositionMeters()); + Logger.recordOutput("Climber/CurrentPositionMeters", encoder.getPosition()); + Logger.recordOutput("Climber/AtSetpoint", controller.isAtSetpoint()); + } + + public Command maintainPosition(ClimberPositions position) { + targetPosition = position; + + return run(() -> { + controller.setSetpoint( + position.getPositionMeters(), + ControlType.kPosition + ); + }); + } + + public Command manualSpeed(DoubleSupplier speed) { + targetPosition = null; + + return run(() -> { + motor.set(speed.getAsDouble()); + }); + } + + public Command stop() { + return manualSpeed(() -> 0); + } +}