Skip to content

VAALE updates #141

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
nlaverdure merged 16 commits into from
Mar 9, 2026
Merged

VAALE updates #141

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
88f0559
increase veolicyt, add feeder stop
caparomula Mar 7, 2026
78588ab
Auto & evlocity teaks
caparomula Mar 7, 2026
1a1f9d6
Turret desaturation tweaks
caparomula Mar 7, 2026
6e7a2fe
Fix null reference
caparomula Mar 7, 2026
21bca33
Use until() for chassis aiming command
caparomula Mar 7, 2026
76f14fd
Remove chassis aiming from autos
caparomula Mar 7, 2026
65f4a3f
Initial waypoint change
caparomula Mar 7, 2026
e860e1b
Up intake velocity, comment out divide by zero logging
caparomula Mar 8, 2026
90502d6
added back second intake motor
DylanTaylor29 Mar 8, 2026
d9fbb04
Drive velocity tweak, intake follower signals uncommented
caparomula Mar 8, 2026
ba434f3
drive constants formatting and organization
nlaverdure Mar 9, 2026
b756422
Convert intake from follower/leader to independent motors.
caparomula Mar 9, 2026
1cd5e5e
remove redundant launcher default command setters
nlaverdure Mar 9, 2026
96fb60a
Merge branch 'vaale-day1' of https://github.com/TripleHelixProgrammin…
caparomula Mar 9, 2026
7060533
Merge branch 'vaale-day1' of https://github.com/TripleHelixProgrammin…
caparomula Mar 9, 2026
652a254
constant renaming
nlaverdure Mar 9, 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
466 changes: 221 additions & 245 deletions src/main/deploy/choreo/BlueLeftNeutralZone.traj
View file
Open in desktop

Large diffs are not rendered by default.

227 changes: 102 additions & 125 deletions src/main/deploy/choreo/BlueLeftToDepot.traj
View file
Open in desktop

Large diffs are not rendered by default.

448 changes: 223 additions & 225 deletions src/main/deploy/choreo/BlueLeftTransitionToNZ.traj
View file
Open in desktop

Large diffs are not rendered by default.

560 changes: 279 additions & 281 deletions src/main/deploy/choreo/RedLeftNeutralZone.traj
View file
Open in desktop

Large diffs are not rendered by default.

235 changes: 107 additions & 128 deletions src/main/deploy/choreo/RedLeftToDepot.traj
View file
Open in desktop

Large diffs are not rendered by default.

469 changes: 230 additions & 239 deletions src/main/deploy/choreo/RedLeftTransitionToNZ.traj
View file
Open in desktop

Large diffs are not rendered by default.

16 changes: 8 additions & 8 deletions src/main/deploy/choreo/Rho.chor
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,16 @@
"poses":{
"BLTransition":{
"x":{
"exp":"4.55 m",
"val":4.55
"exp":"4 m",
"val":4.0
},
"y":{
"exp":"7.5 m",
"val":7.5
},
"heading":{
"exp":"0 deg",
"val":0.0
"exp":"180 deg",
"val":3.141592653589793
}
},
"BRTransition":{
Expand All @@ -35,16 +35,16 @@
},
"RLTransition":{
"x":{
"exp":"12.5 m",
"val":12.5
"exp":"13 m",
"val":13.0
},
"y":{
"exp":"0.6 m",
"val":0.6
},
"heading":{
"exp":"180 deg",
"val":3.141592653589793
"exp":"0 deg",
"val":0.0
}
},
"RRTransition":{
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,8 @@ public static final class CAN2 {
public static final int kicker = 17;

// Intake
public static final int intakeRollerLeader = 22;
public static final int intakeRollerFollower = 23;
public static final int intakeRollerLower = 22;
public static final int intakeRollerUpper = 23;
}

public static final class CANHD {
Expand Down
24 changes: 10 additions & 14 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 @@ -265,7 +265,13 @@ public Robot() {
intake.setDefaultCommand(intake.getDefaultCommand());
// hopper.setDefaultCommand(hopper.getDefaultCommand());
launcher.setDefaultCommand(
Commands.startEnd(launcher::stop, () -> {}, launcher).withName("Stop"));
launcher
.initializeHoodCommand()
.andThen(
new RunCommand(
() -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()),
launcher)
.withName("Aim at hub")));
}

/** This function is called periodically during all modes. */
Expand Down Expand Up @@ -329,14 +335,6 @@ public void disabledPeriodic() {
@Override
public void autonomousInit() {
drive.setDefaultCommand(Commands.runOnce(drive::stop, drive).withName("Stop"));
launcher.setDefaultCommand(
launcher
.initializeHoodCommand()
.andThen(
new RunCommand(
() -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()),
launcher)
.withName("Aim at hub")));
autoSelector.scheduleAuto();
leds.clear();
}
Expand All @@ -351,8 +349,6 @@ public void autonomousPeriodic() {
/** This function is called once when teleop mode is enabled. */
@Override
public void teleopInit() {
launcher.setDefaultCommand(
Commands.startEnd(launcher::stop, () -> {}, launcher).withName("Stop"));
autoSelector.cancelAuto();
ControllerSelector.getInstance().scan(true);
leds.clear();
Expand Down Expand Up @@ -671,17 +667,17 @@ private Command createDesaturateAndShootCommand(DriverController driver) {
driver::getYTranslationInput,
// launcher::desaturateTurret,
() -> {
if (launcher.turretDesaturated()) {
if (launcher.isTurretDesaturated()) {
return driver.getRotationInput();
} else {
return launcher.desaturateTurret();
return launcher.getTurretDesaturationDelta();
}
},
driver::getFieldRelativeInput,
allianceSelector::fieldRotated)
.withName("Desaturate turret"),
Commands.sequence(
Commands.waitUntil(launcher::turretDesaturated),
Commands.waitUntil(launcher::isTurretDesaturated),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withName("Advance")));
}
}
14 changes: 8 additions & 6 deletions src/main/java/frc/robot/auto/B_LeftTrenchAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import choreo.auto.AutoRoutine;
import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.intake.Intake;
Expand Down Expand Up @@ -54,20 +55,20 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
blueLeftNeutralZone.resetOdometry(),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta)
.withTimeout(1.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(3.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueLeftNeutralZone.cmd(),
Commands.sequence(
Commands.waitSeconds(2), intake.getDeployCommand().withTimeout(8.0)))));
blueLeftNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0))));

