diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java index 48ffd14..5137ec7 100644 --- a/src/main/java/org/team199/robot2022/RobotContainer.java +++ b/src/main/java/org/team199/robot2022/RobotContainer.java @@ -11,8 +11,6 @@ import org.team199.robot2022.commands.PassiveAutomaticIntake; import org.team199.robot2022.commands.PassiveManualIntake; import org.team199.robot2022.commands.Regurgitate; -import org.team199.robot2022.commands.ResetAndExtendClimber; -import org.team199.robot2022.commands.ResetAndRetractClimber; import org.team199.robot2022.commands.RetractClimber; import org.team199.robot2022.commands.Shoot; import org.team199.robot2022.commands.TeleopDrive; @@ -149,9 +147,8 @@ public RobotContainer(Robot robot) { private void configureButtonBindingsLeftJoy() { new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualAddPort).whenPressed(new InstantCommand(intakeFeeder::manualAdd)); new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualSubtractPort).whenPressed(new InstantCommand(intakeFeeder::manualSub)); - new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndExtendClimberPort).whenPressed(new ResetAndExtendClimber(climber)); - new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndRetractClimberPort).whenPressed(new ResetAndRetractClimber(climber)); - new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(climber::resetEncodersToZero)); + + new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(()->climber.resetEncodersTo(Climber.EncoderPos.zero,Climber.bothMotors))); new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleDriveMode).whenPressed(new InstantCommand( () -> {SmartDashboard.putBoolean("Field Oriented", SmartDashboard.getBoolean("Field Oriented", true) ? false : true);})); new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleLongShot).whenPressed(new InstantCommand(shooter::toggleLongShot)); new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetFieldOriented).whenPressed(new SequentialCommandGroup(new InstantCommand(() -> {SmartDashboard.putBoolean("Field Oriented", true);}), new WaitCommand(0.05), new InstantCommand(() -> {SmartDashboard.putNumber("Field Offset from North (degrees)", SmartDashboard.getNumber("Field Offset from North (degrees)", 0) - dt.getHeadingDeg() + 180);}))); @@ -159,11 +156,12 @@ private void configureButtonBindingsLeftJoy() { private void configureButtonBindingsRightJoy() { new JoystickButton(rightJoy, Constants.OI.RightJoy.shootPort).whenPressed(new Shoot(intakeFeeder, shooter)); - new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(climber::slowExtendLeft)).whenReleased(new InstantCommand(climber::stopLeft)); - new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(climber::slowRetractLeft)).whenReleased(new InstantCommand(climber::stopLeft)); - new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(climber::slowExtendRight)).whenReleased(new InstantCommand(climber::stopRight)); - new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(climber::slowRetractRight)).whenReleased(new InstantCommand(climber::stopRight)); + new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor))); + new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor))); + new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor))); + new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor))); new JoystickButton(rightJoy, Constants.OI.RightJoy.toggleShooterModePort).whenPressed(new InstantCommand(shooter::toggleDutyCycleMode)); + new JoystickButton(rightJoy, Constants.OI.RightJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override)); } @@ -237,7 +235,7 @@ private double getStickValue(Constants.OI.StickType stick, Constants.OI.StickDir /** * Processes an input from the joystick into a value between -1 and 1 - * + * * @param value The value to be processed. * @return The processed value. */ diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java index a7f6313..1fcfc03 100644 --- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java +++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java @@ -15,18 +15,17 @@ public ExtendClimber(Climber climber) { super( new FunctionalCommand( () -> {}, - climber::extendLeft, - climber::stopLeft, - climber::isLeftExtended + () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.rightMotor), + (interrupted) -> climber.stopMotors(Climber.rightMotor), + () -> climber.isMotorExtended(Climber.rightMotor) ), new FunctionalCommand( () -> {}, - climber::extendRight, - climber::stopRight, - climber::isRightExtended + () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.leftMotor), + (interrupted) -> climber.stopMotors(Climber.leftMotor), + () -> climber.isMotorExtended(Climber.leftMotor) ) ); addRequirements(climber); } - } diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java deleted file mode 100644 index 43d9ac6..0000000 --- a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.team199.robot2022.commands; -import org.team199.robot2022.subsystems.Climber; - -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -public class ResetAndExtendClimber extends ParallelCommandGroup{ - public ResetAndExtendClimber(Climber climber) { - super( - new FunctionalCommand( - climber::resetEncodersToRetracted, - climber::slowExtendLeft, - climber::stopLeft, - climber::isLeftResetExtended - ), - new FunctionalCommand( - climber::resetEncodersToRetracted, - climber::slowExtendRight, - climber::stopRight, - climber::isRightResetExtended - ) - ); - addRequirements(climber); - } - -} diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java deleted file mode 100644 index 50a31ac..0000000 --- a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.team199.robot2022.commands; -import org.team199.robot2022.subsystems.Climber; - -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -public class ResetAndRetractClimber extends ParallelCommandGroup{ - public ResetAndRetractClimber(Climber climber) { - super( - new FunctionalCommand( - climber::resetEncodersToExtended, - climber::slowRetractLeft, - climber::stopLeft, - climber::isLeftResetRetracted - ), - new FunctionalCommand( - climber::resetEncodersToExtended, - climber::slowRetractRight, - climber::stopRight, - climber::isRightResetRetracted - ) - ); - addRequirements(climber); - } - -} diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java index 7b9636f..cc2823a 100644 --- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java +++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java @@ -14,19 +14,18 @@ public class RetractClimber extends ParallelCommandGroup { public RetractClimber(Climber climber) { super( new FunctionalCommand( - () -> {}, - climber::retractLeft, - climber::stopLeft, - climber::isLeftRetracted + () -> {}, + () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.rightMotor), + (interrupted) -> climber.stopMotors(Climber.rightMotor), + () -> climber.isMotorRetracted(Climber.rightMotor) ), new FunctionalCommand( - () -> {}, - climber::retractRight, - climber::stopRight, - climber::isRightRetracted + () -> {}, + () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.leftMotor), + (interrupted) -> climber.stopMotors(Climber.leftMotor), + () -> climber.isMotorRetracted(Climber.leftMotor) ) ); addRequirements(climber); } - } diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java index 9daa85f..dea90b3 100644 --- a/src/main/java/org/team199/robot2022/subsystems/Climber.java +++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java @@ -11,6 +11,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import java.util.function.DoubleConsumer; import org.team199.robot2022.Constants; @@ -20,43 +21,56 @@ public class Climber extends SubsystemBase { private static final double kDiameterIn = 1; private static final double kDesiredRetractSpeedInps = 1; private static final double kDesiredExtendSpeedInps = 24*.6; - - // Torque is 2 * 9 * 0.5 = 9 - // Torque on the motor is Torque / ( gearing = 9 ) = 1 - - private static final double kVoltsToCounterTorque = 10.5; - private static final boolean leftInverted = false; - private static final double extendPositionLeft = 6.315; - private static final double extendPositionRight = 6.315; - - private static final double retractPositionLeft = -0.6; - private static final double retractPositionRight = -0.6; private static final double gearing = 9; private static final double kInPerSec = ((kNEOFreeSpeedRPM / gearing) * Math.PI * kDiameterIn / 60); - private static double kRetractSpeed = -((kDesiredRetractSpeedInps / kInPerSec) - + (kVoltsToCounterTorque / 12)); // ~ -0.06151 - private static double kExtendSpeed = (kDesiredExtendSpeedInps / kInPerSec); // ~0.30261 - private static final double kSlowDesiredRetractSpeedInps = 2; private static final double kSlowDesiredExtendSpeedInps = 2; // Torque is 2 * 9 * 0.5 = 9 // Torque on the motor is Torque / ( gearing = 9 ) = 1 - + private static final double kVoltsToCounterTorque = 10.5; private static final double kSlowVoltsToCounterTorque = (1.1D / 32) * 12; - private static final double kSlowRetractSpeed = -((kSlowDesiredRetractSpeedInps / kInPerSec) - + (kSlowVoltsToCounterTorque / 12)); // ~ -0.06151 - private static final double kSlowExtendSpeed = (kSlowDesiredExtendSpeedInps / kInPerSec); // ~0.30261 + + private boolean keepPosition = true; + private double holdTolerance = 0.05; + + public static final int leftMotor = 0;//because someone requested this and there's no point slapping them in an enum + public static final int bothMotors = 1; + public static final int rightMotor = 2; + private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft, TemperatureLimit.NEO); private final CANSparkMax right = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberRight, TemperatureLimit.NEO); private final RelativeEncoder leftEncoder = left.getEncoder(); private final RelativeEncoder rightEncoder = right.getEncoder(); - private boolean keepPosition = true; - private double holdTolerance = 0.05; + private final DoubleConsumer[] setEncoder = new DoubleConsumer[]{ // faster to array[](inp) than a bunch of if's in a func + (pos) -> leftEncoder.setPosition(pos), + (pos) -> { + leftEncoder.setPosition(pos); + rightEncoder.setPosition(pos); + }, + (pos) -> rightEncoder.setPosition(pos), + }; + + private final DoubleConsumer[] setMotor = new DoubleConsumer[]{ + (speed) -> { + left.set(speed); + SmartDashboard.putString("Left Climber State", "Moving"); + }, + (speed) -> { + left.set(speed); + right.set(speed); + SmartDashboard.putString("Left Climber State", "Moving"); + SmartDashboard.putString("Right Climber State", "Moving"); + }, + (speed) -> { + right.set(speed); + SmartDashboard.putString("Right Climber State", "Moving"); + }, + }; public Climber() { left.setSmartCurrentLimit(80); @@ -68,16 +82,17 @@ public Climber() { leftEncoder.setPosition(0); rightEncoder.setPositionConversionFactor(1 / gearing); rightEncoder.setPosition(0); - SmartDashboard.putString("Left climber is", "Stopped"); - SmartDashboard.putString("Right climber is", "Stopped"); + SmartDashboard.putString("Left Climber State", "Stopped"); + SmartDashboard.putString("Right Climber State", "Stopped"); SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps); SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps); } @Override public void periodic() { - SmartDashboard.putNumber("Left Climber Position", getLeftPosition()); - SmartDashboard.putNumber("Right Climber Position", getRightPosition()); + SmartDashboard.putNumber("L Climber Pos", leftEncoder.getPosition()); + SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition()); + holdTolerance = SmartDashboard.getNumber("Climber: Tolerance", holdTolerance); SmartDashboard.putNumber("Climber: Tolerance", holdTolerance); SmartDashboard.putBoolean("Climber: Keep Zeroed", keepPosition); @@ -86,147 +101,86 @@ public void periodic() { public void keepZeroed() { if(keepPosition) { - if(Math.abs(getLeftPosition()) > holdTolerance) { - left.set(Math.signum(getLeftPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed); + if(Math.abs(leftEncoder.getPosition()) > holdTolerance) { + left.set(Math.signum(leftEncoder.getPosition()) > 0 ? MotorSpeed.slowRetract.value : MotorSpeed.slowExtend.value); } else { left.set(0); } - if(Math.abs(getRightPosition()) > holdTolerance) { - right.set(Math.signum(getRightPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed); + if(Math.abs(rightEncoder.getPosition()) > holdTolerance) { + right.set(Math.signum(rightEncoder.getPosition()) > 0 ? MotorSpeed.slowRetract.value : MotorSpeed.slowExtend.value); } else { right.set(0); } } } - public void resetEncodersToExtended() { - leftEncoder.setPosition(extendPositionLeft); - rightEncoder.setPosition(extendPositionRight); - } - - public void resetEncodersToRetracted() { - leftEncoder.setPosition(retractPositionLeft); - rightEncoder.setPosition(retractPositionRight); - } - public void resetEncodersToZero() { - leftEncoder.setPosition(0); - rightEncoder.setPosition(0); - } - - public void extendLeft() { - left.set(kExtendSpeed); - SmartDashboard.putString("Left climber is", "Extending"); - keepPosition = false; - } - - public void extendRight() { - right.set(kExtendSpeed); - SmartDashboard.putString("Right climber is", "Extending"); - keepPosition = false; - } - - public void retractLeft() { - left.set(kRetractSpeed); - SmartDashboard.putString("Left climber is", "Retracting"); - keepPosition = false; + //motor = -1 left or 1 right or 0 both + public void resetEncodersTo(EncoderPos posEnum,int motor){ + // setEncoder[motor+1].accept(dEncoderPos.get(pos)); + setEncoder[motor].accept(posEnum.value); } - public void retractRight() { - right.set(kRetractSpeed); - SmartDashboard.putString("Right climber is", "Retracting"); - keepPosition = false; + public void moveMotors(MotorSpeed speedEnum, int motor){ + setMotor[motor].accept(speedEnum.value); } - public void slowExtendLeft() { - left.set(kSlowExtendSpeed); - SmartDashboard.putString("Left climber is", "Extending"); - keepPosition = false; - } - - public void slowExtendRight() { - right.set(kSlowExtendSpeed); - SmartDashboard.putString("Right climber is", "Extending"); - keepPosition = false; - } - public void slowRetractLeft() { - left.set(kSlowRetractSpeed); - SmartDashboard.putString("Left climber is", "Retracting"); - keepPosition = false; - } - - public void slowRetractRight() { - right.set(kSlowRetractSpeed); - SmartDashboard.putString("Right climber is", "Retracting"); - keepPosition = false; - } - - public void stop() { + public void stopMotors(int motor){ + if (motor!=rightMotor){ left.set(0); + SmartDashboard.putString("Left Climber State", "Stopped"); + } + if (motor!=leftMotor){ right.set(0); - SmartDashboard.putString("Left climber is", "Stopped"); - SmartDashboard.putString("Right climber is", "Stopped"); - } - - public void stopLeft() { - left.set(0); - SmartDashboard.putString("Left climber is", "Stopped"); - } - - public void stopRight() { - right.set(0); - SmartDashboard.putString("Right climber is", "Stopped"); - } - - public void stop(boolean interrupted) { - stop(); - } - - public void stopLeft(boolean interrupted) { - stopLeft(); - } - - public void stopRight(boolean interrupted) { - stopRight(); - } - - public double getLeftPosition() { - return leftEncoder.getPosition(); + SmartDashboard.putString("Right Climber State", "Stopped"); + } } - public double getRightPosition() { - return rightEncoder.getPosition(); + public boolean isMotorExtended(int motor){//is the motor(s) extended? + if (motor!=rightMotor && leftEncoder.getPosition() < EncoderPos.extendLeft.value){ + return false; + } + if (motor!=leftMotor && rightEncoder.getPosition() < EncoderPos.extendRight.value){ + return false; + } + return true; } - public boolean isLeftExtended() { - return getLeftPosition() >= extendPositionLeft; + public boolean isMotorRetracted(int motor){//is the motor(s) retracted? + if (motor!=rightMotor && leftEncoder.getPosition() > EncoderPos.retractLeft.value){ + return false; + } + if (motor!=leftMotor && rightEncoder.getPosition() > EncoderPos.retractRight.value){ + return false; + } + return true; } - public boolean isRightExtended() { - return getRightPosition() >= extendPositionRight; - } + public static enum MotorSpeed{ + retract(-((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))),// ~ -0.06151 + extend(kDesiredExtendSpeedInps / kInPerSec),// ~0.30261 + slowRetract(-((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))),// ~ -0.06151 + slowExtend(kSlowDesiredExtendSpeedInps / kInPerSec);// ~0.30261 - public boolean isLeftRetracted() { - return getLeftPosition() <= retractPositionLeft; - } + public final double value; - public boolean isRightRetracted() { - return getRightPosition() <= retractPositionRight; - } + private MotorSpeed(double value) { + this.value = value; + } + }; - public boolean isRightResetExtended() { - return getRightPosition() >= 0; - } - public boolean isLeftResetExtended() { - return getLeftPosition() >= 0; - } + public static enum EncoderPos{ + extendLeft(6.315), + extendRight(6.315), + retractLeft(-0.6), + retractRight(-0.6), + zero(0.0); - public boolean isRightResetRetracted() { - return getRightPosition() <= 0; - } + public final double value; - public boolean isLeftResetRetracted() { - return getLeftPosition() <= 0; - } + private EncoderPos(double value) { + this.value = value; + } + }; }