Skip to content

Next auto flow #417

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

Open
rokadias wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Next auto flow #417

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6533d8f
Setting up next auto flow.
rokadias Apr 10, 2026
ad7c902
Next auto
rokadias Apr 10, 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
6 changes: 5 additions & 1 deletion src/main/java/competition/auto_programs/AutoCommandFactory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import xbot.common.command.BaseRobot;
import xbot.common.subsystems.autonomous.AutonomousCommandSelector;

import javax.inject.Inject;
Expand Down Expand Up @@ -198,8 +199,11 @@ public Command driveToAllianceAndShoot(Command shootingDeadline) {
* @param shootingDeadline when this command ends we stop shooting
*/
public Command fireWhenReadyUntilDone(Command shootingDeadline) {
var fireAndCloseIntake = runFeederProvider.get().alongWith(intakeOscillatingProvider.get());
if (BaseRobot.isSimulation()) {
return new WaitForDurationCommand(() -> 1.0);
Copy link
Copy Markdown
Contributor

@aschokking aschokking Apr 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What's the logic here? Could you add a comment?

}

var fireAndCloseIntake = runFeederProvider.get().alongWith(intakeOscillatingProvider.get());
return waitForRotationAndHoodAndShooterToBeAtGoalProvider.get()
.andThen(shootingDeadline
.deadlineFor(fireAndCloseIntake));
Expand Down
8 changes: 1 addition & 7 deletions src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeSecondTimeCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,8 @@ public DriveToNeutralZoneForIntakeSecondTimeCommand(DriveSubsystem drive, PoseSu
@Override
public void initialize() {
var currentPose = pose.getCurrentPose2d();
var pathPoses = this.autoLandmarks.getStartCollectionPath(currentPose);
var pathPoses = this.autoLandmarks.getNearestAllianceToNeutralTrenchPath(currentPose);

var endPathPose = this.autoLandmarks.getStartCollectionPose(currentPose);
var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose);
var endPose = new Pose2d(endPathPose.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 0.75),
fullEndCollectionLocation.getRotation());

pathPoses.add(endPose);
super.setSegmentType(SegmentType.Start);
super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(currentPose, pathPoses,
false, true));
Expand Down
23 changes: 7 additions & 16 deletions src/main/java/competition/command_groups/FirstDriveForCollectionCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,46 +9,37 @@
import competition.subsystems.drive.DriveSubsystem;
import competition.subsystems.pose.AutoLandmarks;
import competition.subsystems.pose.PoseSubsystem;
import competition.subsystems.pose.RefinedSwervePointPathPlanning;
import edu.wpi.first.math.geometry.Pose2d;
import xbot.common.logging.RobotAssertionManager;
import xbot.common.properties.PropertyFactory;
import xbot.common.subsystems.drive.control_logic.HeadingModule;
import xbot.common.subsystems.oracle.SwervePointPathPlanning;
import xbot.common.subsystems.pose.GameField;
import xbot.common.trajectory.XbotSwervePoint;

public class FirstDriveForCollectionCommand extends BaseDriveWithSimpleBezierCommand {
private final AutoLandmarks autoLandmarks;
private final PoseSubsystem pose;
private final SwervePointPathPlanning pathPlanning;
private final RefinedSwervePointPathPlanning pathPlanning;

@Inject
public FirstDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose,
PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory,
RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield,
RobotAssertionManager robotAssertionManager, RefinedSwervePointPathPlanning pathPlanning, GameField gamefield,
AutoLandmarks autoLandmarks) {
super(drive, pose, pf, headingModuleFactory, robotAssertionManager);
this.pose = pose;
this.pathPlanning = pathPlanning;
this.autoLandmarks = autoLandmarks;
}

private List<XbotSwervePoint> calcSwervePoints() {
@Override
public void initialize() {
var currentPose = this.pose.getCurrentPose2d();
var startPose = this.autoLandmarks.getStartCollectionPose(currentPose);
var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose);
var endPose = new Pose2d(startPose.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 1.25),
fullEndCollectionLocation.getRotation());
var path = this.autoLandmarks.getMidBallPitCollectionPath(startPose);

List<XbotSwervePoint> points = new ArrayList<>();
points.addAll(this.pathPlanning.generateSwervePoints(startPose, endPose, false));

return points;
}

