Added climber

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<DriveLayout> driveMode = new SendableChooser<>();
   public static SendableChooser<AutonMode> autoMode = new SendableChooser<>();
   public static SendableChooser<String> 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);

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;
+  }
+}

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;

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;
+  }
+}