From a96ec58b6b190ff9f84cab478deb70b54845a9cb Mon Sep 17 00:00:00 2001 From: moo Date: Fri, 1 May 2026 13:26:27 -0500 Subject: [PATCH] fewafwe --- src/main/java/frc/robot/RobotContainer.java | 9 ++++++++- .../frc/robot/commands/auto_comm/ChoreoPathCommand.java | 4 ++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a6efc97..a014318 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; @@ -300,6 +301,10 @@ public class RobotContainer { } } + public void addChoreoAuto(String name, AutoRoutine auto) { + choreoAutoChooser.addCmd(name, auto::cmd); + } + /** * Initialize the SendableChooser on the SmartDashboard. * Fill the SendableChooser with available Commands. @@ -357,6 +362,7 @@ public class RobotContainer { addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false)); addAuto("testChoreo", ChoreoPathCommand.basicTrajectoryAuto("test.traj", true, autoFactory)); + addChoreoAuto("choreoLiberalLeft", ChoreoPathCommand.leftConservative(autoFactory, intake, spindexer, turret, hood)); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); @@ -396,7 +402,8 @@ public class RobotContainer { } public Command getAutoCommand() { - return autoChooser.getSelected(); + // return autoChooser.getSelected(); + return choreoAutoChooser.selectedCommand(); } public void logComponents() { diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java index 15711fd..5b0469d 100644 --- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java @@ -21,6 +21,10 @@ import frc.robot.subsystems.turret.Turret; public class ChoreoPathCommand { + public ChoreoPathCommand(Intake intake, Spindexer spindexer, Turret turret) { + + } + public static Command basicTrajectoryAuto(String pathName, boolean resetOdemetry, AutoFactory factory) { Command command = factory.trajectoryCmd(pathName); -- 2.39.5