@Override
public void initialize() {
super.logic.setKeyPoints(this.calcSwervePoints());
super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(startPose, path, false, true));
super.setSegmentType(SegmentType.Mid);
super.setMaxSpeed(MaxSpeed.Intake);

Expand Down
29 changes: 10 additions & 19 deletions src/main/java/competition/command_groups/SecondDriveForCollectionCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,48 +9,39 @@
import competition.subsystems.drive.DriveSubsystem;
import competition.subsystems.pose.AutoLandmarks;
import competition.subsystems.pose.PoseSubsystem;
import competition.subsystems.pose.RefinedSwervePointPathPlanning;
import edu.wpi.first.math.geometry.Pose2d;
import xbot.common.logging.RobotAssertionManager;
import xbot.common.properties.PropertyFactory;
import xbot.common.subsystems.drive.control_logic.HeadingModule;
import xbot.common.subsystems.oracle.SwervePointPathPlanning;
import xbot.common.subsystems.pose.GameField;
import xbot.common.trajectory.XbotSwervePoint;

public class SecondDriveForCollectionCommand extends BaseDriveWithSimpleBezierCommand {
private final AutoLandmarks autoLandmarks;
private final PoseSubsystem pose;
private final SwervePointPathPlanning pathPlanning;
private final RefinedSwervePointPathPlanning pathPlanning;

@Inject
public SecondDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose,
PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory,
RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield,
RobotAssertionManager robotAssertionManager, RefinedSwervePointPathPlanning pathPlanning, GameField gamefield,
AutoLandmarks autoLandmarks) {
super(drive, pose, pf, headingModuleFactory, robotAssertionManager);
this.pose = pose;
this.pathPlanning = pathPlanning;
this.autoLandmarks = autoLandmarks;
}

private List<XbotSwervePoint> calcSwervePoints() {
var currentPose = this.pose.getCurrentPose2d();
var lastCollectFinish = this.autoLandmarks.getStartCollectionPose(currentPose);
var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose);
var startPose = new Pose2d(lastCollectFinish.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 0.75),
fullEndCollectionLocation.getRotation());
var endPose = new Pose2d(lastCollectFinish.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 1.2),
fullEndCollectionLocation.getRotation());

List<XbotSwervePoint> points = new ArrayList<>();
points.addAll(this.pathPlanning.generateSwervePoints(startPose, endPose, false));

return points;
}

@Override
public void initialize() {
super.logic.setKeyPoints(this.calcSwervePoints());
var currentPose = this.pose.getCurrentPose2d();
var lastDriveFinish = this.autoLandmarks.getNearestAllianceToNeutralTrenchPath(currentPose);
var startPose = lastDriveFinish.get(lastDriveFinish.size() - 1);
var pathPoses = this.autoLandmarks.getDriverCloserCollectionPath(currentPose);

super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(startPose, pathPoses,
false, true));
super.setSegmentType(SegmentType.Mid);
super.setMaxSpeed(MaxSpeed.Intake);

Expand Down
57 changes: 55 additions & 2 deletions src/main/java/competition/subsystems/pose/AutoLandmarks.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,18 +81,50 @@ public Pose2d getStartCollectionPose(Pose2d pose) {
return path.get(path.size() - 1);
}

