From 5dc17ca1a6b3293c09dff7db99601c7d18c8aeb8 Mon Sep 17 00:00:00 2001 From: Bob Bell Date: Sun, 14 Apr 2024 15:42:55 -0400 Subject: [PATCH] Enable and recalibrate climber subsytem --- src/main/java/frc/robot/Constants.java | 14 ++--- src/main/java/frc/robot/RobotContainer.java | 57 ++++++++++--------- .../robot/subsystems/ClimberSubsystem.java | 8 +-- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7388057..c4af04a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -251,21 +251,21 @@ public class Climber { public static final InvertedValue motorOutputInverted = InvertedValue.Clockwise_Positive; public static final NeutralModeValue motorNeutralValue = NeutralModeValue.Brake; - public static final double extendedPosition = -875.0; - public static final double midPosition = -450.0; + public static final double extendedPosition = -230.0; + // public static final double midPosition = -100.0; //-450.0; public static final double retractedPosition = -15.0; - public static final double positionError = 1.5; - public static final double slowVoltage = 2.75; + public static final double positionError = 1.0; + public static final double slowVoltage = 1.5; - public static final double RPSperVolt = 8.5; // RPS increase with every volt + public static final double RPSperVolt = 7.9; // RPS increase with every volt public static final double kP = 0.50; // output per unit of error in position (output/rotation) public static final double kI = 0.0; // output per unit of integrated error in position (output/(rotation*s)) public static final double kD = 0.0; // output per unit of error in velocity (output/rps) - public static final double kS = 0.25; // output to overcome static friction (output) + public static final double kS = 0.22; // output to overcome static friction (output) public static final double kV = 1.0 / RPSperVolt; // output per unit of target velocity (output/rps) public static final double kA = 0.0; // output per unit of target acceleration (output/(rps/s)) public static final double kG = 0.0; // do not factory in gravity - public static final double cruiseVelocity = 125.0; // RPS + public static final double cruiseVelocity = 120.0; // RPS public static final double acceleration = cruiseVelocity * 0.5; // Accelerate in 0.5 seconds public static final double timeCutOff = 25.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38acb04..bf7a1aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.*; import frc.robot.subsystems.*; -// import frc.robot.subsystems.ClimberSubsystem.ClimberSelection; +import frc.robot.subsystems.ClimberSubsystem.ClimberSelection; import frc.robot.subsystems.ShooterSubsystem.Speed; /** @@ -51,15 +51,15 @@ public class RobotContainer { private final Trigger intakeButton = driver.leftBumper(); private final Trigger shooterButton = driver.rightBumper(); private final Trigger ejectButton = driver.start(); - // private final Trigger leftClimberButton = driver.leftTrigger(); - // private final Trigger rightClimberButton = driver.rightTrigger(); + private final Trigger leftClimberButton = driver.leftTrigger(); + private final Trigger rightClimberButton = driver.rightTrigger(); /* Different Position Test Buttons */ private final Trigger ampButton = driver.a(); private final Trigger dumpShotButton = driver.b(); private final Trigger defaultShotButton = driver.back(); private final Trigger slideShotButton = driver.x(); - // private final Trigger climberExtendButton = driver.y(); + private final Trigger climberExtendButton = driver.y(); private final Trigger ampShotButton = driver.povDown(); /* Subsystems */ @@ -67,8 +67,8 @@ public class RobotContainer { private final IntakeSubsystem s_Intake = new IntakeSubsystem(); private final ShooterSubsystem s_Shooter = new ShooterSubsystem(); private final IndexSubsystem s_Index = new IndexSubsystem(); - // private final ClimberSubsystem s_LeftClimber = new ClimberSubsystem(ClimberSelection.LEFT); - // private final ClimberSubsystem s_RightClimber = new ClimberSubsystem(ClimberSelection.RIGHT); + private final ClimberSubsystem s_LeftClimber = new ClimberSubsystem(ClimberSelection.LEFT); + private final ClimberSubsystem s_RightClimber = new ClimberSubsystem(ClimberSelection.RIGHT); @SuppressWarnings ("unused") private final LEDSubsystem s_Led = new LEDSubsystem(); private final VisionSubsystem s_Vision = new VisionSubsystem(); @@ -210,22 +210,20 @@ public RobotContainer() { SmartDashboard.putData("Reset heading", Commands.print("Resetting heading").andThen(Commands.runOnce(s_Swerve::resetHeading, s_Swerve)).andThen(Commands.print("Heading reset"))); // Allow for direct climber control - // SmartDashboard.putData("Stop climbers", Commands.runOnce(() -> { s_LeftClimber.stop(); s_RightClimber.stop(); }, s_LeftClimber, s_RightClimber)); - // SmartDashboard.putData("Left down slow", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_LeftClimber)); - // SmartDashboard.putData("Right down slow", Commands.runOnce(() -> { s_RightClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_RightClimber)); + SmartDashboard.putData("Stop climbers", Commands.runOnce(() -> { s_LeftClimber.stop(); s_RightClimber.stop(); }, s_LeftClimber, s_RightClimber)); + SmartDashboard.putData("Left down slow", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_LeftClimber)); + SmartDashboard.putData("Right down slow", Commands.runOnce(() -> { s_RightClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_RightClimber)); - // SmartDashboard.putNumber("Left climber voltage", 0.0); - // SmartDashboard.putNumber("Right climber voltage", 0.0); - // SmartDashboard.putData("Set climber voltage", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(SmartDashboard.getNumber("Left climber voltage", 0.0)); s_RightClimber.applyVoltage(SmartDashboard.getNumber("Right climber voltage", 0.0));}, s_LeftClimber, s_RightClimber)); - // SmartDashboard.putData("Zero climbers", Commands.runOnce(() -> { s_LeftClimber.zero(); s_RightClimber.zero(); }, s_LeftClimber, s_RightClimber)); - // SmartDashboard.putBoolean("climber/Climbers enabled", true); + SmartDashboard.putNumber("Left climber voltage", 0.0); + SmartDashboard.putNumber("Right climber voltage", 0.0); + SmartDashboard.putData("Set climber voltage", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(SmartDashboard.getNumber("Left climber voltage", 0.0)); s_RightClimber.applyVoltage(SmartDashboard.getNumber("Right climber voltage", 0.0));}, s_LeftClimber, s_RightClimber)); + SmartDashboard.putData("Zero climbers", Commands.runOnce(() -> { s_LeftClimber.zero(); s_RightClimber.zero(); }, s_LeftClimber, s_RightClimber)); + SmartDashboard.putBoolean("climber/Climbers enabled", true); -/* SmartDashboard.putNumber("Left climber target position", 0.0); - SmartDashboard.putData("Set left climber position", Commands.runOnce(() -> { s_Climber.setPositionLeft(SmartDashboard.getNumber("Left climber target position", 0.0));}, s_Climber)); + SmartDashboard.putData("Set left climber position", new ClimberPositionCommand(SmartDashboard.getNumber("Left climber target position", 0.0), LEDSubsystem.TempState.RETRACTING, s_LeftClimber)); SmartDashboard.putNumber("Right climber target position", 0.0); - SmartDashboard.putData("Set right climber position", Commands.runOnce(() -> { s_Climber.setPositionRight(SmartDashboard.getNumber("Right climber target position", 0.0));}, s_Climber)); -*/ + SmartDashboard.putData("Set right climber position", new ClimberPositionCommand(SmartDashboard.getNumber("Right climber target position", 0.0), LEDSubsystem.TempState.RETRACTING, s_RightClimber)); // Testing... // SmartDashboard.putData("Score in Amp", new PathPlannerAuto("Score in Amp")); @@ -270,17 +268,20 @@ private void configureButtonBindings() { new ShootCommand(s_Shooter, s_Index, s_Swerve), () -> SmartDashboard.getBoolean("Direct set RPM", false)) .withName("Shoot")); + climberExtendButton.onTrue( + new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber) + .alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber))); // climberExtendButton.onTrue( - // Commands.either( - // new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber) - // .alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)), - // new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber) - // .alongWith(new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)), - // () -> s_LeftClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError || - // s_RightClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError)); - - // leftClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_LeftClimber)); - // rightClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_RightClimber)); + // Commands.either( + // new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber) + // .alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)), + // new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber) + // .alongWith(new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)), + // () -> s_LeftClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError || + // s_RightClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError)); + + leftClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_LeftClimber)); + rightClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_RightClimber)); /* Buttons to set the next shot */ ampButton.onTrue(Commands.runOnce(() -> { s_Shooter.setNextShot(Speed.AMP); }).withName("Set amp shot")); diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 2c8ad43..0504ad0 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -54,10 +54,10 @@ private void applyConfigs() { var motionMagicConfigs = m_ClimberMotorsConfiguration.MotionMagic; motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Climber.cruiseVelocity; motionMagicConfigs.MotionMagicAcceleration = Constants.Climber.acceleration; - //motionMagicConfigs.MotionMagicJerk = Constants.Climber.jerk; + // motionMagicConfigs.MotionMagicJerk = Constants.Climber.jerk; /* Apply Shooters Motor Configs */ - // motor.getConfigurator().apply(m_ClimberMotorsConfiguration); + motor.getConfigurator().apply(m_ClimberMotorsConfiguration); } public void applyVoltage(double voltage) { @@ -73,11 +73,11 @@ public double getPosition() { } public void stop() { - // motor.setControl(voltageOut.withOutput(0.0)); + motor.setControl(voltageOut.withOutput(0.0)); } public void zero() { - // motor.setPosition(0.0); + motor.setPosition(0.0); } @Override