Skip to content

Pre-testing #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
k4limul merged 21 commits into from
Feb 24, 2026
Merged

Pre-testing #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
436eb40
Feat: Add voltageOverride
lucas01b0x Feb 21, 2026
f429160
FEAT: add signed kS and rezeroing in periodic for hood
k4limul Feb 21, 2026
27a0bf7
FEAT: Interpolation state implementation
k4limul Feb 21, 2026
244c2c1
Feat: Add voltageOverride
lucas01b0x Feb 21, 2026
4d1e336
Merge branch 'climber-hopper' of https://github.com/StuyPulse/2026-Ro…
lucas01b0x Feb 21, 2026
79097bb
Feat: ClimberHopperReset command
lucas01b0x Feb 21, 2026
655d859
Feat: Finish ClimberHopper encoder reset logic
lucas01b0x Feb 21, 2026
fe73f62
Fix
lucas01b0x Feb 21, 2026
baaaa24
Feat: Comment out ClimberHopper vertical expansion stuff
lucas01b0x Feb 21, 2026
3e73f45
Fix: Convert between meters and rotations with handling ClimberHopper…
lucas01b0x Feb 22, 2026
282cc8a
Merge pull request #8 from StuyPulse/climber-hopper
k4limul Feb 22, 2026
717ac87
CLEAN + FEAT: reorganize Settings and add CANCoderConfig wrapper
k4limul Feb 22, 2026
346028b
FEAT: port over brute force approach to turret zeroing
k4limul Feb 22, 2026
d52acab
FIX: hood configured and zeroing logic setup
k4limul Feb 22, 2026
93c4062
CLEAN: spotlessApply
k4limul Feb 22, 2026
a86ed3e
FEAT: copy over SOTM calculation and shooter/ferry interp impl from a…
k4limul Feb 22, 2026
294e9b6
feat: burger intake routine + changes
SP-COMPuter Feb 24, 2026
2564065
Merge branch 'pre-robot' into pre-testing
SP-COMPuter Feb 24, 2026
9492d8a
CLEAN: more cleaning up
k4limul Feb 24, 2026
faba05a
FIX: fix intake subsystem structure and IntakeSetState command
k4limul Feb 24, 2026
1b4d0e4
FIX: update ports excluding swerve
k4limul Feb 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
### Mechanisms

- Swerve
- L1 Climb + Vertically Expanding Hopper
- L1 Climb + ~~Vertically Expanding Hopper~~
- Handoff -> Turret + Hooded Shooter
- Spindexer
- Intake + Horizontally Expanding Hopper
- Intake + Passive Horizontally Expanding Hopper
- Vision with 3 LimeLight 4s
137 changes: 60 additions & 77 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@

import com.stuypulse.robot.commands.BuzzController;
import com.stuypulse.robot.commands.auton.DoNothingAuton;
import com.stuypulse.robot.commands.climberhopper.ClimberDown;
import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand;
import com.stuypulse.robot.commands.climberhopper.ClimberUp;
import com.stuypulse.robot.commands.handoff.HandoffReverse;
import com.stuypulse.robot.commands.handoff.HandoffRun;
import com.stuypulse.robot.commands.handoff.HandoffStop;
Expand All @@ -22,9 +24,9 @@
import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterRightCorner;
import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot;
import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow;
import com.stuypulse.robot.commands.intake.IntakeIntake;
import com.stuypulse.robot.commands.intake.IntakeOutake;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.intake.IntakeDeploy;
import com.stuypulse.robot.commands.intake.IntakeStopRollers;
import com.stuypulse.robot.commands.intake.IntakeStow;
import com.stuypulse.robot.commands.spindexer.SpindexerRun;
import com.stuypulse.robot.commands.spindexer.SpindexerStop;
import com.stuypulse.robot.commands.swerve.SwerveClimbAlign;
Expand Down Expand Up @@ -73,7 +75,6 @@ public interface EnabledSubsystems {

// Gamepads
public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER);
public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);

