Skip to content

Feature/operator pose override + other controller stuff #61

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
spellingcat merged 14 commits into from
Mar 13, 2026
Merged

Feature/operator pose override + other controller stuff #61

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
5ff7809
i so eepy
spellingcat Mar 11, 2026
ce35132
have to merge in intake now
spellingcat Mar 11, 2026
952f3ff
Merge branch 'bringup/intake-pivot' into feature/operator-pose-override
spellingcat Mar 11, 2026
50f84bd
i forgot what i'm doing
spellingcat Mar 11, 2026
6334331
force intake retract for defense
spellingcat Mar 11, 2026
791cd48
track feed target when resting
spellingcat Mar 11, 2026
7adf845
where were you when sysid + other unused stuff was kil. no
spellingcat Mar 11, 2026
8ad4acd
remove zeroing against left hardstop
spellingcat Mar 13, 2026
48c5c3a
lord i hope this goes well Merge branch 'main' into feature/operator-…
spellingcat Mar 13, 2026
5b1cec7
bad news Revert "remove zeroing against left hardstop"
spellingcat Mar 13, 2026
63cdb52
Revert "bad news Revert "remove zeroing against left hardstop""
spellingcat Mar 13, 2026
b5cd5fc
hopefully fix trench hood lock and fxed shots running into each other
spellingcat Mar 13, 2026
745968e
Merge branch 'main' into feature/operator-pose-override
spellingcat Mar 13, 2026
0903422
fmt
spellingcat Mar 13, 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
173 changes: 29 additions & 144 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,10 +213,6 @@ public enum RobotEdition {
@AutoLogOutput(key = "Superstructure/Autoaim Request")
private Trigger autoAimReq;

// TODO
// @AutoLogOutput(key = "Superstructure/Autoaim Request")
// private Trigger climbAutoAlignInAutoReq;

// Auto stuff
private final Autos autos;
private Optional<Alliance> lastAlliance = Optional.empty();
Expand Down Expand Up @@ -329,10 +325,10 @@ public Robot() {
new SpindexerSubsystem(
canivore,
(ROBOT_MODE == RobotMode.REAL)
? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore)
? new RollerIO(9, SpindexerSubsystem.getIndexerConfig(), canivore)
: new RollerIOSim(
9,
SpindexerSubsystem.getIndexerConfigs(),
SpindexerSubsystem.getIndexerConfig(),
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX44Foc(1),
Expand All @@ -342,10 +338,10 @@ public Robot() {
MotorType.KrakenX44,
canivore),
(ROBOT_MODE == RobotMode.REAL)
? new RollerIO(10, SpindexerSubsystem.getKickerConfigs(), canivore)
? new RollerIO(10, SpindexerSubsystem.getKickerConfig(), canivore)
: new RollerIOSim(
10,
SpindexerSubsystem.getKickerConfigs(),
SpindexerSubsystem.getKickerConfig(),
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX44Foc(1),
Expand Down Expand Up @@ -410,7 +406,9 @@ public Robot() {
11,
TurretSubsystem.HOOD_MIN_ANGLE,
TurretSubsystem.HOOD_MAX_ANGLE),
ROBOT_MODE == RobotMode.REAL ? new TurretIO(canivore) : new TurretIOSim(canivore),
ROBOT_MODE == RobotMode.REAL
? new TurretIO(canivore, TurretSubsystem.getTurretConfig())
: new TurretIOSim(canivore),
ROBOT_MODE == RobotMode.REAL
? new CANcoderIO(5, TurretSubsystem.getCancoder24tConfigs(), canivore)
: new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore),
Expand All @@ -422,26 +420,11 @@ public Robot() {
climber =
ROBOT_EDITION == RobotEdition.ALPHA
? new EmptyClimberSubsystem(canivore)
: new ClimberSubsystem(
new ClimberIO(canivore)); // TODO: SWITCH BACK TO REAL CLIMBER WHEN FIXED
// : new ClimberSubsystem(new ClimberIO(canivore));
: new ClimberSubsystem(new ClimberIO(canivore));
// now that we've assigned the correct subsystems based on robot edition, we can pass them into
// the superstructure
superstructure =
new Superstructure(swerve, indexer, intake, shooter, climber, driver, operator);
addCompSysids(climber, indexer, intake, shooter);

autoAimReq =
driver
.leftBumper()
.or(
new Trigger(
() ->
Superstructure.getState() == SuperState.SPIN_UP_SCORE
|| Superstructure.getState() == SuperState.SCORE)
.and(() -> isTeleopEnabled()));

// climbAutoAlignInAutoReq = Autos.autoAlignClimbReq;

DriverStation.silenceJoystickConnectionWarning(true);
SignalLogger.enableAutoLogging(false);
Expand Down Expand Up @@ -509,12 +492,7 @@ public Robot() {
SmartDashboard.putData(
"Zero Intake Off Cancoder", intake.zeroPivotOffCancoder().ignoringDisable(true));
SmartDashboard.putData("Zero Hood", shooter.zeroHood().ignoringDisable(true));
SmartDashboard.putData(
"Test shot",
Commands.parallel(
shooter.torqueCurrentTest(swerve::getPose, swerve::getVelocityFieldRelative),
Commands.waitUntil(new Trigger(shooter::atFlywheelVelocitySetpoint).debounce(0.1))
.andThen(indexer.testShoot())));

SmartDashboard.putData(
"Set Turret to 0", shooter.resetTurretToPosition(Rotation2d.kZero).ignoringDisable(true));
SmartDashboard.putData(
Expand All @@ -529,7 +507,8 @@ public Robot() {
// Set default commands
driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0));
operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
shooter.setDefaultCommand(shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative));
shooter.setDefaultCommand(
shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::canScore));
swerve.setDefaultCommand(
swerve
.driveOpenLoopFieldRelative(
Expand All @@ -543,18 +522,9 @@ public Robot() {
* SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed())
.times(-1))
.withName("default"));
// swerve.setDefaultCommand(swerve.stop());
indexer.setDefaultCommand(indexer.rest());
intake.setDefaultCommand(intake.restExtended());
climber.setDefaultCommand(climber.retract());
// swerve.faceHubSOTM(
// () ->
// modifyJoystick(driver.getLeftX())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
// () ->
// modifyJoystick(driver.getLeftY())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()
// * -1));

addControllerBindings(indexer, shooter, intake);

Expand Down Expand Up @@ -593,8 +563,6 @@ public Robot() {
.ignoringDisable(true));

SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true));
// SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true));
// SmartDashboard.putData("Test Shot", shooter.testShoot());

// Reset alert timers
canInitialErrorTimer.restart();
Expand Down Expand Up @@ -628,22 +596,13 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
() ->
swerve.setYaw(
DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue)
// ? Rotation2d.kCW_90deg
// : Rotation2d.kCCW_90deg)));
? Rotation2d.kZero
: Rotation2d.k180deg)));