blueLeftNeutralZone
.done()
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -76,7 +77,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/auto/B_LeftTrenchMoveFirstAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -70,7 +70,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
8 changes: 7 additions & 1 deletion src/main/java/frc/robot/auto/B_RightTrenchAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import choreo.auto.AutoRoutine;
import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.intake.Intake;
Expand Down Expand Up @@ -54,7 +55,10 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
blueRightNeutralZone.resetOdometry(),
DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta)
.withTimeout(1.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(3.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueRightNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0))));

Expand All @@ -64,6 +68,7 @@ public AutoRoutine getAutoRoutine() {
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -72,7 +77,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/auto/B_RightTrenchMoveFirstAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ public AutoRoutine getAutoRoutine() {
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
blueRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -68,7 +69,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
14 changes: 8 additions & 6 deletions src/main/java/frc/robot/auto/R_LeftTrenchAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import choreo.auto.AutoRoutine;
import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.intake.Intake;
Expand Down Expand Up @@ -54,20 +55,20 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
redLeftNeutralZone.resetOdometry(),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta)
.withTimeout(1.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(3.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redLeftNeutralZone.cmd(),
Commands.sequence(
Commands.waitSeconds(2.0), intake.getDeployCommand().withTimeout(8.0)))));
redLeftNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0))));

redLeftNeutralZone
.done()
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redLeftTransitionToNZ.cmd(),
Commands.sequence(
Expand All @@ -78,7 +79,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/auto/R_LeftTrenchMoveFirstAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(launcher::desaturateTurret, () -> {}, launcher).withTimeout(0.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -69,7 +69,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
8 changes: 7 additions & 1 deletion src/main/java/frc/robot/auto/R_RightTrenchAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import choreo.auto.AutoRoutine;
import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.intake.Intake;
Expand Down Expand Up @@ -54,7 +55,10 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
redRightNeutralZone.resetOdometry(),
DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta)
.withTimeout(1.5),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(3.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redRightNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0))));

Expand All @@ -64,6 +68,7 @@ public AutoRoutine getAutoRoutine() {
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -72,7 +77,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/auto/R_RightTrenchMoveFirstAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ public AutoRoutine getAutoRoutine() {
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder),
Commands.parallel(
redRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));

Expand All @@ -69,7 +70,8 @@ public AutoRoutine getAutoRoutine() {
.onTrue(
Commands.sequence(
Commands.runOnce(drive::stop, drive),
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0)));
Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
Commands.runOnce(feeder::stop, feeder)));

return routine;
}
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,12 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y)
.getTranslation();
}

public static final Command getChassisAimingCommand(
Drive drive, DoubleSupplier desaturationSupplier) {
return DriveCommands.joystickDrive(
drive, () -> 0, () -> 0, desaturationSupplier, () -> true, () -> false);
}

/**
* Field relative drive command using two joysticks (controlling linear and angular velocities).
*/
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -250,7 +250,7 @@ public void runVelocity(ChassisSpeeds speeds) {
Logger.recordOutput("SwerveStates/Setpoints", states);

// 2: Desaturate (apply wheel limits FIRST)
SwerveDriveKinematics.desaturateWheelSpeeds(states, maxDriveSpeed.in(MetersPerSecond));
SwerveDriveKinematics.desaturateWheelSpeeds(states, drivetrainSpeedLimit.in(MetersPerSecond));

// 3: Reconstruct the ACTUAL chassis speeds after limiting
ChassisSpeeds limitedSpeeds = kinematics.toChassisSpeeds(states);
Expand All @@ -263,7 +263,8 @@ public void runVelocity(ChassisSpeeds speeds) {

// (Optional but usually unnecessary)
// desaturate again for safety
SwerveDriveKinematics.desaturateWheelSpeeds(finalStates, maxDriveSpeed.in(MetersPerSecond));
SwerveDriveKinematics.desaturateWheelSpeeds(
finalStates, drivetrainSpeedLimit.in(MetersPerSecond));

// 6: Send to modules
for (int i = 0; i < 4; i++) {
Expand Down
Loading