Skip to content

Commit 5dc17ca

Browse files
rjbell4rjbell4
committed
Enable and recalibrate climber subsytem
1 parent 6e75e83 commit 5dc17ca

3 files changed

Lines changed: 40 additions & 39 deletions

File tree

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -251,21 +251,21 @@ public class Climber {
251251
public static final InvertedValue motorOutputInverted = InvertedValue.Clockwise_Positive;
252252
public static final NeutralModeValue motorNeutralValue = NeutralModeValue.Brake;
253253

254-
public static final double extendedPosition = -875.0;
255-
public static final double midPosition = -450.0;
254+
public static final double extendedPosition = -230.0;
255+
// public static final double midPosition = -100.0; //-450.0;
256256
public static final double retractedPosition = -15.0;
257-
public static final double positionError = 1.5;
258-
public static final double slowVoltage = 2.75;
257+
public static final double positionError = 1.0;
258+
public static final double slowVoltage = 1.5;
259259

260-
public static final double RPSperVolt = 8.5; // RPS increase with every volt
260+
public static final double RPSperVolt = 7.9; // RPS increase with every volt
261261
public static final double kP = 0.50; // output per unit of error in position (output/rotation)
262262
public static final double kI = 0.0; // output per unit of integrated error in position (output/(rotation*s))
263263
public static final double kD = 0.0; // output per unit of error in velocity (output/rps)
264-
public static final double kS = 0.25; // output to overcome static friction (output)
264+
public static final double kS = 0.22; // output to overcome static friction (output)
265265
public static final double kV = 1.0 / RPSperVolt; // output per unit of target velocity (output/rps)
266266
public static final double kA = 0.0; // output per unit of target acceleration (output/(rps/s))
267267
public static final double kG = 0.0; // do not factory in gravity
268-
public static final double cruiseVelocity = 125.0; // RPS
268+
public static final double cruiseVelocity = 120.0; // RPS
269269
public static final double acceleration = cruiseVelocity * 0.5; // Accelerate in 0.5 seconds
270270

271271
public static final double timeCutOff = 25.0;

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 29 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
import edu.wpi.first.wpilibj2.command.button.Trigger;
2626
import frc.robot.commands.*;
2727
import frc.robot.subsystems.*;
28-
// import frc.robot.subsystems.ClimberSubsystem.ClimberSelection;
28+
import frc.robot.subsystems.ClimberSubsystem.ClimberSelection;
2929
import frc.robot.subsystems.ShooterSubsystem.Speed;
3030

3131
/**
@@ -51,24 +51,24 @@ public class RobotContainer {
5151
private final Trigger intakeButton = driver.leftBumper();
5252
private final Trigger shooterButton = driver.rightBumper();
5353
private final Trigger ejectButton = driver.start();
54-
// private final Trigger leftClimberButton = driver.leftTrigger();
55-
// private final Trigger rightClimberButton = driver.rightTrigger();
54+
private final Trigger leftClimberButton = driver.leftTrigger();
55+
private final Trigger rightClimberButton = driver.rightTrigger();
5656

5757
/* Different Position Test Buttons */
5858
private final Trigger ampButton = driver.a();
5959
private final Trigger dumpShotButton = driver.b();
6060
private final Trigger defaultShotButton = driver.back();
6161
private final Trigger slideShotButton = driver.x();
62-
// private final Trigger climberExtendButton = driver.y();
62+
private final Trigger climberExtendButton = driver.y();
6363
private final Trigger ampShotButton = driver.povDown();
6464

6565
/* Subsystems */
6666
private final Swerve s_Swerve = new Swerve();
6767
private final IntakeSubsystem s_Intake = new IntakeSubsystem();
6868
private final ShooterSubsystem s_Shooter = new ShooterSubsystem();
6969
private final IndexSubsystem s_Index = new IndexSubsystem();
70-
// private final ClimberSubsystem s_LeftClimber = new ClimberSubsystem(ClimberSelection.LEFT);
71-
// private final ClimberSubsystem s_RightClimber = new ClimberSubsystem(ClimberSelection.RIGHT);
70+
private final ClimberSubsystem s_LeftClimber = new ClimberSubsystem(ClimberSelection.LEFT);
71+
private final ClimberSubsystem s_RightClimber = new ClimberSubsystem(ClimberSelection.RIGHT);
7272
@SuppressWarnings ("unused")
7373
private final LEDSubsystem s_Led = new LEDSubsystem();
7474
private final VisionSubsystem s_Vision = new VisionSubsystem();
@@ -210,22 +210,20 @@ public RobotContainer() {
210210
SmartDashboard.putData("Reset heading", Commands.print("Resetting heading").andThen(Commands.runOnce(s_Swerve::resetHeading, s_Swerve)).andThen(Commands.print("Heading reset")));
211211

212212
// Allow for direct climber control
213-
// SmartDashboard.putData("Stop climbers", Commands.runOnce(() -> { s_LeftClimber.stop(); s_RightClimber.stop(); }, s_LeftClimber, s_RightClimber));
214-
// SmartDashboard.putData("Left down slow", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_LeftClimber));
215-
// SmartDashboard.putData("Right down slow", Commands.runOnce(() -> { s_RightClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_RightClimber));
213+
SmartDashboard.putData("Stop climbers", Commands.runOnce(() -> { s_LeftClimber.stop(); s_RightClimber.stop(); }, s_LeftClimber, s_RightClimber));
214+
SmartDashboard.putData("Left down slow", Commands.runOnce(() -> { s_LeftClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_LeftClimber));
215+
SmartDashboard.putData("Right down slow", Commands.runOnce(() -> { s_RightClimber.applyVoltage(Constants.Climber.slowVoltage); }, s_RightClimber));
216216

217-
// SmartDashboard.putNumber("Left climber voltage", 0.0);
218-
// SmartDashboard.putNumber("Right climber voltage", 0.0);
219-
// 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));
220-
// SmartDashboard.putData("Zero climbers", Commands.runOnce(() -> { s_LeftClimber.zero(); s_RightClimber.zero(); }, s_LeftClimber, s_RightClimber));
221-
// SmartDashboard.putBoolean("climber/Climbers enabled", true);
217+
SmartDashboard.putNumber("Left climber voltage", 0.0);
218+
SmartDashboard.putNumber("Right climber voltage", 0.0);
219+
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));
220+
SmartDashboard.putData("Zero climbers", Commands.runOnce(() -> { s_LeftClimber.zero(); s_RightClimber.zero(); }, s_LeftClimber, s_RightClimber));
221+
SmartDashboard.putBoolean("climber/Climbers enabled", true);
222222

223-
/*
224223
SmartDashboard.putNumber("Left climber target position", 0.0);
225-
SmartDashboard.putData("Set left climber position", Commands.runOnce(() -> { s_Climber.setPositionLeft(SmartDashboard.getNumber("Left climber target position", 0.0));}, s_Climber));
224+
SmartDashboard.putData("Set left climber position", new ClimberPositionCommand(SmartDashboard.getNumber("Left climber target position", 0.0), LEDSubsystem.TempState.RETRACTING, s_LeftClimber));
226225
SmartDashboard.putNumber("Right climber target position", 0.0);
227-
SmartDashboard.putData("Set right climber position", Commands.runOnce(() -> { s_Climber.setPositionRight(SmartDashboard.getNumber("Right climber target position", 0.0));}, s_Climber));
228-
*/
226+
SmartDashboard.putData("Set right climber position", new ClimberPositionCommand(SmartDashboard.getNumber("Right climber target position", 0.0), LEDSubsystem.TempState.RETRACTING, s_RightClimber));
229227

230228
// Testing...
231229
// SmartDashboard.putData("Score in Amp", new PathPlannerAuto("Score in Amp"));
@@ -270,17 +268,20 @@ private void configureButtonBindings() {
270268
new ShootCommand(s_Shooter, s_Index, s_Swerve),
271269
() -> SmartDashboard.getBoolean("Direct set RPM", false))
272270
.withName("Shoot"));
271+
climberExtendButton.onTrue(
272+
new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber)
273+
.alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)));
273274
// climberExtendButton.onTrue(
274-
// Commands.either(
275-
// new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber)
276-
// .alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)),
277-
// new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber)
278-
// .alongWith(new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)),
279-
// () -> s_LeftClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError ||
280-
// s_RightClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError));
281-
282-
// leftClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_LeftClimber));
283-
// rightClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_RightClimber));
275+
// Commands.either(
276+
// new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber)
277+
// .alongWith(new ClimberPositionCommand(Constants.Climber.extendedPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)),
278+
// new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_LeftClimber)
279+
// .alongWith(new ClimberPositionCommand(Constants.Climber.midPosition, LEDSubsystem.TempState.EXTENDING, s_RightClimber)),
280+
// () -> s_LeftClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError ||
281+
// s_RightClimber.getPosition() < Constants.Climber.midPosition + 2 * Constants.Climber.positionError));
282+
283+
leftClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_LeftClimber));
284+
rightClimberButton.whileTrue(new ClimberPositionCommand(Constants.Climber.retractedPosition, LEDSubsystem.TempState.RETRACTING, s_RightClimber));
284285

