Skip to content

The grind before competition #25

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
ultimatehecker merged 10 commits into from
Feb 6, 2026
Merged

The grind before competition #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
d669cdc
made the turret a default command
ultimatehecker Feb 5, 2026
05e6397
fixed small bugs and add ability for togglable robotCentric
ultimatehecker Feb 5, 2026
74af75a
ball tracking
ultimatehecker Feb 5, 2026
27b8019
changed to prevent turret from resuming same speed (in case manual ov…
ultimatehecker Feb 5, 2026
794d7e0
shooter completely tuned
ultimatehecker Feb 6, 2026
d0eb0a2
tuned turret
ultimatehecker Feb 6, 2026
e57821c
bulk reading and follow tracjectory for engame park
ultimatehecker Feb 6, 2026
971d965
added hecker bindings
ultimatehecker Feb 6, 2026
35b4c96
auto shooting command now works
ultimatehecker Feb 6, 2026
d519c9d
changes throughout the day
ultimatehecker Feb 6, 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
15 changes: 10 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,7 @@ public void initialize() {

schedule(
new RunCommand(() -> telemetryManager.update(telemetry)),
new RunCommand(led::update),
new RunCommand(() -> turret.setPosition(turret.computeAngle(drivetrain.getPose(), turret.getTargetPose(savedAllianceColor), 0, -3)))
new RunCommand(led::update)
);
}

Expand Down Expand Up @@ -155,19 +154,20 @@ public void initialize_loop() {
drawUnlockedUI(elapsed);
} else {
drawLockedUI();
setGlobalSettings();
}

telemetryManager.update(telemetry);
led.update();
}

public void initializeOpMode() {
private void initializeOpMode() {
if (teleopInitialized) return;
teleopInitialized = true;
}

@SuppressLint("DefaultLocale")
public void drawUnlockedUI(double elaspedTimeSinceStarted) {
private void drawUnlockedUI(double elaspedTimeSinceStarted) {
telemetryManager.addData("Saved Location", savedLocation);
telemetryManager.addData("Saved Autonomous Routine", savedAutonomousRoutine);
telemetryManager.addData("Saved Alliance Selection", savedAllianceColor);
Expand All @@ -181,13 +181,18 @@ public void drawUnlockedUI(double elaspedTimeSinceStarted) {
telemetryManager.addLine("Locks automatically at 4s");
}

public void drawLockedUI() {
private void drawLockedUI() {
telemetry.addLine("=== Configuration Locked ===");
telemetryManager.addData("Location", savedLocation);
telemetryManager.addData("Auto", savedAutonomousRoutine);
telemetryManager.addData("Alliance", savedAllianceColor);
}

private void setGlobalSettings() {
GlobalConstants.allianceColor = savedAllianceColor;
GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP;
}

private <T> T cycleRight(T current, T[] values) {
for (int i = 0; i < values.length; i++) {
if (values[i] == current) return values[(i + 1) % values.length];
Expand Down
133 changes: 98 additions & 35 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,18 @@
package org.firstinspires.ftc.teamcode;

import org.firstinspires.ftc.library.command.InstantCommand;
import org.firstinspires.ftc.library.command.button.Trigger;
import org.firstinspires.ftc.library.gamepad.GamepadEx;
import org.firstinspires.ftc.library.gamepad.GamepadKeys;
import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory;
import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory;
import org.firstinspires.ftc.teamcode.command_factories.SuperstructureFactory;
import org.firstinspires.ftc.teamcode.command_factories.TransferFactory;
import org.firstinspires.ftc.teamcode.commands.AimTowardShootingRegion;
import org.firstinspires.ftc.teamcode.commands.MaintainShooterNumericals;
import org.firstinspires.ftc.teamcode.commands.TeleopMecanum;
import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants;
import org.firstinspires.ftc.teamcode.constants.GlobalConstants;
import org.firstinspires.ftc.teamcode.constants.LEDConstants;
import org.firstinspires.ftc.teamcode.constants.ShooterConstants;
import org.firstinspires.ftc.teamcode.constants.TransferConstants;
Expand All @@ -24,39 +30,81 @@ private TeleopBindings() {}
public static void configureBindings(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, Lift lift, LED led) {
/* ------------------------------ Driver Controls ------------------------------ */

new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
.whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
.whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
if(GlobalConstants.getCurrentDriverType() == GlobalConstants.DriverType.HANISH) {
new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
.whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
.whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));

new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
.whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500))
.whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
.whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500))
.whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));