// Subsystem
private final ClimberHopper climberHopper = ClimberHopper.getInstance();
Expand Down Expand Up @@ -115,18 +116,22 @@ private void configureDefaultCommands() {
/***************/

private void configureButtonBindings() {
// Intake Up and Off
driver.getLeftTriggerButton()
.onTrue(new IntakeStow());

driver.getDPadDown()
.onTrue(new TurretIdle())
.onTrue(new TurretSeed());
// Intake Down and On
driver.getRightTriggerButton()
.onTrue(new IntakeDeploy());

// Reset Heading
driver.getDPadUp()
.onTrue(new SwerveResetHeading());

// SCORING ROUTINE
// Scoring Routine using Interpolation Settings
driver.getTopButton()
.whileTrue(new TurretShoot()
.alongWith(new HoodedShooterShoot())
.whileTrue(new HoodedShooterInterpolation()
.alongWith(new TurretShoot())
.alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance()))
.andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance())
.alongWith(new WaitUntilCommand(() -> handoff.atTolerance()))
Expand All @@ -135,39 +140,28 @@ private void configureButtonBindings() {
.alongWith(new HoodedShooterStow())
.alongWith(new HandoffStop()));

driver.getTopButton()
.whileTrue(new TurretShoot()
.alongWith(new HoodedShooterInterpolation())
// Ferry Routine using Interpolation Settings
driver.getBottomButton()
.onTrue(new HoodedShooterFerry()
.alongWith(new TurretFerry())
.alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance()))
.andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance())
.alongWith(new WaitUntilCommand(() -> handoff.atTolerance()))
.andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))))
.andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))
)
.onFalse(new SpindexerStop()
.alongWith(new HoodedShooterStow())
.alongWith(new HandoffStop()));

// driver.getDPadDown()
// .onTrue(new HoodedShooterShoot())
// .onFalse(new HoodedShooterStow());

// driver.getDPadUp()
// .onTrue(new HoodedShooterFerry())
// .onFalse(new HoodedShooterStow());

// driver.getDPadUp().whileTrue(new HoodedShooterShoot()
// .alongWith(new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())
// .andThen(new FeederFeed())))
// .onFalse(new HoodedShooterStow()
// .alongWith(new FeederStop()));

//-------------------------------------------------------------------------------------------------------------------------\\
//-------------------------------------------------------------------------------------------------------------------------\\
//-------------------------------------------------------------------------------------------------------------------------\\
//-------------------------------------------------------------------------------------------------------------------------\\
//-------------------------------------------------------------------------------------------------------------------------\\
/*
// Climb Align
driver.getTopButton()
.whileTrue(new SwerveClimbAlign());
.whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()));

// Left Corner Shoot
driver.getLeftButton()
Expand Down Expand Up @@ -217,21 +211,22 @@ private void configureButtonBindings() {
new HandoffStop()))
);

// Intake On
// Intake Up and Off
driver.getLeftTriggerButton()
.onTrue(new IntakeIntake());
.onTrue(new IntakeStow());

// Intake Off
// Intake Down and On
driver.getRightTriggerButton()
.onTrue(new IntakeStop());
.onTrue(new IntakeDeploy());

// Climb Down Placeholder
driver.getLeftBumper()
.onTrue(new BuzzController(driver));
.onTrue(new BuzzController(driver).alongWith(new ClimberDown()));

// Climb Up Placeholder
driver.getRightBumper()
.onTrue(new BuzzController(driver));
.onTrue(new BuzzController(driver))
.whileTrue(new ClimberUp());

// Reset Heading
driver.getDPadUp()
Expand Down Expand Up @@ -268,19 +263,7 @@ private void configureButtonBindings() {
new SpindexerRun().alongWith(
new HandoffStop()))
);

// Unjam
driver.getDPadDown()
.whileTrue(
new HoodedShooterReverse().alongWith(
new HandoffReverse().alongWith(
new IntakeOutake())))
.onFalse(
new HoodedShooterStow().alongWith(
new SpindexerRun().alongWith(
new HandoffStop().alongWith(
new IntakeStop())))
);
*/
}

/**************/
Expand All @@ -305,35 +288,35 @@ public void configureSysids() {
// autonChooser.addOption("SysID Module Rotation Quasi Forwards", swerve.sysIdRotQuasi(Direction.kForward));
// autonChooser.addOption("SysID Module Rotation Quasi Backwards", swerve.sysIdRotQuasi(Direction.kReverse));

SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine();
autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward));
autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse));
autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward));
autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse));

SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine();
autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward));
autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse));
autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward));
autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse));

SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine();
autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward));
autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse));
autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward));
autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse));

SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine();
autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward));
autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse));
autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward));
autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse));

SysIdRoutine handoffSysId = handoff.getSysIdRoutine();
autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward));
autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse));
autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward));
autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse));
// SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine();
// autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward));
// autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse));
// autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward));
// autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse));

// SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine();
// autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward));
// autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse));
// autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward));
// autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse));

// SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine();
// autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward));
// autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse));
// autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward));
// autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse));

// SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine();
// autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward));
// autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse));
// autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward));
// autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse));

// SysIdRoutine handoffSysId = handoff.getSysIdRoutine();
// autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward));
// autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse));
// autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward));
// autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse));

}

Expand Down
82 changes: 42 additions & 40 deletions src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,26 +5,22 @@
/***************************************************************/
package com.stuypulse.robot.commands.climberhopper;

import com.stuypulse.robot.constants.Field;
import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper;
import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState;
import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;


public class ClimberHopperDefaultCommand extends Command {
private final ClimberHopper climberHopper;
private final CommandSwerveDrivetrain swerve;
private Pose2d pose;
private boolean flag = false; // To prevent repeated stalling under trench
// private final CommandSwerveDrivetrain swerve;
// private Pose2d pose;
// private boolean flag = false; // To prevent repeated stalling under trench

public ClimberHopperDefaultCommand() {
climberHopper = ClimberHopper.getInstance();
swerve = CommandSwerveDrivetrain.getInstance();
pose = swerve.getPose();
// swerve = CommandSwerveDrivetrain.getInstance();
// pose = swerve.getPose();


addRequirements(climberHopper);
Expand All @@ -34,43 +30,49 @@ public ClimberHopperDefaultCommand() {
public void execute() {
// === Robot Position Logic ===
// Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left.
pose = swerve.getPose();
// pose = swerve.getPose();

boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY();
// boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY();

boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY();
// boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY();

boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance;
// boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance;

boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance;
// boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance;

boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance
// boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance

// === Climber Position and State Logic ===
boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN;

boolean stalledByBalls = climberHopper.getStalling() && inDownState;
// boolean stalledByBalls = true;

if (isUnderTrench) {
if (!stalledByBalls && !flag) {
climberHopper.setState(ClimberHopperState.HOPPER_DOWN);
} else {
climberHopper.setState(ClimberHopperState.HOPPER_UP);
// TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!!
flag = true; // prevent hopper from going back down while still under trench with too many balls
}
} else { // If not under trench...
if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) {
// Set the hopper up
climberHopper.setState(ClimberHopperState.HOPPER_UP);
}
// // === Climber Position and State Logic ===
// boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN;

// boolean stalledByBalls = climberHopper.getStalling() && inDownState;
// // boolean stalledByBalls = true;

// if (isUnderTrench) {
// if (!stalledByBalls && !flag) {
// climberHopper.setState(ClimberHopperState.HOPPER_DOWN);
// } else {
// climberHopper.setState(ClimberHopperState.HOPPER_UP);
// // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!!
// flag = true; // prevent hopper from going back down while still under trench with too many balls
// }
// } else { // If not under trench...
// if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) {
// // Set the hopper up
// climberHopper.setState(ClimberHopperState.HOPPER_UP);
// }
// }

// if (!isUnderTrench) {
// flag = false;
// }

// SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench);

// !!! AFTER ABANDONING VERTICAL EXPANSION:
if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) {
// Set the hopper down
climberHopper.setState(ClimberHopperState.HOPPER_DOWN);
}

if (!isUnderTrench) {
flag = false;
}

SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench);
}
}
39 changes: 31 additions & 8 deletions src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,19 +5,42 @@
/***************************************************************/
package com.stuypulse.robot.commands.climberhopper;

import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper;
// import java.util.Optional;

// import com.stuypulse.robot.Robot;
// import com.stuypulse.robot.constants.Settings;
// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper;

import edu.wpi.first.wpilibj2.command.Command;

public class ClimberHopperReset extends Command {
private final ClimberHopper climberHopper;
// private final ClimberHopper climberHopper;

public ClimberHopperReset() {
climberHopper = ClimberHopper.getInstance();
}
// public ClimberHopperReset() {
// climberHopper = ClimberHopper.getInstance();
// }

// @Override
// public void initialize() {
// climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE));
// }

// @Override
// public boolean isFinished() {
// if (climberHopper.getStalling()) {
// climberHopper.resetPostionUpper();
// return true;
// }
// return false;
// }

// @Override
// public void end(boolean interrupted) {
// climberHopper.setVoltageOverride(Optional.empty());
// }

@Override
public void execute() {
// @Override
// public void execute() {

}
// }
}
Loading