// autoaim (alpha)
autoAimReq
.and(() -> ROBOT_EDITION == RobotEdition.ALPHA)
.whileTrue(
// swerve.faceHubSOTM(
// () ->
// modifyJoystick(driver.getLeftY())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
// () ->
// modifyJoystick(driver.getLeftX())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()));
swerve.faceHub(
() ->
-1
Expand All @@ -657,63 +616,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE));

// climbAutoAlignInAutoReq.whileTrue(
// swerve.alignToClimb(
// () ->
// ClimbTargets.CLIMB_TARGETS_LIST.stream()
// .filter(target -> target.getLeftHanded() == leftClimbTarget)
// .filter(
// target ->
// target.isBlueAlliance()
// == (DriverStation.getAlliance().orElse(Alliance.Blue)
// == Alliance.Blue))
// .findFirst()
// .get()));

// climbAutoAlignInAutoReq.whileTrue(
// swerve.alignToClimb(
// () ->
// ClimbTargets.CLIMB_TARGETS_LIST.stream()
// .filter(target -> target.getLeftHanded() == leftClimbTarget)
// .filter(
// target ->
// target.isBlueAlliance()
// == (DriverStation.getAlliance().orElse(Alliance.Blue)
// == Alliance.Blue))
// .findFirst()
// .get()));

// new Trigger(swerve::isCloseToBump)
// .whileTrue(
// swerve.bumpAlign(
// () ->
// DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue)
// ? -1
// : 1
// * modifyJoystick(driver.getLeftY())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
// () ->
// DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue)
// ? -1
// : 1
// * modifyJoystick(driver.getLeftX())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()));

new Trigger(swerve::isNearTrench)
.and(() -> Superstructure.getState() != SuperState.INTAKE)
.and(() -> isTeleopEnabled())
.and(() -> !Superstructure.getPoseOverride())
.whileTrue(
swerve
// .driveClosedLoopFieldRelative(
// () ->
// new ChassisSpeeds(
// modifyJoystick(driver.getLeftY())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
// modifyJoystick(driver.getLeftX())
// * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
// AutoAlign.calculateRotationVelocity(
// swerve.getRotation(), Rotation2d.fromDegrees(30)))
// .times(-1))
.trenchAlign(
() ->
modifyJoystick(driver.getLeftY())
Expand All @@ -722,7 +630,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
modifyJoystick(driver.getLeftX())
* SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())
.alongWith(Commands.print("afkljsdflkjs")));
// 0));

