Improve Climber Code

src/main/java/frc4388/robot/RobotContainer.java

@@ -29,6 +29,7 @@ import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
 import frc4388.robot.commands.auto.Wait;
+import frc4388.robot.commands.climber.DisengageRachet;
 import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
@@ -101,6 +102,8 @@ public class RobotContainer {
         m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
         m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
 
+        m_robotLeveler.passRequiredSubsystem(m_robotClimber);
+
         configureButtonBindings();
 
         /* Default Commands */
@@ -118,7 +121,7 @@ public class RobotContainer {
         // drives climber with input from triggers on the opperator controller
         m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
         // drives the leveler with an axis input from the driver controller
-        m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
+        m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController()));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
@@ -161,7 +164,9 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
 
-
+        // Disengages the rachet to allow for a climb
+        new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
+            .whileHeld(new DisengageRachet(m_robotClimber));
 
         /* Operator Buttons */
         // shoots until released

src/main/java/frc4388/robot/commands/climber/DisengageRachet.java

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Climber;
+
+public class DisengageRachet extends CommandBase {
+  Climber m_climber;
+  
+  /**
+   * Creates a new DisengageRachet command.
+   */
+  public DisengageRachet(Climber subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_climber = subsystem;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_climber.climberSafety) {
+      m_climber.shiftServo(false);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java

@@ -40,12 +40,15 @@ public class RunClimberWithTriggers extends CommandBase {
     if (rightTrigger < .5) {
       if(rightTrigger > leftTrigger) {
         output = rightTrigger;
+        m_climber.shiftServo(false);
       }
       if (leftTrigger > rightTrigger) {
         output = -leftTrigger;
+        m_climber.shiftServo(true);
       }
     } else {
       output = rightTrigger;
+      m_climber.shiftServo(false);
     }
     m_climber.runClimber(output);
   }

src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java

@@ -36,7 +36,7 @@ public class RunLevelerWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double input = m_controller.getLeftXAxis();
+    double input = m_controller.getRightXAxis();
     m_leveler.runLeveler(input);
   }
 

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -13,6 +13,8 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ClimberConstants;
 
@@ -20,16 +22,21 @@ public class Climber extends SubsystemBase {
   CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
   CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
 
+  Servo m_servo;
+  //Spark m_spark = new Spark(4);
+
   public boolean climberSafety = false;
 
   /**
    * Creates a new Climber.
    */
   public Climber() {
+    m_servo = new Servo(4);
+
     m_climberMotor.restoreFactoryDefaults();
 
     m_climberMotor.setIdleMode(IdleMode.kBrake);
-    m_climberMotor.setInverted(false);
+    m_climberMotor.setInverted(true);
     
     m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
@@ -48,6 +55,7 @@ public class Climber extends SubsystemBase {
    */
   public void runClimber(double input) {
     if(climberSafety){
+      input *= 1.0;
       m_climberMotor.set(input);
     }
     else{
@@ -66,4 +74,15 @@ public class Climber extends SubsystemBase {
   {
     climberSafety = false;
   }
+
+  /**
+   * @param shift true to enage rachet, false to disengage
+   */
+  public void shiftServo(boolean shift) {
+    if (shift) {
+      m_servo.setPosition(0.5);
+    } else {
+      m_servo.setPosition(0.56);
+    }
+  }
 }

src/main/java/frc4388/robot/subsystems/Leveler.java

@@ -17,7 +17,7 @@ import frc4388.robot.Constants.LevelerConstants;
 public class Leveler extends SubsystemBase {
   CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
 
-  private final Climber m_robotClimber = new Climber();
+  Climber m_climberSubsystem;
 
   /**
    * Creates a new Leveler.
@@ -39,11 +39,19 @@ public class Leveler extends SubsystemBase {
    * @param input the percent output to run motor at
    */
   public void runLeveler(double input) {
-    if(m_robotClimber.climberSafety){
+    if(m_climberSubsystem.climberSafety){
       m_levelerMotor.set(input);
     }
     else{
       m_levelerMotor.set(0);
     }
   }
+
+  /**
+   * Passes subsystem needed.
+   * @param subsystem Subsystem needed.
+   */
+  public void passRequiredSubsystem(Climber subsystem) {
+    m_climberSubsystem = subsystem;
+  }
 }