diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d789d02..127d562 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,6 +31,12 @@ public static class CAN { public static final int Victor4 = 4; public static final int Talon5 = 5; public static final int Falcon = 14; + public static final int climberMotor = -1; + } + + public static class IDs{ + public static final int climberLimitHome = -1; + public static final int climberLimitExtended = -1; } public static class Swerve { diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/Climb.java new file mode 100644 index 0000000..5664210 --- /dev/null +++ b/src/main/java/frc/robot/commands/Climb.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.ExampleSubsystem; + +/** An example command that uses an example subsystem. */ +public class Climb extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Climber climber; + + public Climb(ExampleSubsystem subsystem) { + climber = new Climber(); + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.goHome(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMoving(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.isHome(); + } +} diff --git a/src/main/java/frc/robot/commands/ExtendClimber.java b/src/main/java/frc/robot/commands/ExtendClimber.java new file mode 100644 index 0000000..857ba44 --- /dev/null +++ b/src/main/java/frc/robot/commands/ExtendClimber.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.ExampleSubsystem; + +public class ExtendClimber extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Climber climber; + + public ExtendClimber(ExampleSubsystem subsystem) { + climber = new Climber(); + addRequirements(climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.extend(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMoving(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.isExtended(); + } +} 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..efb2ada --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Climber extends SubsystemBase { + private final TalonFX climberMotor; + private final DigitalInput limitHome; + private final DigitalInput limitExtended; + private final double climbSpeed = .1; + + public Climber() { + climberMotor = new TalonFX(Constants.CAN.climberMotor, "rio"); + limitHome = new DigitalInput(Constants.IDs.climberLimitHome); + limitExtended = new DigitalInput(Constants.IDs.climberLimitExtended); + climberMotor.getConfigurator().apply(new TalonFXConfiguration()); + } + public void stopMoving() { + climberMotor.set(0); + } + public void extend() { + climberMotor.set(climbSpeed); + } + public boolean isExtended() { + return limitExtended.get(); + } + public void goHome(){climberMotor.set(-climbSpeed);} + public boolean isHome() { + return limitHome.get(); + } + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } +}