From 0d409ab4deb48466924f04106600e67dc858fb6c Mon Sep 17 00:00:00 2001 From: asun121 Date: Wed, 9 Mar 2022 10:21:53 -0500 Subject: [PATCH] Added climber --- src/main/java/org/team639/RobotContainer.java | 8 ++++ .../commands/Climber/ToggleClimber.java | 35 +++++++++++++++ src/main/java/org/team639/lib/Constants.java | 5 +++ .../java/org/team639/subsystems/Climber.java | 45 +++++++++++++++++++ 4 files changed, 93 insertions(+) create mode 100644 src/main/java/org/team639/commands/Climber/ToggleClimber.java create mode 100644 src/main/java/org/team639/subsystems/Climber.java diff --git a/src/main/java/org/team639/RobotContainer.java b/src/main/java/org/team639/RobotContainer.java index 97b2329..57eada8 100644 --- a/src/main/java/org/team639/RobotContainer.java +++ b/src/main/java/org/team639/RobotContainer.java @@ -8,6 +8,7 @@ import org.team639.auto.*; import org.team639.commands.Acquisition.*; +import org.team639.commands.Climber.ToggleClimber; import org.team639.commands.Drive.*; import org.team639.commands.Indexer.*; import org.team639.commands.Shooter.*; @@ -44,6 +45,7 @@ public class RobotContainer { private final Shooter shooter = new Shooter(); private final Indexer indexer = new Indexer(); private final Acquisition acquisition = new Acquisition(); + private final Climber climb = new Climber(); private final LED candle = new LED(); // Command Declaration @@ -55,6 +57,7 @@ public class RobotContainer { private final ToggleGears shiftGears = new ToggleGears(driveTrain); private final ReverseHeading swap = new ReverseHeading(driveTrain); private final TurnToAngleRelative aimbot = new TurnToAngleRelative(driveTrain); + public final TurnToBall turnToBall = new TurnToBall(driveTrain); // Acquisition private final ToggleAcquisition toggleAcquisition = new ToggleAcquisition(acquisition); @@ -69,6 +72,9 @@ public class RobotContainer { private final AutoShootAtDistance autoShoot = new AutoShootAtDistance(indexer, shooter, acquisition); private final ToggleActuator resetHood = new ToggleActuator(shooter); + //Climber + private final ToggleClimber toggleClimb = new ToggleClimber(climb); + public static SendableChooser driveMode = new SendableChooser<>(); public static SendableChooser autoMode = new SendableChooser<>(); public static SendableChooser songChooser = new SendableChooser<>(); @@ -193,6 +199,7 @@ private void configureButtonBindings() { ControllerWrapper.DriverButtonA.whenHeld(new DJRobot(driveTrain, 10)); ControllerWrapper.DriverButtonX.whenPressed(aimbot.withTimeout(1)); ControllerWrapper.DriverButtonY.whenPressed(tele.aimbotshot); + ControllerWrapper.DriverButtonB.whenPressed(turnToBall); // Controller ControllerWrapper.ControlRightBumper.whenHeld(index); @@ -201,6 +208,7 @@ private void configureButtonBindings() { ControllerWrapper.ControlButtonY.whenPressed(fenderShot); ControllerWrapper.ControlButtonB.whenPressed(toggleAcquisition); ControllerWrapper.ControlButtonX.whenPressed(autoShoot); + ControllerWrapper.ControlButtonA.whenPressed(toggleClimb); // RESET BUTTONS ControllerWrapper.ControlDPadUp.whenPressed(resetHood); diff --git a/src/main/java/org/team639/commands/Climber/ToggleClimber.java b/src/main/java/org/team639/commands/Climber/ToggleClimber.java new file mode 100644 index 0000000..d396b0c --- /dev/null +++ b/src/main/java/org/team639/commands/Climber/ToggleClimber.java @@ -0,0 +1,35 @@ +// 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 org.team639.commands.Climber; + +import org.team639.subsystems.Climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ToggleClimber extends CommandBase { + private Climber climb; + + /** Creates a new ToggleClimber. */ + public ToggleClimber(Climber climb) { + this.climb = climb; + addRequirements(climb); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if(climb.getClimberPosition()) + climb.climberRetract(); + else + climb.climberExtend(); + } + + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/team639/lib/Constants.java b/src/main/java/org/team639/lib/Constants.java index 7ec0938..3c77c1c 100644 --- a/src/main/java/org/team639/lib/Constants.java +++ b/src/main/java/org/team639/lib/Constants.java @@ -52,6 +52,11 @@ public interface Acquisition public static final int acquisitionExtend = 1; } + public interface Climber + { + public static final int climbID = 3; + } + public interface PneumaticsModuleType { public static final int phCompressorID = 1; diff --git a/src/main/java/org/team639/subsystems/Climber.java b/src/main/java/org/team639/subsystems/Climber.java new file mode 100644 index 0000000..df0b26f --- /dev/null +++ b/src/main/java/org/team639/subsystems/Climber.java @@ -0,0 +1,45 @@ +// 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 org.team639.subsystems; + +import org.team639.lib.Constants; + +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + + private boolean climbExtended = false; + Solenoid main = new Solenoid(PneumaticsModuleType.REVPH, Constants.Ports.Climber.climbID); + + /** Creates a new Climber. */ + public Climber() { + climberRetract(); + } + + public void climberExtend() + { + if(!climbExtended) + { + main.set(true); + climbExtended = true; + } + } + + public void climberRetract() + { + if(climbExtended) + { + main.set(false); + climbExtended = false; + } + } + + public boolean getClimberPosition() + { + return climbExtended; + } +}