From 87d5d50c142ee8b0bfab1efd1c79123d9fbc3c3b Mon Sep 17 00:00:00 2001 From: Nosoup831 Date: Fri, 18 Feb 2022 16:42:20 -0800 Subject: [PATCH 1/2] Programmed the climber --- src/main/java/frc/robot/Constants.java | 54 ++++++ src/main/java/frc/robot/RobotContainer.java | 13 +- .../robot/commands/ClimberExtendsArmTwoC.java | 43 +++++ .../frc/robot/commands/ClimberExtendsC.java | 43 +++++ .../frc/robot/commands/ClimberFoldDownC.java | 30 +++ .../frc/robot/commands/ClimberFoldUpC.java | 30 +++ .../commands/ClimberRetractsArmTwoC.java | 44 +++++ .../frc/robot/commands/ClimberRetractsC.java | 43 +++++ .../java/frc/robot/subsystems/ClimberS.java | 171 ++++++++++++++++++ 9 files changed, 470 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/ClimberExtendsArmTwoC.java create mode 100644 src/main/java/frc/robot/commands/ClimberExtendsC.java create mode 100644 src/main/java/frc/robot/commands/ClimberFoldDownC.java create mode 100644 src/main/java/frc/robot/commands/ClimberFoldUpC.java create mode 100644 src/main/java/frc/robot/commands/ClimberRetractsArmTwoC.java create mode 100644 src/main/java/frc/robot/commands/ClimberRetractsC.java create mode 100644 src/main/java/frc/robot/subsystems/ClimberS.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc47eb6..0b7e4b7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,6 +18,7 @@ public final class Constants { public static final int CAN_ID_BACK_LEFT_DRIVE_MOTOR = 12; public static final int CAN_ID_BACK_RIGHT_DRIVE_MOTOR = 10; public static final int USB_PORT_DRIVER_CONTROLLER = 0; + public static final int USB_PORT_OPERATOR_CONTROLLER = 1; public static final double DRIVEBASE_DEADBAND = 0.02; // anything under 1.5% of full joystick away from 0 should be considered 0 public static final double DRIVEBASE_TURN_SLEW_LIMIT = 2.0; // 0 to 200% in one second. 0 to full in 1/2 second. public static final double DRIVEBASE_FWD_BACK_SLEW_LIMIT = 1; // 0 to 100% in one second. @@ -67,5 +68,58 @@ public final class Constants { public static final double INTAKE_SPEED = 0.25; public static final double INTAKE_EJECT_SPEED = -0.5; + //Climber Constants + /** + * The proportional constant for the climber + */ + public static final double CLIMBER_P_CONSTANT = 0; + /** + * The integral constant for the climber + */ + public static final double CLIMBER_I_CONSTANT = 0; + /** + * The derivative constant for the climber + */ + public static final double CLIMBER_D_CONSTANT = 0; + /** + * The CAN ID of the climber motor + */ + public static final int CAN_ID_CLIMBER_MOTOR = 0; + /** + * The PID error of the climber + */ + public static final int CLIMBER_PID_ERROR = 10; + /** + * The max amount of rotations extending the climber + */ + public static final int CLIMBER_SOFT_LIMIT_FORWARD = 20; + /** + * The maximum a mount of rotations retracting the climber + */ + public static final int CLIMBER_SOFT_LIMIT_BACK = -1; + /** + * The port number of the double solenoid when it folds the climber up + */ + public static final int DOUBLE_SOLENOID_CLIMBER_UP = 4; + /** + * The port number of the double solenoid when it folds the climber down + */ + public static final int DOUBLE_SOLENOID_CLIMBER_DOWN = 5; + /** + * The CAN ID of the back climb motor + */ + public static final int CAN_ID_BACK_CLIMB_MOTOR = 0; + /** + * The proportional constant for the back climber + */ + public static final int BACK_CLIMBER_P_CONSTANT = 0; + /** + * The integral constant for the back climber + */ + public static final int BACK_CLIMBER_I_CONSTANT = 0; + /** + * The derivative constant for the back climber + */ + public static final int BACK_CLIMBER_D_CONSTANT = 0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae6e663..921aac3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,10 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.ClimberExtendsC; +import frc.robot.commands.ClimberRetractsC; import frc.robot.commands.ShooterC; +import frc.robot.subsystems.ClimberS; import frc.robot.subsystems.DrivebaseS; import frc.robot.subsystems.ShooterS; import frc.robot.subsystems.TurretS; @@ -35,22 +38,25 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ private XboxController driverController; + private XboxController operatorController; private Command xboxDriveCommand; private Command xboxShooterCommand; private DrivebaseS drivebaseS; private ShooterS shooterS; private TurretS turretS; + private ClimberS climberS; private Command runTurretC; private Command turretHomingC; private Command turretTurningC; - // Trigger definitions private Trigger spinTurretTrigger; private Trigger turretHomeTrigger; private Trigger turretTurnTrigger; + private Trigger extendsClimber; + private Trigger retractClimber; public RobotContainer() { // Configure the button bindings @@ -77,10 +83,15 @@ private void configureButtonBindings() { turretHomeTrigger.whenActive(turretHomingC); turretTurnTrigger = new Trigger(driverController::getYButton); turretTurnTrigger.whenActive(turretTurningC); + extendsClimber = new Trigger(operatorController::getAButton); + extendsClimber.whenActive(new ClimberExtendsC(climberS)); + retractClimber = new Trigger(operatorController::getBButton); + retractClimber.whenActive(new ClimberRetractsC(climberS)); } private void createControllers() { driverController = new XboxController(Constants.USB_PORT_DRIVER_CONTROLLER); + operatorController = new XboxController(Constants.USB_PORT_OPERATOR_CONTROLLER); } private void createCommands() { diff --git a/src/main/java/frc/robot/commands/ClimberExtendsArmTwoC.java b/src/main/java/frc/robot/commands/ClimberExtendsArmTwoC.java new file mode 100644 index 0000000..32c410b --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberExtendsArmTwoC.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.CommandBase; +import frc.robot.subsystems.ClimberS; + +/** + * Extends the back two arms + */ +public class ClimberExtendsArmTwoC extends CommandBase { + /** Creates a new ClimberExtendsArmTwoC. */ + ClimberS climber; + public ClimberExtendsArmTwoC(ClimberS climbers) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // 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() { + climber.setClimberRotations(9); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMotorArmTwo(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.atTarget(); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberExtendsC.java b/src/main/java/frc/robot/commands/ClimberExtendsC.java new file mode 100644 index 0000000..0709779 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberExtendsC.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.CommandBase; +import frc.robot.subsystems.ClimberS; + +/** + * Extends the climber a certain amount of rotations + */ +public class ClimberExtendsC extends CommandBase { + /** Creates a new ClimberC. */ + private ClimberS climber; + public ClimberExtendsC(ClimberS climber) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // 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() { + climber.setClimberRotations(9); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMotor(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.atTarget(); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberFoldDownC.java b/src/main/java/frc/robot/commands/ClimberFoldDownC.java new file mode 100644 index 0000000..0bf680b --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberFoldDownC.java @@ -0,0 +1,30 @@ +// 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.InstantCommand; +import frc.robot.subsystems.ClimberS; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +/** + * Folds the Climber down + */ +public class ClimberFoldDownC extends InstantCommand { + ClimberS climber; + public ClimberFoldDownC(ClimberS climbers) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.climberDown(); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberFoldUpC.java b/src/main/java/frc/robot/commands/ClimberFoldUpC.java new file mode 100644 index 0000000..d34a294 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberFoldUpC.java @@ -0,0 +1,30 @@ +// 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.InstantCommand; +import frc.robot.subsystems.ClimberS; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +/** + * Folds the Climber up + */ +public class ClimberFoldUpC extends InstantCommand { + ClimberS climber; + public ClimberFoldUpC(ClimberS climbers) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.climberUp(); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberRetractsArmTwoC.java b/src/main/java/frc/robot/commands/ClimberRetractsArmTwoC.java new file mode 100644 index 0000000..3d36c2d --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberRetractsArmTwoC.java @@ -0,0 +1,44 @@ +// 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.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ClimberS; + +/** + * Retracts the back arms + */ +public class ClimberRetractsArmTwoC extends CommandBase { + /** Creates a new ClimberRetractsArmTwoC. */ + ClimberS climber; + public ClimberRetractsArmTwoC(ClimberS climbers) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // 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() { + climber.setClimberRotations(0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMotorArmTwo(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.atTarget(); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberRetractsC.java b/src/main/java/frc/robot/commands/ClimberRetractsC.java new file mode 100644 index 0000000..5bbecb3 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberRetractsC.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.CommandBase; +import frc.robot.subsystems.ClimberS; + +/** + * Retracts the climber a certain amount of rotations + */ +public class ClimberRetractsC extends CommandBase { + /** Creates a new ClimberRetractsC. */ + private ClimberS climber; + public ClimberRetractsC(ClimberS climber) { + this.climber = climber; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // 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() { + climber.setClimberRotations(0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stopMotor(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return climber.atTarget(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberS.java b/src/main/java/frc/robot/subsystems/ClimberS.java new file mode 100644 index 0000000..22ca2c9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberS.java @@ -0,0 +1,171 @@ +// 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 javax.xml.namespace.QName; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxRelativeEncoder; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class ClimberS extends SubsystemBase { + private CANSparkMax sparkMax = new CANSparkMax(Constants.CAN_ID_CLIMBER_MOTOR, MotorType.kBrushless); + private RelativeEncoder sparkMaxEncoder = sparkMax.getEncoder(SparkMaxRelativeEncoder.Type.kHallSensor, 42); + private PIDController climberPID = new PIDController(Constants.CLIMBER_P_CONSTANT, Constants.CLIMBER_I_CONSTANT, Constants.CLIMBER_D_CONSTANT); + private DoubleSolenoid doubleSolenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, Constants.DOUBLE_SOLENOID_CLIMBER_UP, + Constants.DOUBLE_SOLENOID_CLIMBER_DOWN); + private CANSparkMax backSparkMax = new CANSparkMax(Constants.CAN_ID_BACK_CLIMB_MOTOR, MotorType.kBrushless); + private RelativeEncoder sparkMaxEncoderTwo = backSparkMax.getEncoder(SparkMaxRelativeEncoder.Type.kHallSensor, 42); + private PIDController climberPIDTwo = new PIDController(Constants.BACK_CLIMBER_P_CONSTANT, Constants.BACK_CLIMBER_I_CONSTANT, + Constants.BACK_CLIMBER_D_CONSTANT); + /** Creates a new ClimberS. */ + public ClimberS() { + sparkMax.restoreFactoryDefaults(); + sparkMax.enableSoftLimit(SoftLimitDirection.kForward, true); + sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true); + sparkMax.setSoftLimit(SoftLimitDirection.kForward, Constants.CLIMBER_SOFT_LIMIT_FORWARD); + sparkMax.setSoftLimit(SoftLimitDirection.kReverse, Constants.CLIMBER_SOFT_LIMIT_BACK); + climberPID.setTolerance(Constants.CLIMBER_PID_ERROR, 0); + climberPID.setIntegratorRange(0, 0); + climberPIDTwo.setTolerance(Constants.CLIMBER_PID_ERROR, 0); + climberPIDTwo.setIntegratorRange(0, 0); + } + + /** + * Extends the climber at a speed of 0.1 + */ + public void extend() { + sparkMax.set(0.1); + } + + /** + * Retracts the climber at a speed of -0.1 + */ + public void retract() { + sparkMax.set(-0.1); + } + + /** + * Gets the number of encoder counts + * + * @return the position of the climber + */ + public double getEncoderCounts() { + return sparkMaxEncoder.getPosition(); + } + + /** + * Stops the motor + */ + public void stopMotor() { + sparkMax.set(0); + } + + /** + * sets the speed of the climber from -1 to 1 + * ensures the position of the climber is within the soft limit + * calculates the power to give to the motor + * + * @param target the desired amount of rotations + */ + public void setClimberRotations(double target) { + if (target > Constants.CLIMBER_SOFT_LIMIT_FORWARD) { + target = Constants.CLIMBER_SOFT_LIMIT_FORWARD; + } + if (target < Constants.CLIMBER_SOFT_LIMIT_BACK) { + target = Constants.CLIMBER_SOFT_LIMIT_BACK; + } + sparkMax.set(MathUtil.clamp(climberPID.calculate(this.getEncoderCounts(), target), -1, 1)); + } + + /** + * gets the position error + * + * @return the position error + */ + public double error() { + return climberPID.getPositionError(); + } + + /** + * Determines whether the climber is at the desired position + * + * @return true or false if the climber is at the desired position + */ + public boolean atTarget(){ + return climberPID.atSetpoint(); + } + + /** + * Extends the climber so that it folds up + */ + public void climberUp() { + doubleSolenoid.set(Value.kForward); + } + + /** + * Retracts the climber so that it folds down + */ + public void climberDown() { + doubleSolenoid.set(Value.kReverse); + } + + /** + * Sets back spark max speed to 0.1. + */ + public void extendArmTwo() { + backSparkMax.set(0.1); + } + /** + * Set back spark max speed to -0.1. + */ + public void restractArmTwo() { + backSparkMax.set(-0.1); + } + /** + * Get encoder counts for 2nd spark max. + * + * @return sparkMaxEncoderTwo.getPosition() + */ + public double getEncoderCountsArmTwo() { + return sparkMaxEncoderTwo.getPosition(); + } + + /** + * Stops the motor + */ + public void stopMotorArmTwo() { + backSparkMax.set(0); + } + /** + * Set rotations for motor. + * + * @param target + */ + public void setClimberRotationsArmTwo(double target) { + if (target > Constants.CLIMBER_SOFT_LIMIT_FORWARD) { + target = Constants.CLIMBER_SOFT_LIMIT_FORWARD; + } + if (target < Constants.CLIMBER_SOFT_LIMIT_BACK) { + target = Constants.CLIMBER_SOFT_LIMIT_BACK; + } + backSparkMax.set(MathUtil.clamp(climberPIDTwo.calculate(this.getEncoderCountsArmTwo(), target), -1, 1)); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From e532399f7b2a6fa0e1df7ab7b264b7860ebf9e8d Mon Sep 17 00:00:00 2001 From: gaknsmk Date: Sat, 26 Feb 2022 10:52:44 -0800 Subject: [PATCH 2/2] testing 3 position solenoid --- src/main/java/frc/robot/Constants.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 35 ++++++++++++---- .../java/frc/robot/commands/ClimberBackC.java | 40 ++++++++++++++++++ .../frc/robot/commands/ClimberForwardC.java | 41 +++++++++++++++++++ .../commands/turret/TurretCommandFactory.java | 2 +- .../java/frc/robot/subsystems/ClimberS.java | 7 ++++ 6 files changed, 118 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ClimberBackC.java create mode 100644 src/main/java/frc/robot/commands/ClimberForwardC.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0b7e4b7..ed9fa56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -100,15 +100,15 @@ public final class Constants { /** * The port number of the double solenoid when it folds the climber up */ - public static final int DOUBLE_SOLENOID_CLIMBER_UP = 4; + public static final int DOUBLE_SOLENOID_CLIMBER_UP = 0; /** * The port number of the double solenoid when it folds the climber down */ - public static final int DOUBLE_SOLENOID_CLIMBER_DOWN = 5; + public static final int DOUBLE_SOLENOID_CLIMBER_DOWN = 1; /** * The CAN ID of the back climb motor */ - public static final int CAN_ID_BACK_CLIMB_MOTOR = 0; + public static final int CAN_ID_BACK_CLIMB_MOTOR = 1; /** * The proportional constant for the back climber */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 921aac3..c443434 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,12 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.ClimberBackC; import frc.robot.commands.ClimberExtendsC; +import frc.robot.commands.ClimberForwardC; import frc.robot.commands.ClimberRetractsC; +import frc.robot.commands.ClimberForwardC; +import frc.robot.commands.ClimberBackC; import frc.robot.commands.ShooterC; import frc.robot.subsystems.ClimberS; import frc.robot.subsystems.DrivebaseS; @@ -50,13 +54,17 @@ public class RobotContainer { private Command runTurretC; private Command turretHomingC; private Command turretTurningC; + private Command climberForwardC; + private Command climberBackC; // Trigger definitions private Trigger spinTurretTrigger; private Trigger turretHomeTrigger; private Trigger turretTurnTrigger; - private Trigger extendsClimber; - private Trigger retractClimber; + //private Trigger extendsClimber; + //private Trigger retractClimber; + private Trigger climberForward; + private Trigger climberBack; public RobotContainer() { // Configure the button bindings @@ -76,17 +84,22 @@ public RobotContainer() { */ private void configureButtonBindings() { - new Trigger(driverController::getAButton).whileActiveOnce(xboxShooterCommand); - spinTurretTrigger = new Trigger(driverController::getBButton); + //new Trigger(driverController::getAButton).whileActiveOnce(xboxShooterCommand); + /*spinTurretTrigger = new Trigger(driverController::getBButton); spinTurretTrigger.whileActiveOnce(runTurretC); turretHomeTrigger = new Trigger(driverController::getXButton); turretHomeTrigger.whenActive(turretHomingC); turretTurnTrigger = new Trigger(driverController::getYButton); - turretTurnTrigger.whenActive(turretTurningC); - extendsClimber = new Trigger(operatorController::getAButton); - extendsClimber.whenActive(new ClimberExtendsC(climberS)); - retractClimber = new Trigger(operatorController::getBButton); - retractClimber.whenActive(new ClimberRetractsC(climberS)); + turretTurnTrigger.whenActive(turretTurningC);*/ + //extendsClimber = new Trigger(operatorController::getAButton); + //extendsClimber.whenActive(new ClimberExtendsC(climberS)); + //retractClimber = new Trigger(operatorController::getBButton); + //retractClimber.whenActive(new ClimberRetractsC(climberS)); + climberForward = new Trigger(operatorController::getAButton); + climberForward.whileActiveOnce(climberForwardC); + climberBack = new Trigger(operatorController::getBButton); + climberBack.whileActiveOnce(climberBackC); + } private void createControllers() { @@ -115,11 +128,15 @@ private void createCommands() { turretTurningC = TurretCommandFactory.createTurretTurnC(40, turretS); SmartDashboard.putData(new InstantCommand(turretS::resetEncoder)); + + climberForwardC = new ClimberForwardC(climberS); + climberBackC = new ClimberBackC(climberS); } private void createSubsystems() { drivebaseS = new DrivebaseS(); turretS = new TurretS(); + climberS = new ClimberS(); } /** diff --git a/src/main/java/frc/robot/commands/ClimberBackC.java b/src/main/java/frc/robot/commands/ClimberBackC.java new file mode 100644 index 0000000..ae8c9ec --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberBackC.java @@ -0,0 +1,40 @@ +// 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.CommandBase; +import frc.robot.subsystems.ClimberS; + +public class ClimberBackC extends CommandBase { + ClimberS climber; + /** Creates a new ClimberBackC. */ + public ClimberBackC(ClimberS climber) { + // Use addRequirements() here to declare subsystem dependencies. + this.climber = climber; + addRequirements(this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.climberDown(); + } + + // 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.climberOff(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberForwardC.java b/src/main/java/frc/robot/commands/ClimberForwardC.java new file mode 100644 index 0000000..2d111ec --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberForwardC.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.CommandBase; +import frc.robot.subsystems.ClimberS; + +public class ClimberForwardC extends CommandBase { + ClimberS climber; + /** Creates a new ClimberForwardC. */ + public ClimberForwardC(ClimberS climber) { + this.climber = climber; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.climberUp(); + } + + // 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.climberOff(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/turret/TurretCommandFactory.java b/src/main/java/frc/robot/commands/turret/TurretCommandFactory.java index 9ca9d10..25f5e25 100644 --- a/src/main/java/frc/robot/commands/turret/TurretCommandFactory.java +++ b/src/main/java/frc/robot/commands/turret/TurretCommandFactory.java @@ -70,7 +70,7 @@ public static Command createTurretManualC(DoubleSupplier speedSupplier, TurretS interrupted -> { turretS.stopMotor(); }, - () -> false + () -> false, turretS ) .withName("TurretManualC"); } diff --git a/src/main/java/frc/robot/subsystems/ClimberS.java b/src/main/java/frc/robot/subsystems/ClimberS.java index 22ca2c9..7d3f68a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberS.java +++ b/src/main/java/frc/robot/subsystems/ClimberS.java @@ -122,6 +122,13 @@ public void climberDown() { doubleSolenoid.set(Value.kReverse); } + /** + * Sets the climber to Off so that it locks in place + */ + public void climberOff() { + doubleSolenoid.set(Value.kOff); + } + /** * Sets back spark max speed to 0.1. */