diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 56e9545..c44884a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 diff --git a/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java b/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java new file mode 100644 index 0000000..631e23e --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java index be72d49..ffa4b68 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java index 7cf5b2f..2052ef4 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index e9adf86..fc932d0 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/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); + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index d29c748..be5e77d 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/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; + } }