diff --git a/docs/imgs/climber-state-machine.png b/docs/imgs/climber-state-machine.png new file mode 100644 index 0000000..4fb1ceb Binary files /dev/null and b/docs/imgs/climber-state-machine.png differ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b304cb7..66c01a9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,12 +9,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.algaeflywheel.AlgaeRoller; import frc.robot.algaepivot.AlgaeSubsystem; import frc.robot.auton.AutonMaster; +import frc.robot.climber.Climber; import frc.robot.driver.DriverXbox; import frc.robot.drivetrain.CommandSwerveDrivetrain; import frc.robot.drivetrain.TunerConstants; @@ -24,7 +23,6 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import frc.robot.operator.OperatorXbox; import frc.robot.vision.PoseEstimatorSubsystem; -import frc.robot.vision.VisionConfig; import frc.robot.rushinator.*; import frc.robot.rushinator.commands.*; @@ -38,7 +36,7 @@ public class RobotContainer { public static double kMaxVelocity = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); public static double kMaxAngularVelocity = RotationsPerSecond.of(1.0).in(RadiansPerSecond); - + public static boolean modeFast = true; /* Setting up bindings for necessary control of the swerve drive platform */ @@ -70,7 +68,7 @@ public RobotContainer() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // return CommandSwerveDrivetrain.getInstance().applyRequest(() -> + // return CommandSwerveDrivetrain.getInstance().applyRequest(() -> // RobotContainer.drive.withVelocityX(0.25 * RobotContainer.kMaxVelocity) // Drive forward with negative Y (forward) // .withVelocityY(0.0) // Drive left with negative X (left) // .withRotationalRate(0.0) // Drive counterclockwise with negative X (left) @@ -84,6 +82,10 @@ public void setDefaultCommands() { final var operator = OperatorXbox.getInstance(); final var currAlliance = DriverStation.getAlliance().get(); + // TODO: Re-enable climber once setpoints have been tuned + // final var climber = new Climber(operator::getClimberDeploy, operator::getClimberRetract, operator::getClimberOverride, Climber.OperatingMode.kManual); + // climber.setDefaultCommand(new Climber.DefaultCommand(climber)); + CommandSwerveDrivetrain.getInstance().setDefaultCommand( CommandSwerveDrivetrain.getInstance().applyRequest(() -> { if (currAlliance == Alliance.Blue){ @@ -143,7 +145,7 @@ public void setDefaultCommands() { // } // RushinatorWrist.getInstance().setDefaultCommand(new HoldWristPosition()); // RushinatorWrist.getInstance().setDefaultCommand(new ManualWristControl(() -> operator.getLeftY())); - + // CommandSwerveDrivetrain.getInstance().setDefaultCommand( // CommandSwerveDrivetrain.getInstance().applyRequest(() -> { @@ -172,12 +174,12 @@ public void setDefaultCommands() { ); RushinatorWrist.getInstance().setDefaultCommand( new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelRight), - new SetWristState(RushinatorWrist.State.kTravelLeft), + new SetWristState(RushinatorWrist.State.kTravelRight), + new SetWristState(RushinatorWrist.State.kTravelLeft), () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid || RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || @@ -185,10 +187,10 @@ public void setDefaultCommands() { RushinatorWrist.kLastState == RushinatorWrist.State.kClimblRight ) ); - + AlgaeRoller.getInstance().setDefaultCommand(new AlgaeRoller.DefaultCommand()); - + // CoralSubsystem.getInstance().setDefaultCommand(new CoralSubsystem.TuningCommand(() -> (driver.getRightX() + 1) / 2.0f)); } } - \ No newline at end of file + diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 63df640..43cc6ea 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -1,12 +1,5 @@ package frc.robot.climber; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -15,173 +8,195 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.BangBangController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.rushinator.RushinatorPivot; -public class Climber extends SubsystemBase{ - public static class Settings { - static final int kClimberPivotId = 27; //TODO: change this to sparkmax ID - - // static final InvertedValue kClimberPivotInverted = InvertedValue.Clockwise_Positive; - static final boolean kClimberPivotInverted = false; - - static final int kStallCurrentLimit = 80; // in amps - - static final double kG = 0.0; // V - static final double kS = 0.0; // V / rad - static final double kV = 0.0; // V * sec / rad - static final double kA = 0.0; // V * sec^2 / rad - - static final double kP = 0.0; - static final double kI = 0.0; - static final double kD = 0.0; +import java.util.ArrayList; +import java.util.function.Supplier; +import java.util.stream.Collectors; - public static final Rotation2d kMaxVelocity = Rotation2d.fromDegrees(200); //120 - public static final Rotation2d kMaxAcceleration = Rotation2d.fromDegrees(300); +public class Climber extends SubsystemBase { + public static class Settings { + static final int kClimberPivotId = 27; - public static final Rotation2d kMaxPos = Rotation2d.fromRotations(0.8); //TODO: change this - public static final Rotation2d kMinPos = Rotation2d.fromRotations(0.0); //TODO: change this + public static final Rotation2d kMaxPos = Rotation2d.fromRotations(270.0); + public static final Rotation2d kMinPos = Rotation2d.fromRotations(0.0); - public static final Rotation2d kAFFAngleOffset = Rotation2d.fromDegrees(0); + public static final Rotation2d kHoldPos = Rotation2d.fromRotations(263.0); + public static final Rotation2d kDeployPos = Rotation2d.fromRotations(180.0); - static final double kCurrentLimit = 40.0; + static final double kMaxVoltage = 12.0f; + static final int kMaxCurrent = 40; } - public enum State { - //TODO: redo these states based on the climber encoder position - kDeploy(Rotation2d.fromRotations(150.91650390625)), //TODO - kRetract(Rotation2d.fromRotations(80.5693359375)), //TODO - kStow(Rotation2d.fromRotations(0)); - - State(Rotation2d pos) { - this.pos = pos; - - } - public final Rotation2d pos; + private enum State { + kFloating, + kStowed, + kDeploying, + kDeployed, + kRetracting, + kHold; } - public static Climber mInstance; - - // private TalonFX ClimberPivot; - // private final ProfiledPIDController mPPIDController; - private Constraints mConstraints; - private final BangBangController mBBController; - private final ArmFeedforward mAFFController; - - private SparkMax mClimberPivotMotor; - private SparkMaxConfig mClimberPivotMotorConfig; - private RelativeEncoder mClimberPivotMotorEncoder; - - public static State kLastState; - - public Climber() { - mClimberPivotMotor = new SparkMax(Settings.kClimberPivotId, MotorType.kBrushless); - - mClimberPivotMotorConfig = new SparkMaxConfig(); - mClimberPivotMotorConfig.inverted(Settings.kClimberPivotInverted); - mClimberPivotMotorConfig.smartCurrentLimit(Settings.kStallCurrentLimit); - mClimberPivotMotorConfig.idleMode(IdleMode.kBrake); - mClimberPivotMotor.configure(mClimberPivotMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - mClimberPivotMotorEncoder = mClimberPivotMotor.getEncoder(); - - mAFFController = new ArmFeedforward(Settings.kS, Settings.kG, Settings.kV, Settings.kA); - - mBBController = new BangBangController(2.0f); // Tolerance in rotations - - if (kLastState == null) { - kLastState = State.kStow; - } - // mPPIDController.setGoal(kLastState.pos.getRotations()); - - - // ClimberPivot = new TalonFX(Settings.kClimberPivotId); - - // var ElbowPivotConfigurator = ClimberPivot.getConfigurator(); - - // var ElbowPivotConfigs = new MotorOutputConfigs(); - - // ClimberPivot.getConfigurator().apply(new TalonFXConfiguration().withMotorOutput(new MotorOutputConfigs() - // .withInverted(InvertedValue.Clockwise_Positive) - // .withNeutralMode(NeutralModeValue.Brake) - // )); - // ClimberPivot.getConfigurator().apply(new CurrentLimitsConfigs().withSupplyCurrentLimit(Settings.kCurrentLimit)); - - // set invert to CW+ and apply config change - // ElbowPivotConfigs.Inverted = Settings.kClimberPivotInverted; + public enum OperatingMode { + kManual, + kCompetition + } - // ElbowPivotConfigurator.apply(ElbowPivotConfigs); + private final SparkMax spark; + private final RelativeEncoder encoder; - // mPPIDController = new ProfiledPIDController(Settings.kP, Settings.kI, Settings.kD, new TrapezoidProfile.Constraints( - // Settings.kMaxVelocity.getRadians(), - // Settings.kMaxAcceleration.getRadians() - // )); - // mPPIDController.setTolerance(0.01); - + private final Supplier deployProvider, retractProvider; + private final Supplier overrideProvider; - // ClimberPivot.setPosition(0.0); + private State state; + private final OperatingMode operatingMode; - // ClimberPivot.getConfigurator().apply(new CurrentLimitsConfigs().withSupplyCurrentLimit(Settings.kCurrentLimit)); + public Climber(Supplier deployProvider, Supplier retractProvider, Supplier overrideProvider) { + this(deployProvider, retractProvider, overrideProvider, OperatingMode.kCompetition); } - public static Climber getInstance() { - if (mInstance == null) { - mInstance = new Climber(); - } - return mInstance; + public Climber(Supplier deployProvider, Supplier retractProvider, Supplier overrideProvider, OperatingMode operatingMode) { + this.deployProvider = deployProvider; + this.retractProvider = retractProvider; + this.overrideProvider = overrideProvider; + + this.spark = new SparkMax(Settings.kClimberPivotId, MotorType.kBrushless); + this.encoder = spark.getEncoder(); + + final var sparkConfig = new SparkMaxConfig() + .inverted(true) + .smartCurrentLimit(Settings.kMaxCurrent) + .idleMode(IdleMode.kBrake) ; + spark.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + this.state = switch (operatingMode) { + case kManual -> State.kFloating; + case kCompetition -> State.kStowed; + }; + this.operatingMode = operatingMode; } - public void setTargetPos(Rotation2d pos) { - // mPPIDController.setGoal(pos.getRotations()); - mBBController.setSetpoint(pos.getRotations()); + public void setVoltage(double voltage) { + // Ensure we don't drive past either of the soft limits for the climber by clamping the voltage + if ((voltage < 0 && getPos().getRotations() <= Settings.kMinPos.getRotations()) + || (voltage > 0 && getPos().getRotations() >= Settings.kMaxPos.getRotations())) { + voltage = 0; + } + spark.setVoltage(voltage); } public Rotation2d getPos() { - return Rotation2d.fromRotations(mClimberPivotMotorEncoder.getPosition()); - } - - public Rotation2d getAngularVelocity() { - // return Rotation2d.fromRotations(ClimberPivot.getVelocity().getValueAsDouble()); - return Rotation2d.fromRotations(mClimberPivotMotorEncoder.getVelocity()); //TODO: i dont know what the unit conversions are + return Rotation2d.fromRotations(encoder.getPosition()); } - public static class DefaultCommand extends Command { - - public DefaultCommand() { - addRequirements(Climber.getInstance()); + private State nextState(State state) { + if (operatingMode == OperatingMode.kManual) { + return State.kFloating; } - @Override - public void execute() { - Climber.getInstance().setTargetPos(State.kStow.pos); + if (overrideProvider.get() != 0.0f) { + return State.kFloating; } + ArrayList observedStates = new ArrayList<>(); + while (true) { + var previousState = state; + observedStates.add(previousState); + switch (state) { + case kFloating: + if (deployProvider.get()) { + state = State.kDeploying; + } else if (retractProvider.get()) { + state = State.kRetracting; + } + break; + case kStowed: + if (deployProvider.get()) { + state = State.kDeploying; + } + break; + case kDeploying: + if (retractProvider.get()) { + state = State.kFloating; + } else if (getPos().getRotations() >= Settings.kDeployPos.getRotations()) { + state = State.kDeployed; + } + break; + case kDeployed: + if (retractProvider.get()) { + state = State.kRetracting; + } + break; + case kRetracting: + if (deployProvider.get()) { + state = State.kFloating; + } else if (getPos().getRotations() <= Settings.kHoldPos.getRotations()) { + state = State.kHold; + } + break; + case kHold: + if (deployProvider.get()) { + state = State.kFloating; + } + // TODO: If a low enough current is detected for a sufficiently long time, we should transition to floating. + // This will prevent over-driving the climber when it isn't under load. + break; + } + + // If we don't transition state in an iteration, we've reached the terminal state and can return + if (previousState == state) { + return state; + } + + // If the list of observed states contains the current state, we've entered a loop. This shouldn't occur and + // indicates an issue in the state machine, but we should log a warning before breaking the loop. + if (observedStates.contains(state)) { + System.out.printf("[WARN:CLIMBER] Encountered loop while transitioning climber state (states: %s)", observedStates.stream().map(Object::toString).collect(Collectors.joining(", "))); + return state; + } + } } @Override public void periodic() { - SmartDashboard.putNumber("Climber Pivot Angle (Rotations)", getPos().getRotations()); - SmartDashboard.putNumber("Climber Pivot Angular Velocity (Rotations / sec)", getAngularVelocity().getRotations() * 60.0); - SmartDashboard.putNumber("Climber Pivot Current (Amps)", mClimberPivotMotor.getOutputCurrent()); - - // SmartDashboard.putNumber("Climber Target Pos", mPPIDController.getSetpoint().position); - // SmartDashboard.putNumber("Target Vel", mPPIDController.getSetpoint().velocity); + SmartDashboard.putNumber("Climber Angle (Rotations)", getPos().getRotations()); + SmartDashboard.putNumber("Climber Current (Amps)", spark.getOutputCurrent()); + + switch (state) { + case kFloating: + setVoltage(overrideProvider.get() * Settings.kMaxVoltage); + break; + case kStowed: + setVoltage(0); + break; + case kDeploying: + setVoltage(8.0f); + break; + case kDeployed: + setVoltage(0.0f); + break; + case kRetracting: + setVoltage(-6.0f); + break; + case kHold: + // TODO: Make this value proportional to the distance from the zero point + setVoltage(-2.0f); + break; + } - SmartDashboard.putNumber("Climber Target Pos", mBBController.getSetpoint()); - SmartDashboard.putNumber("Target Vel (RPS)", mBBController.getSetpoint() / 60.0); + final var ns = nextState(state); + if (ns != state) { + System.out.printf("[INFO:CLIMBER] Transitioning state (previous: %s, new: %s)", state.toString(), ns.toString()); + state = ns; + } + } - // Method to run pivots - double voltage = mBBController.calculate(getPos().getRotations()) * 2.0f; - voltage += mAFFController.calculate(getPos().getRotations(), mBBController.getSetpoint() / 60.0); - mClimberPivotMotor.setVoltage(voltage); - SmartDashboard.putNumber("Climber Pivot Voltage", voltage); + public static class DefaultCommand extends Command { + public DefaultCommand(Climber climber) { + addRequirements(climber); + } } } diff --git a/src/main/java/frc/robot/climber/commands/ClimberPivotCommands.java b/src/main/java/frc/robot/climber/commands/ClimberPivotCommands.java deleted file mode 100644 index 1ac7762..0000000 --- a/src/main/java/frc/robot/climber/commands/ClimberPivotCommands.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot.climber.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.climber.Climber; - -public class ClimberPivotCommands { - public static Command setClimberAngle(Climber.State state) { - return new SetClimberAngle(state.pos); - } - - public static Command setClimberAngle(Supplier targetSupplier) { - return new SetClimberAngle(targetSupplier); - } -} diff --git a/src/main/java/frc/robot/climber/commands/SetClimberAngle.java b/src/main/java/frc/robot/climber/commands/SetClimberAngle.java deleted file mode 100644 index de70008..0000000 --- a/src/main/java/frc/robot/climber/commands/SetClimberAngle.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.climber.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.climber.Climber; - -public class SetClimberAngle extends Command{ - Climber mClimber; - Supplier targetSupplier; - - public SetClimberAngle(Rotation2d angleIn) { - mClimber = Climber.getInstance(); - targetSupplier = ()-> angleIn; - } - public SetClimberAngle(Supplier angleIn) { - mClimber = Climber.getInstance(); - targetSupplier = angleIn; - } - - @Override - public void initialize() { - mClimber.setTargetPos(targetSupplier.get()); - } - - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - - } -} diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java index 47ff131..5d37333 100644 --- a/src/main/java/frc/robot/driver/DriverXbox.java +++ b/src/main/java/frc/robot/driver/DriverXbox.java @@ -19,7 +19,6 @@ import frc.robot.algaepivot.commands.SetAngleAlgaePivot; import frc.robot.auton.AutonMaster; import frc.robot.climber.Climber; -import frc.robot.climber.commands.SetClimberAngle; import frc.robot.commands.RobotCommands; import frc.robot.drivetrain.CommandSwerveDrivetrain; import frc.robot.elevator.ElevatorSubsystem; diff --git a/src/main/java/frc/robot/operator/OperatorXbox.java b/src/main/java/frc/robot/operator/OperatorXbox.java index 8e0f6fb..63fab83 100644 --- a/src/main/java/frc/robot/operator/OperatorXbox.java +++ b/src/main/java/frc/robot/operator/OperatorXbox.java @@ -5,10 +5,7 @@ import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.crevolib.util.ExpCurve; import frc.crevolib.util.Gamepad; @@ -20,7 +17,6 @@ import frc.robot.algaepivot.commands.AlgaePivotCommands; import frc.robot.algaepivot.commands.SetAngleAlgaePivot; import frc.robot.climber.Climber; -import frc.robot.climber.commands.SetClimberAngle; import frc.robot.commands.RobotCommands; import frc.robot.driver.DriverXbox; import frc.robot.drivetrain.commands.DrivetrainCommands; @@ -47,6 +43,9 @@ private static class Settings { ExpCurve intakePivotManualCurve; ExpCurve positionTestCurve; ExpCurve elevatorCurve; + + final ExpCurve climberOverrideCurve; + private static OperatorXbox mInstance; ExpCurve shooterManualCurve; @@ -55,6 +54,7 @@ private OperatorXbox() { super(Settings.name, Settings.port); stickCurve = new ExpCurve(1, 0, 1, Settings.kDeadzone); + climberOverrideCurve = new ExpCurve(1, 0, 1, Settings.kDeadzone); } public static OperatorXbox getInstance() { @@ -68,6 +68,14 @@ public static OperatorXbox getInstance() { public void setupTeleopButtons() { /*Comp Bindings */ + // Climber bindings + // TODO: Enable climbing commands once setpoints have been tuned +// controller.povLeft().and(leftTriggerOnly()).onTrue(new SetElevatorState(State.kClimb)); +// controller.povLeft().and(leftTriggerOnly()).onTrue(new SequentialCommandGroup( +// new WaitCommand(0.4), +// new SetAngleAlgaePivot(AlgaeSubsystem.State.kClimb) +// )); + // Algae Reef Intake controller.leftBumper().and(leftTriggerOnly()).whileTrue(new AlgaeRoller.IntakeCommand()); controller.leftBumper().and(leftTriggerOnly()).whileTrue(new SetAngleAlgaePivot(AlgaeSubsystem.State.kReefIntake)); @@ -91,93 +99,93 @@ public void setupTeleopButtons() { // Algae SCorring for Process & Barge controller.rightBumper().and(leftTriggerOnly()).whileTrue(new AlgaeRoller.ProcessShootCommand()); controller.rightBumper().whileTrue(new AlgaeRoller.ShootCommand()); - + // ADjusting Coral ORinetaiton controller.povLeft().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelLeft), - new SetWristState(RushinatorWrist.State.kTravelL4Left), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight - ) + new SetWristState(RushinatorWrist.State.kTravelLeft), + new SetWristState(RushinatorWrist.State.kTravelL4Left), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight + ) ); controller.povRight().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelRight), - new SetWristState(RushinatorWrist.State.kTravelL4Right), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight - ) + new SetWristState(RushinatorWrist.State.kTravelRight), + new SetWristState(RushinatorWrist.State.kTravelL4Right), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight + ) ); // Score Prime L1 controller.a().whileTrue(RobotCommands.coralPrime( - RushinatorPivot.State.kScoreL1, ElevatorSubsystem.State.kZero) + RushinatorPivot.State.kScoreL1, ElevatorSubsystem.State.kZero) ); controller.a().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kScoreL1Mid), - new SetWristState(RushinatorWrist.State.kScoreL1Mid), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || - RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid - ) + new SetWristState(RushinatorWrist.State.kScoreL1Mid), + new SetWristState(RushinatorWrist.State.kScoreL1Mid), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || + RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid + ) ); // Score Prime L2 controller.x().whileTrue(RobotCommands.coralPrime( - RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL2) + RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL2) ); controller.x().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelRight), - new SetWristState(RushinatorWrist.State.kTravelLeft), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || - RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid - ) + new SetWristState(RushinatorWrist.State.kTravelRight), + new SetWristState(RushinatorWrist.State.kTravelLeft), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || + RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid + ) ); // Score Prime L3 controller.b().onTrue(RobotCommands.coralPrime( - RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL3) + RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL3) ); controller.b().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelRight), - new SetWristState(RushinatorWrist.State.kTravelLeft), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || - RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid - ) + new SetWristState(RushinatorWrist.State.kTravelRight), + new SetWristState(RushinatorWrist.State.kTravelLeft), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || + RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid + ) ); // Score Prime L4 controller.y().onTrue(RobotCommands.coralPrime( - RushinatorPivot.State.kStowL4, ElevatorSubsystem.State.kCoralL4) + RushinatorPivot.State.kStowL4, ElevatorSubsystem.State.kCoralL4) ); controller.y().onTrue(new ConditionalCommand( - new SetWristState(RushinatorWrist.State.kTravelL4Right), - new SetWristState(RushinatorWrist.State.kTravelL4Left), - () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || - RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || - RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid || - RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || - RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid - ) + new SetWristState(RushinatorWrist.State.kTravelL4Right), + new SetWristState(RushinatorWrist.State.kTravelL4Left), + () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || + RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || + RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid || + RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || + RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid + ) ); // Algae L3 @@ -245,7 +253,7 @@ public void setupTeleopButtons() { // controller.leftTrigger().whileTrue(IndexerCommands.setOutput(() -> 1.0)); // controller.leftBumper().whileTrue(new AlgaeRoller.ShootCommand()); -/*Used Commands */ + /*Used Commands */ // controller.leftTrigger().whileTrue(new SetAngleAlgaePivot(AlgaeSubsystem.State.kFloorIntake)); // controller.leftTrigger().whileTrue(new AlgaeRoller.PrimeCommand()); // controller.leftBumper().whileTrue(new AlgaeRoller.ShootCommand()); @@ -285,6 +293,20 @@ public void setupDisabledButtons() { public void setupTestButtons() { } + public boolean getClimberRetract() { + return controller.leftTrigger().getAsBoolean() && controller.povLeft().getAsBoolean(); + } + + public boolean getClimberDeploy() { + return controller.leftTrigger().getAsBoolean() && controller.povRight().getAsBoolean(); + } + + public double getClimberOverride() { + if (!controller.leftTrigger().getAsBoolean()) { + return 0.0f; + } + return climberOverrideCurve.calculate(controller.getLeftX()); + } public Translation2d getDriveTranslation() { @@ -302,7 +324,7 @@ public double getDriveRotation() { return -stickCurve.calculate(-controller.getRightX()); } - public double getLeftY() { + public double getLeftY() { return controller.getLeftY(); } }