driver.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive(
() -> shooter.setHoodPosition(-2),
() -> shooter.setHoodPosition(2)
);

driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed(
ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.smartShootingCommand(intake, transfer, led))
);

driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
.whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));

driver.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001));

driver.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(ShooterFactory.openLoopSetpointCommand(shooter, () -> -0.3))
.whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
} else {
new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
.whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
.whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));

new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
.whenActive(SuperstructureFactory.smartShootingCommand(intake, transfer, led));

new Trigger(() -> transfer.firstCSDistance() < 2.5).whenActive(() -> led.setSolid(LEDConstants.ColorValue.YELLOW));
driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
.whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));

driver.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive(
() -> shooter.setHoodPosition(-2),
() -> shooter.setHoodPosition(2)
);
driver.getGamepadButton(GamepadKeys.Button.RIGHT_STICK_BUTTON).toggleWhenPressed(
TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition),
TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition)
);

driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
.whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
driver.getGamepadButton(GamepadKeys.Button.BACK).whenPressed(
SuperstructureFactory.auomaticallyParkAndLiftCommand(drivetrain, lift)
);

driver.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001));
driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed(
new InstantCommand(drivetrain::toggleRobotFieldCentric)
);

driver.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(ShooterFactory.openLoopSetpointCommand(shooter, () -> -0.3))
.whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
driver.getGamepadButton(GamepadKeys.Button.CROSS).whenPressed(
new InstantCommand(drivetrain::manualResetPoseForAlliance)
);

driver.getGamepadButton(GamepadKeys.Button.LEFT_STICK_BUTTON).whenPressed(
TransferFactory.runKickerCycle(transfer)
);
}

/* ------------------------------ Operator Controls ------------------------------ */

new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
.whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 4500));
operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT))
.whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kTopLeftParkingPoseBlue : DrivetrainConstants.kTopLeftParkingPoseRed)));

operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT))
.whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kTopRightParkingPoseBlue : DrivetrainConstants.kTopRightParkingPoseRed)));

new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) < 0.5)
.whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 0));
operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT))
.whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kBottomLeftParkingPoseBlue : DrivetrainConstants.kBottomLeftParkingPoseRed)));

operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT))
.whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kBottomRightParkingPoseBlue : DrivetrainConstants.kBottomRightParkingPoseRed)));

operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(() -> lift.setPower(1))
Expand All @@ -69,23 +117,38 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
operator.getGamepadButton(GamepadKeys.Button.SQUARE)
.whenPressed(TransferFactory.runKickerCycle(transfer));

operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT).toggleWhenPressed(
() -> transfer.setBlockerPosition(TransferConstants.blockerAllowPosition),
() -> transfer.setBlockerPosition(TransferConstants.blockerIdlePosition)
);

new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
.whenActive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0.75))
.whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));

new Trigger(transfer::doesTransferContainAnyBalls)
.whenActive(new InstantCommand(() -> shooter.setFlywheelCommanded(true)))
.whenInactive(new InstantCommand(() -> shooter.setFlywheelCommanded(false)));
}

public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, LED led) {
drivetrain.setDefaultCommand(new TeleopMecanum(
drivetrain,
driver::getLeftY,
() -> -driver.getLeftX(),
() -> -driver.getRightX(),
() -> false
if(GlobalConstants.getCurrentOpModeType() == GlobalConstants.OpModeType.TELEOP) {
drivetrain.setDefaultCommand(new TeleopMecanum(
drivetrain,
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
drivetrain::isRobotCentric
));
}

turret.setDefaultCommand(new AimTowardShootingRegion(
turret,
drivetrain::getPose,
GlobalConstants::getCurrentAllianceColor,
turret::isTurretAutoTrackingEnabled
));

shooter.setDefaultCommand(new MaintainShooterNumericals(
shooter,
() -> shooter.calculateFlywheelSpeeds(drivetrain.getDistanceToPose3D(shooter.getTargetPose(GlobalConstants.getCurrentAllianceColor()), 38, 12)),
() -> shooter.calculateHoodPosition(drivetrain.getDistanceToPose3D(shooter.getTargetPose(GlobalConstants.getCurrentAllianceColor()), 38, 12)),
shooter::isFlywheelCommanded
));
}
}
117 changes: 0 additions & 117 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleop.java
View file
Open in desktop

This file was deleted.

Loading
Loading