Skip to content

Commit

Permalink
Enable and recalibrate climber subsytem
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjbell4
rjbell4 committed Apr 14, 2024
1 parent 6e75e83 commit 5dc17ca
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 39 deletions.
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
57 changes: 29 additions & 28 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

/**
Expand All @@ -51,24 +51,24 @@ 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 */
private final Swerve s_Swerve = new Swerve();
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();
Expand Down Expand Up @@ -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"));
Expand Down Expand Up @@ -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"));
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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
Expand Down

0 comments on commit 5dc17ca

Please sign in to comment.