public Pose2d getMidBallPitCollectionPose(Pose2d pose) {
public List<Pose2d> getMidBallPitCollectionPath(Pose2d pose) {
var results = new ArrayList<Pose2d>();
var alliance = this.getAlliance();
var ballPitEdge = Landmarks.getClosestAutoBallPitEdge(this.gamefield, pose, alliance);

var multiplierY = ballPitEdge.getY() > this.gamefield.getFieldCenter().getY() ? 1 : -1;
var multiplierX = ballPitEdge.getX() > this.gamefield.getFieldCenter().getX() ? 1 : -1;
var rotationToDriver = ballPitEdge.getY() > this.gamefield.getFieldCenter().getY() ? Rotation2d.kCCW_Pi_2 : Rotation2d.kCW_Pi_2;
var adjustedForRobot = new Translation2d(Units.Meters.of(0.5).times(multiplierX),
this.robotRadius.plus(this.pathPlanning.getAdditionalClearance()).times(multiplierY));

var adjustedTranslation = new Translation2d(this.gamefield.getFieldCenter().getX(), this.gamefield.getFieldCenter().getY())
.plus(adjustedForRobot);
return new Pose2d(adjustedTranslation, ballPitEdge.getRotation());
results.add(new Pose2d(adjustedTranslation, ballPitEdge.getRotation()));

var rotationTowardsDriverStation = alliance == Alliance.Blue
? Rotation2d.kZero
: Rotation2d.kPi;
var nextCollectionLocation = new Pose2d(
adjustedTranslation.getX() + (0.5 * multiplierX),
adjustedTranslation.getY() + (0.75 * multiplierY),
ballPitEdge.getRotation().plus(rotationToDriver));

results.add(nextCollectionLocation);

nextCollectionLocation = new Pose2d(
nextCollectionLocation.getX(),
adjustedTranslation.getY() + (0.75 * multiplierY),
rotationTowardsDriverStation);

results.add(nextCollectionLocation);

nextCollectionLocation = new Pose2d(
nextCollectionLocation.getX() + (0.5 * multiplierX),
adjustedTranslation.getY() + (0.75 * multiplierY),
ballPitEdge.getRotation().plus(Rotation2d.kPi));

results.add(nextCollectionLocation);
return results;
}

public Pose2d getMidBallPitCollectionPose(Pose2d pose) {
var path = this.getMidBallPitCollectionPath(pose);
return path.get(path.size() - 1);
}

public Pose2d getFinishBallPitCollectionPose(Pose2d pose) {
Expand All @@ -109,6 +141,27 @@ public Pose2d getFinishBallPitCollectionPose(Pose2d pose) {
returnPathPose.getRotation());
}

public List<Pose2d> getDriverCloserCollectionPath(Pose2d pose) {
var results = new ArrayList<Pose2d>();
var lastDriveFinish = this.getNearestAllianceToNeutralTrenchPath(pose);
var originalCollectionPose = this.getStartCollectionPose(pose);
var startPoseCollection = lastDriveFinish.get(lastDriveFinish.size() - 1);

var multiplierX = this.gamefield.getFieldCenter().getX() > startPoseCollection.getX() ? 1 : -1;
var multiplierY = this.gamefield.getFieldCenter().getY() > startPoseCollection.getY() ? 1 : -1;
var firstPoseAdjustment = new Translation2d(Units.Meters.of(0.05).times(multiplierX), Units.Meters.of(0.3).times(multiplierY));
var firstPathPose = new Pose2d(startPoseCollection.getTranslation().plus(firstPoseAdjustment), originalCollectionPose.getRotation());

results.add(firstPathPose);

var adjustedForRobotY = this.robotRadius.plus(this.pathPlanning.getAdditionalClearance()).times(multiplierY * -1).in(Units.Meters);
var midFieldAdjustedRobotTranslation = new Translation2d(firstPathPose.getX(), this.gamefield.getFieldCenter().getY() + adjustedForRobotY);
var endCollectionPose = new Pose2d(midFieldAdjustedRobotTranslation, firstPathPose.getRotation());
results.add(endCollectionPose);

return results;
}

public List<Pose2d> getAllianceShootingStartingPath(Pose2d pose) {
var alliance = this.getAlliance();
var results = new ArrayList<Pose2d>();
Expand Down
Loading