// current zero shooter hood
driver
Expand All @@ -736,6 +643,15 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta

new Trigger(() -> AutoAim.targetInTurretDeadzone())
.onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25));
// ---zeroing stuff---
driver.povUp().whileTrue(shooter.currentZeroTurretAgainstForwardHardstop());

driver
.leftBumper()
.onTrue(
Commands.parallel(
shooter.resetTurretToPosition(shooter.getCalculatedTurretRotations()),
intake.zeroPivotOffCancoder()));

operator
.leftBumper()
Expand All @@ -745,23 +661,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
.rightBumper()
.or(Autos.autoLeftClimbReq.negate())
.onTrue(Commands.runOnce(() -> leftClimbTarget = false));
// I HATE THIS!
operator.leftStick().whileTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));

// TODO: ACTUAL BINDING LOL
// test shot
driver
.rightBumper()
.whileTrue(
// Commands.parallel(
// // shooter.torqueCurrentTest(),
// shooter.testShoot(swerve::getPose, swerve::getVelocityFieldRelative),
// Commands.waitUntil(
// new Trigger(shooter::atFlywheelVelocitySetpoint).debounce(0.05)
// // .and(shooter::atTurretSetpoint)
// // .debounce(0.25)
// )
// .andThen(indexer.kick())));
// SmartDashboard.putData(
// "climb align",
swerve.alignToClimb(
() ->
ClimbTargets.CLIMB_TARGETS_LIST.stream()
Expand All @@ -774,10 +679,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
.findFirst()
.get()));
// turn swerve if target is in turret deadzone
driver
.leftBumper()
.and(AutoAim::targetInTurretDeadzone)
// driver
// .leftBumper()
// .and(
new Trigger(AutoAim::targetInTurretDeadzone)
.and(() -> Superstructure.getState().isAScoreState())
.and(() -> !Superstructure.getPoseOverride())
.whileTrue(
swerve.faceHubComp(
() ->
Expand All @@ -789,7 +696,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
* modifyJoystick(driver.getLeftX())
* SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
shooter::getTurretPosition));
// ---zeroing stuff---

// create triggers for joystick disconnect alerts
new Trigger(() -> DriverStation.isJoystickConnected(0))
Expand Down Expand Up @@ -821,36 +727,16 @@ private void addAutos() {
System.out.println("Done generating autos");
}

// Sysid Autos
private void addCompSysids(
ClimberSubsystem climber, Indexer indexer, Intake intake, Shooter shooter) {
autoChooser.addOption("Climber Sysid", climber.runClimberSysid());
autoChooser.addOption("Indexer Roller Sysid", indexer.runRollerSysId());
autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid());
autoChooser.addOption("Intake Pivot Sysid", intake.runPivotSysid());
autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());

autoChooser.addOption("Hood Sysid", shooter.runHoodSysid());
autoChooser.addOption("Turret Sysid", shooter.runTurretSysid());
autoChooser.addOption("Kicker Sysid", indexer.runKickerSysId());
autoChooser.addOption("Turn Sysid", swerve.runTurnSysid());
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();

superstructure.periodic();

// TODO: YAW VALUE FROM HARDWARE
Pose3d turretPose =
new Pose3d(
new Translation3d(-0.177413, -0.111702, 0.350341),
// new Rotation3d(0, 0, Units.degreesToRadians(turretAngle.getAsDouble())));
new Rotation3d(0, 0, shooter.getTurretPosition().getRadians()));
// 0));
// ));
// TODO: USE MEASURED EXTENSIONS AND ANGLES
Logger.recordOutput(
"Robot/Mechanism Poses",
new Pose3d[] {
Expand All @@ -877,12 +763,11 @@ public void robotPeriodic() {
new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero)
});

// TODO: ACTUAL SETPOINT
Pose3d turretSetpoint =
new Pose3d(
new Translation3d(-0.177413, -0.111702, 0.350341),
new Rotation3d(0, 0, shooter.getTurretSetpoint().getRadians()));
// TODO: ACTUAL SETPOINTS

Logger.recordOutput(
"Robot/Mechanism Setpoints",
new Pose3d[] {
Expand Down
Loading
Loading