285286
/* Buttons to set the next shot */
286287
ampButton.onTrue(Commands.runOnce(() -> { s_Shooter.setNextShot(Speed.AMP); }).withName("Set amp shot"));

‎src/main/java/frc/robot/subsystems/ClimberSubsystem.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -54,10 +54,10 @@ private void applyConfigs() {
5454
var motionMagicConfigs = m_ClimberMotorsConfiguration.MotionMagic;
5555
motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Climber.cruiseVelocity;
5656
motionMagicConfigs.MotionMagicAcceleration = Constants.Climber.acceleration;
57-
//motionMagicConfigs.MotionMagicJerk = Constants.Climber.jerk;
57+
// motionMagicConfigs.MotionMagicJerk = Constants.Climber.jerk;
5858

5959
/* Apply Shooters Motor Configs */
60-
// motor.getConfigurator().apply(m_ClimberMotorsConfiguration);
60+
motor.getConfigurator().apply(m_ClimberMotorsConfiguration);
6161
}
6262

6363
public void applyVoltage(double voltage) {
@@ -73,11 +73,11 @@ public double getPosition() {
7373
}
7474

7575
public void stop() {
76-
// motor.setControl(voltageOut.withOutput(0.0));
76+
motor.setControl(voltageOut.withOutput(0.0));
7777
}
7878

7979
public void zero() {
80-
// motor.setPosition(0.0);
80+
motor.setPosition(0.0);
8181
}
8282

8383
@Override

0 commit comments

Comments
 (0)