From 633a4ac29fbcc6203aafd136804f62bdc17e1fb4 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 10:55:26 -0800 Subject: [PATCH 1/6] auton examples --- .../main/java/org/team100/frc2026/Robot.java | 25 +++++-- .../org/team100/frc2026/auton/AllAutons.java | 57 ++++++++++++++ .../org/team100/frc2026/auton/DoNothing.java | 8 ++ .../frc2026/auton/RightTrenchLeave.java | 62 ++++++++++++++++ .../frc2026/auton/StartingPositions.java | 21 ++++++ .../org/team100/frc2026/robot/AllAutons.java | 29 -------- .../org/team100/frc2026/robot/Binder.java | 28 +++---- .../org/team100/frc2026/robot/Machinery.java | 8 +- .../ScoreSmart/ScoreCoralSmart.java | 6 +- .../ScoreSmart/ScoreCoralSmartLuke.java | 4 +- .../frc2025/Swerve/Auto/GoToCoralStation.java | 6 +- .../Swerve/ManualWithProfiledReefLock.java | 4 +- .../field/FieldConstants2025.java} | 8 +- .../field/FieldConstantsLuke.java | 7 +- .../field/MechanicalMayhem2025.java | 2 +- .../java/org/team100/frc2025/field/README.md | 3 + .../team100/frc2025/robot/AllAutons2025.java | 2 +- .../org/team100/frc2025/robot/Auton2025.java | 22 +++--- .../team100/frc2025/robot/DriveAndScore.java | 6 +- .../team100/frc2025/robot/LolipopAuto.java | 14 ++-- .../frc2026/field/FieldConstants2026.java | 74 +++++++++++++++++++ .../team100/lib/config/AnnotatedCommand.java | 5 ++ .../main/java/org/team100/lib/field/README.md | 5 -- .../java/org/team100/lib/hid/Buttons2025.java | 2 +- .../team100/lib/indicator/AutonAlerts.java | 70 ++++++++++++++++++ .../lib/targeting/SimulatedTargetWriter.java | 8 +- .../team100/frc2025/TrajectoryGallery.java | 8 +- .../team100/frc2025/field/SetpointsTest.java | 51 +++++++++++++ .../org/team100/lib/field/SetpointsTest.java | 51 ------------- 29 files changed, 438 insertions(+), 158 deletions(-) create mode 100644 comp/src/main/java/org/team100/frc2026/auton/AllAutons.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/DoNothing.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/StartingPositions.java delete mode 100644 comp/src/main/java/org/team100/frc2026/robot/AllAutons.java rename lib/src/main/java/org/team100/{lib/field/FieldConstants.java => frc2025/field/FieldConstants2025.java} (97%) rename lib/src/main/java/org/team100/{lib => frc2025}/field/FieldConstantsLuke.java (93%) rename lib/src/main/java/org/team100/{lib => frc2025}/field/MechanicalMayhem2025.java (95%) create mode 100644 lib/src/main/java/org/team100/frc2025/field/README.md create mode 100644 lib/src/main/java/org/team100/frc2026/field/FieldConstants2026.java delete mode 100644 lib/src/main/java/org/team100/lib/field/README.md create mode 100644 lib/src/main/java/org/team100/lib/indicator/AutonAlerts.java create mode 100644 lib/src/test/java/org/team100/frc2025/field/SetpointsTest.java delete mode 100644 lib/src/test/java/org/team100/lib/field/SetpointsTest.java diff --git a/comp/src/main/java/org/team100/frc2026/Robot.java b/comp/src/main/java/org/team100/frc2026/Robot.java index 6b3f0a1..c31a550 100644 --- a/comp/src/main/java/org/team100/frc2026/Robot.java +++ b/comp/src/main/java/org/team100/frc2026/Robot.java @@ -1,6 +1,6 @@ package org.team100.frc2026; -import org.team100.frc2026.robot.AllAutons; +import org.team100.frc2026.auton.AllAutons; import org.team100.frc2026.robot.Binder; import org.team100.frc2026.robot.Machinery; import org.team100.frc2026.robot.Prewarmer; @@ -10,7 +10,10 @@ import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.indicator.Alerts; +import org.team100.lib.indicator.AutonAlerts; import org.team100.lib.logging.RobotLog; +import org.team100.lib.uncertainty.IsotropicNoiseSE2; import org.team100.lib.util.Banner; import edu.wpi.first.networktables.NetworkTableInstance; @@ -28,6 +31,8 @@ public class Robot extends TimedRobot100 { private final RobotLog m_robotLog; private final Machinery m_machinery; + private final Alerts m_alerts; + private final AutonAlerts m_autonAlerts; private final AllAutons m_allAutons; private final Binder m_binder; @@ -53,10 +58,16 @@ public Robot() { m_robotLog = new RobotLog(); m_machinery = new Machinery(); - m_allAutons = new AllAutons(m_machinery); m_binder = new Binder(m_machinery); m_binder.bind(); + m_allAutons = new AllAutons(m_machinery, m_binder.m_holonomicController); + m_alerts = new Alerts(); + m_autonAlerts = new AutonAlerts( + m_allAutons::getAnnotated, + m_alerts, + (p) -> m_machinery.m_drive.resetPose(p, IsotropicNoiseSE2.high())); + Prewarmer.init(m_machinery); } @@ -77,6 +88,12 @@ public void robotPeriodic() { } } + /** Check the auton configuration when disabled. */ + @Override + public void disabledPeriodic() { + m_autonAlerts.run(); + } + ////////////////////////////////////////////////////////////////////// // // INITIALIZERS, DO NOT CHANGE THESE @@ -127,10 +144,6 @@ public void testInit() { public void simulationPeriodic() { } - @Override - public void disabledPeriodic() { - } - @Override public void autonomousPeriodic() { } diff --git a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java new file mode 100644 index 0000000..80f533b --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -0,0 +1,57 @@ +package org.team100.frc2026.auton; + +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.config.AutonChooser; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.Logging; + +import edu.wpi.first.wpilibj2.command.Command; + +/** + * Populates the Auton chooser with all available autons. + * + * It's a good idea to instantiate them all here, even if you're not using them + * all, even if they're just in development, so they don't rot. + * + * In 2026, the field is rotationally symmetric, so there's no need to fill out + * the "Alliance" part of the annotated command. + */ +public class AllAutons { + private final AutonChooser m_autonChooser; + + public AllAutons(Machinery machinery, ControllerSE2 controller) { + m_autonChooser = new AutonChooser(); + LoggerFactory log = Logging.instance().rootLogger.name("Auton"); + m_autonChooser.add("Do Nothing", + new AnnotatedCommand( + new DoNothing().withName("Nothing from right bump"), + null, + StartingPositions.RIGHT_BUMP)); + m_autonChooser.add("Right Trench Leave", + AnnotatedCommand.eitherAlliance( + new RightTrenchLeave().get( + log, + machinery.m_swerveKinodynamics, + controller, + machinery), + StartingPositions.RIGHT_TRENCH)); + } + + public Command get() { + AnnotatedCommand annotatedCommand = getAnnotated(); + if (annotatedCommand == null) + return null; + return annotatedCommand.command(); + } + + public AnnotatedCommand getAnnotated() { + return m_autonChooser.get(); + } + + public void close() { + m_autonChooser.close(); + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java new file mode 100644 index 0000000..82202c4 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java @@ -0,0 +1,8 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.wpilibj2.command.Command; + +/** An auton that does nothing at all. */ +public class DoNothing extends Command { + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java new file mode 100644 index 0000000..cd65b3f --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java @@ -0,0 +1,62 @@ +package org.team100.frc2026.auton; + +import java.util.List; +import java.util.function.Function; + +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.trajectory.TrajectorySE2; +import org.team100.lib.trajectory.TrajectorySE2Factory; +import org.team100.lib.trajectory.TrajectorySE2Planner; +import org.team100.lib.trajectory.constraint.TimingConstraint; +import org.team100.lib.trajectory.constraint.TimingConstraintFactory; +import org.team100.lib.trajectory.path.PathSE2Factory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; + +/** Drives a short distance from the right trench starting point */ +public class RightTrenchLeave { + + public Command get( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + LoggerFactory log = parent.name("RightTrenchLeave"); + List constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + TrajectorySE2Factory trajectoryFactory = new TrajectorySE2Factory(constraints); + PathSE2Factory pathFactory = new PathSE2Factory(); + TrajectorySE2Planner planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + + Function trajectoryFn = (p) -> { + List waypoints = List.of( + new WaypointSE2( + p, + new DirectionSE2(1, 0, 0), + 1), + new WaypointSE2( + new Pose2d(), + new DirectionSE2(1, 0, 0), + 1)); + return planner.restToRest(waypoints); + }; + + DriveWithTrajectoryFunction navigator = new DriveWithTrajectoryFunction( + log, + machinery.m_drive, + controller, + machinery.m_trajectoryViz, + trajectoryFn); + + return navigator + .until(navigator::isDone); + + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/StartingPositions.java b/comp/src/main/java/org/team100/frc2026/auton/StartingPositions.java new file mode 100644 index 0000000..a64b346 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/StartingPositions.java @@ -0,0 +1,21 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * These measurements are guesses. + * + * G303: Starting positions must overlap the starting line and not contact the + * bump. + */ +public class StartingPositions { + /** Barely overlaps the tape */ + private static final double X = 4.0; + public static final Pose2d LEFT_TRENCH = new Pose2d(X, 7.4, Rotation2d.kZero); + public static final Pose2d LEFT_BUMP = new Pose2d(X, 5.5, Rotation2d.kZero); + public static final Pose2d CENTER = new Pose2d(X, 4.0, Rotation2d.kZero); + public static final Pose2d RIGHT_BUMP = new Pose2d(X, 2.5, Rotation2d.kZero); + public static final Pose2d RIGHT_TRENCH = new Pose2d(X, 0.6, Rotation2d.kZero); + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java b/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java deleted file mode 100644 index 5403ed3..0000000 --- a/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.team100.frc2026.robot; - -import org.team100.lib.config.AutonChooser; - -import edu.wpi.first.wpilibj2.command.Command; - -/** - * Populates the Auton chooser with all available autons. - * - * It's a good idea to instantiate them all here, even if you're not using them - * all, even if they're just in development, so they don't rot. - */ -public class AllAutons { - private final AutonChooser m_autonChooser; - - public AllAutons(Machinery machinery) { - m_autonChooser = new AutonChooser(); - - } - - public Command get() { - return m_autonChooser.get().command(); - } - - public void close() { - m_autonChooser.close(); - } - -} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index 4a883a0..444dcad 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -36,13 +36,17 @@ public class Binder { private static final LoggerFactory rootLogger = Logging.instance().rootLogger; private static final LoggerFactory fieldLogger = Logging.instance().fieldLogger; private final Machinery m_machinery; + public final ControllerSE2 m_holonomicController; + final LoggerFactory m_log; public Binder(Machinery machinery) { m_machinery = machinery; + m_log = rootLogger.name("Commands"); + m_holonomicController = ControllerFactorySE2.byIdentity(m_log); + } public void bind() { - final LoggerFactory log = rootLogger.name("Commands"); ///////////////////////////////////////////////// /// @@ -56,7 +60,7 @@ public void bind() { // SwerveLimiter limiter = new SwerveLimiter( - log, + m_log, m_machinery.m_swerveKinodynamics, RobotController::getBatteryVoltage); limiter.updateSetpoint(m_machinery.m_drive.getVelocity()); @@ -67,7 +71,7 @@ public void bind() { m_machinery.m_drive, limiter); m_machinery.m_drive.setDefaultCommand(driveManually.withName("drive default")); - final LoggerFactory manLog = log.type(driveManually); + final LoggerFactory manLog = m_log.type(driveManually); driveManually.register("MODULE_STATE", false, new SimpleManualModuleStates(manLog, m_machinery.m_swerveKinodynamics)); @@ -122,22 +126,20 @@ public void bind() { // DRIVETRAIN // - final ControllerSE2 holonomicController = ControllerFactorySE2.byIdentity(log); - // This is for testing pose estimation accuracy and drivetrain positioning // accuracy. HolonomicProfile profile = HolonomicProfileFactory.get( - log, m_machinery.m_swerveKinodynamics, 1, 0.5, 1, 0.2); + m_log, m_machinery.m_swerveKinodynamics, 1, 0.5, 1, 0.2); onTrue(driver::a, new DriveToPoseWithProfile( - log, m_machinery.m_drive, holonomicController, profile, + m_log, m_machinery.m_drive, m_holonomicController, profile, () -> new Pose2d(15.387, 3.501, new Rotation2d(0)))); ///////////////////////////////////////////////////////// /// /// SUBSYSTEMS /// whileTrue(driver::b, m_machinery.m_shooter.shoot()); - + whileTrue(driver::x, m_machinery.m_intake.intake()); whileTrue(driver::y, m_machinery.m_serializer.serialize()); @@ -145,18 +147,16 @@ public void bind() { // Test bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToExtendedPosition()); whileTrue(driver::rightBumper, m_machinery.m_extender.goToRetractedPosition()); - whileTrue(driver:: rightTrigger, m_machinery.m_ClimberExtension.setPosition()); - whileTrue(driver:: x, m_machinery.m_Climber.setClimb0()); - whileTrue(driver:: y, m_machinery.m_Climber.setClimb1()); - whileTrue(driver:: b, m_machinery.m_Climber.setClimb3()); + whileTrue(driver::rightTrigger, m_machinery.m_ClimberExtension.setPosition()); + whileTrue(driver::x, m_machinery.m_Climber.setClimb0()); + whileTrue(driver::y, m_machinery.m_Climber.setClimb1()); + whileTrue(driver::b, m_machinery.m_Climber.setClimb3()); // The real bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToRetractedPosition()); whileTrue(driver::leftTrigger, m_machinery.m_extender.goToExtendedPosition() .andThen(m_machinery.m_intake.intake())); - - /////////////////////////////////////////////////////////// // diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 784da7f..35624ba 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -33,6 +33,7 @@ import org.team100.lib.uncertainty.VariableR1; import org.team100.lib.util.CanId; import org.team100.lib.visualization.RobotPoseVisualization; +import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Twist2d; @@ -62,9 +63,10 @@ public class Machinery { private final SwerveModuleCollection m_modules; private final Runnable m_simulatedTagDetector; - final SwerveKinodynamics m_swerveKinodynamics; + public final TrajectoryVisualization m_trajectoryViz; + public final SwerveKinodynamics m_swerveKinodynamics; final AprilTagRobotLocalizer m_localizer; - final SwerveDriveSubsystem m_drive; + public final SwerveDriveSubsystem m_drive; final Beeper m_beeper; final Shooter m_shooter; final Intake m_intake; @@ -96,7 +98,7 @@ public Machinery() { // VISUALIZATIONS // - // Visualization initializers go here + m_trajectoryViz = new TrajectoryVisualization(fieldLogger); //////////////////////////////////////////////////////////// // diff --git a/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java b/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java index 0fad638..215efc4 100644 --- a/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java +++ b/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java @@ -10,11 +10,11 @@ import java.util.function.Supplier; import org.team100.frc2025.CalgamesArm.CalgamesMech; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.ControllerSE2; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; @@ -36,7 +36,7 @@ public static Command get( DoubleConsumer heedRadiusM, Supplier level, Supplier point) { - Supplier goal = () -> FieldConstants.makeGoal(level.get(), point.get()); + Supplier goal = () -> FieldConstants2025.makeGoal(level.get(), point.get()); return parallel( runOnce(() -> heedRadiusM.accept(HEED_RADIUS_M)), select(Map.ofEntries( diff --git a/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java b/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java index 2ce6576..f74da8b 100644 --- a/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java +++ b/lib/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java @@ -7,11 +7,11 @@ import java.util.function.Supplier; import org.team100.frc2025.CalgamesArm.CalgamesMech; +import org.team100.frc2025.field.FieldConstantsLuke; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.ControllerSE2; -import org.team100.lib.field.FieldConstants.ReefPoint; -import org.team100.lib.field.FieldConstantsLuke; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; diff --git a/lib/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java b/lib/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java index fc5b4a3..a43719f 100644 --- a/lib/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java +++ b/lib/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java @@ -4,8 +4,8 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.CoralStation; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.CoralStation; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -54,7 +54,7 @@ public TrajectorySE2 apply(Pose2d currentPose) { Translation2d currTranslation = currentPose.getTranslation(); Rotation2d courseToGoal = goal.getTranslation().minus(currTranslation).getAngle(); - Rotation2d newInitialSpline = FieldConstants.calculateDeltaSpline( + Rotation2d newInitialSpline = FieldConstants2025.calculateDeltaSpline( courseToGoal, courseToGoal.rotateBy(Rotation2d.fromDegrees(-90)), scaleAdjust); List waypoints = new ArrayList<>(); diff --git a/lib/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java b/lib/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java index 11a5701..f8fc3df 100644 --- a/lib/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java +++ b/lib/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java @@ -2,8 +2,8 @@ import java.util.function.Supplier; +import org.team100.frc2025.field.FieldConstants2025; import org.team100.lib.controller.r1.FeedbackR1; -import org.team100.lib.field.FieldConstants; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; @@ -127,7 +127,7 @@ public VelocitySE2 apply( // take the short path Rotation2d m_goal = Math100.getMinDistance( yawMeasurement, - FieldConstants.angleToReefCenter(m_robotLocation.get())); + FieldConstants2025.angleToReefCenter(m_robotLocation.get())); // use the modulus closest to the measurement m_thetaSetpoint = new ControlR1( diff --git a/lib/src/main/java/org/team100/lib/field/FieldConstants.java b/lib/src/main/java/org/team100/frc2025/field/FieldConstants2025.java similarity index 97% rename from lib/src/main/java/org/team100/lib/field/FieldConstants.java rename to lib/src/main/java/org/team100/frc2025/field/FieldConstants2025.java index 80ed74c..f6d5d0f 100644 --- a/lib/src/main/java/org/team100/lib/field/FieldConstants.java +++ b/lib/src/main/java/org/team100/frc2025/field/FieldConstants2025.java @@ -1,4 +1,4 @@ -package org.team100.lib.field; +package org.team100.frc2025.field; import org.team100.lib.config.ElevatorUtil.ScoringLevel; @@ -9,7 +9,7 @@ /** * Positions on the field used as waypoints. */ -public class FieldConstants { +public class FieldConstants2025 { /** Staged coral ("lollipop") locations. */ public enum CoralMark { LEFT(new Translation2d(1.219, 5.855)), @@ -33,7 +33,7 @@ public enum ReefPoint { A, B, C, D, E, F, G, H, I, J, K, L, AB, CD, EF, GH, IJ, KL, NONE; /** Direction away from each reef face. */ - Rotation2d angle() { + public Rotation2d angle() { return switch (this) { case A, B, AB -> Rotation2d.fromDegrees(180); case C, D, CD -> Rotation2d.fromDegrees(-120); @@ -86,7 +86,7 @@ public static Pose2d makeGoal(ScoringLevel level, ReefPoint point) { /////////////////////////// - static Translation2d getScoringDestination(ReefPoint point, double radius) { + public static Translation2d getScoringDestination(ReefPoint point, double radius) { Rotation2d sectorAngle = point.angle(); Translation2d spoke = new Translation2d(radius, sectorAngle); // center of the face of the reef diff --git a/lib/src/main/java/org/team100/lib/field/FieldConstantsLuke.java b/lib/src/main/java/org/team100/frc2025/field/FieldConstantsLuke.java similarity index 93% rename from lib/src/main/java/org/team100/lib/field/FieldConstantsLuke.java rename to lib/src/main/java/org/team100/frc2025/field/FieldConstantsLuke.java index d50aa34..5279a0b 100644 --- a/lib/src/main/java/org/team100/lib/field/FieldConstantsLuke.java +++ b/lib/src/main/java/org/team100/frc2025/field/FieldConstantsLuke.java @@ -1,7 +1,7 @@ -package org.team100.lib.field; +package org.team100.frc2025.field; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.field.FieldConstants.ReefPoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,7 +23,6 @@ private CoralMark(Translation2d t) { private static final Translation2d REEF_CENTER = new Translation2d(4.489, 4.026); - static Translation2d getScoringDestination(ReefPoint point, double radius) { Rotation2d sectorAngle = point.angle(); Translation2d spoke = new Translation2d(radius, sectorAngle); @@ -51,7 +50,7 @@ static Translation2d getScoringDestination(ReefPoint point, double radius) { } public static Pose2d makeGoal(ScoringLevel level, ReefPoint point) { - double radius = FieldConstants.getRadius(point, level); + double radius = FieldConstants2025.getRadius(point, level); Translation2d destination = getScoringDestination(point, radius); Rotation2d heading = point.angle(); return new Pose2d(destination, heading); diff --git a/lib/src/main/java/org/team100/lib/field/MechanicalMayhem2025.java b/lib/src/main/java/org/team100/frc2025/field/MechanicalMayhem2025.java similarity index 95% rename from lib/src/main/java/org/team100/lib/field/MechanicalMayhem2025.java rename to lib/src/main/java/org/team100/frc2025/field/MechanicalMayhem2025.java index 5805be7..5969042 100644 --- a/lib/src/main/java/org/team100/lib/field/MechanicalMayhem2025.java +++ b/lib/src/main/java/org/team100/frc2025/field/MechanicalMayhem2025.java @@ -1,4 +1,4 @@ -package org.team100.lib.field; +package org.team100.frc2025.field; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/lib/src/main/java/org/team100/frc2025/field/README.md b/lib/src/main/java/org/team100/frc2025/field/README.md new file mode 100644 index 0000000..04c230f --- /dev/null +++ b/lib/src/main/java/org/team100/frc2025/field/README.md @@ -0,0 +1,3 @@ +# frc2025.field + +This package contains constants representing field coordinates. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java b/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java index 545ed5d..8d4fd9a 100644 --- a/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java +++ b/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java @@ -2,12 +2,12 @@ import java.util.List; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.config.AnnotatedCommand; import org.team100.lib.config.AutonChooser; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.ControllerFactorySE2; import org.team100.lib.controller.se2.FullStateControllerSE2; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; import org.team100.lib.profile.se2.HolonomicProfileFactory; diff --git a/lib/src/main/java/org/team100/frc2025/robot/Auton2025.java b/lib/src/main/java/org/team100/frc2025/robot/Auton2025.java index 346e445..b42f942 100644 --- a/lib/src/main/java/org/team100/frc2025/robot/Auton2025.java +++ b/lib/src/main/java/org/team100/frc2025/robot/Auton2025.java @@ -5,22 +5,22 @@ import static edu.wpi.first.wpilibj2.command.Commands.runOnce; import static edu.wpi.first.wpilibj2.command.Commands.sequence; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.C; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.D; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.F; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.H; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.I; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.K; +import static org.team100.frc2025.field.FieldConstants2025.ReefPoint.L; import static org.team100.lib.config.ElevatorUtil.ScoringLevel.L4; -import static org.team100.lib.field.FieldConstants.ReefPoint.C; -import static org.team100.lib.field.FieldConstants.ReefPoint.D; -import static org.team100.lib.field.FieldConstants.ReefPoint.F; -import static org.team100.lib.field.FieldConstants.ReefPoint.H; -import static org.team100.lib.field.FieldConstants.ReefPoint.I; -import static org.team100.lib.field.FieldConstants.ReefPoint.K; -import static org.team100.lib.field.FieldConstants.ReefPoint.L; import org.team100.frc2025.Swerve.Auto.GoToCoralStation; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.CoralStation; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.FullStateControllerSE2; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.CoralStation; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; @@ -102,7 +102,7 @@ private Command embarkAndPreplace(ScoringLevel position, ReefPoint point) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.makeGoal(position, point)); + () -> FieldConstants2025.makeGoal(position, point)); MoveAndHold toL4 = m_machinery.m_mech.homeToL4(); return parallel( runOnce(() -> m_machinery.m_localizer.setHeedRadiusM(HEED_RADIUS_M)), diff --git a/lib/src/main/java/org/team100/frc2025/robot/DriveAndScore.java b/lib/src/main/java/org/team100/frc2025/robot/DriveAndScore.java index 5ab8d5a..3a0b261 100644 --- a/lib/src/main/java/org/team100/frc2025/robot/DriveAndScore.java +++ b/lib/src/main/java/org/team100/frc2025/robot/DriveAndScore.java @@ -5,11 +5,11 @@ import static edu.wpi.first.wpilibj2.command.Commands.sequence; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.FullStateControllerSE2; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; @@ -44,7 +44,7 @@ public Command get(ScoringLevel level, ReefPoint point) { MoveAndHold toReef = new DriveToPoseWithProfile( m_logger, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.makeGoal(level, point)); + () -> FieldConstants2025.makeGoal(level, point)); MoveAndHold toL4 = m_machinery.m_mech.homeToL4(); Command eject = m_machinery.m_manipulator.centerEject().withTimeout(0.5); return sequence( diff --git a/lib/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/lib/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index 629f715..da1feb5 100644 --- a/lib/src/main/java/org/team100/frc2025/robot/LolipopAuto.java +++ b/lib/src/main/java/org/team100/frc2025/robot/LolipopAuto.java @@ -7,11 +7,11 @@ import java.util.List; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.FullStateControllerSE2; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.profile.se2.ProfileSE2; @@ -58,27 +58,27 @@ public Command get() { DriveToPoseWithProfile toReefA = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.makeGoal(ScoringLevel.L4, ReefPoint.A)); + () -> FieldConstants2025.makeGoal(ScoringLevel.L4, ReefPoint.A)); DriveToTranslationFacingWithProfile toCenterCoral = new DriveToTranslationFacingWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.CoralMark.CENTER.value + () -> FieldConstants2025.CoralMark.CENTER.value .plus(new Translation2d(0.7, 0)), new Rotation2d(Math.PI)); DriveToPoseWithProfile toReefB = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.makeGoal(ScoringLevel.L4, ReefPoint.B)); + () -> FieldConstants2025.makeGoal(ScoringLevel.L4, ReefPoint.B)); DriveToTranslationFacingWithProfile toCoralRight = new DriveToTranslationFacingWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.CoralMark.RIGHT.value + () -> FieldConstants2025.CoralMark.RIGHT.value .plus(new Translation2d(0.7, 0).rotateBy(new Rotation2d(Math.PI / 4))), new Rotation2d(Math.PI)); DriveToPoseWithProfile toReefC = new DriveToPoseWithProfile( m_log, m_machinery.m_drive, m_autoController, m_autoProfile, - () -> FieldConstants.makeGoal(ScoringLevel.L4, ReefPoint.C)); + () -> FieldConstants2025.makeGoal(ScoringLevel.L4, ReefPoint.C)); MoveAndHold toL4 = m_machinery.m_mech.homeToL4(); MoveAndHold toL4second = m_machinery.m_mech.homeToL4(); diff --git a/lib/src/main/java/org/team100/frc2026/field/FieldConstants2026.java b/lib/src/main/java/org/team100/frc2026/field/FieldConstants2026.java new file mode 100644 index 0000000..09a35ac --- /dev/null +++ b/lib/src/main/java/org/team100/frc2026/field/FieldConstants2026.java @@ -0,0 +1,74 @@ +package org.team100.frc2026.field; + +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + +/** + * These measurements are from Onshape. + * + * https://cad.onshape.com/documents/8a691e28680da30504859fce/w/c6aa636fb23edb3f1e272fb1/e/c043973ea96914e2eaa1e8fc + */ +public class FieldConstants2026 { + /** Field width in meters */ + private static final double FIELD_WIDTH = 8.069; + private static final double FIELD_LENGTH = 16.541; + /** + * Center of the hub target, inside the cone. + * + * Use this for aiming. + * + * Height is the bottom of the cone. + */ + public static final Translation3d HUB = new Translation3d( + 4.626, 4.035, 1.434); + /** + * The rectangular base of the hub. + * + * Avoid hitting this when lobbing. + */ + public static final Rectangle2d HUB_BASE = new Rectangle2d( + new Translation2d(4.028, 3.438), + new Translation2d(5.223, 4.631)); + /** + * The rectangular base of the opponent's hub. + * + * Avoid hitting this when lobbing. + */ + public static final Rectangle2d OPPONENT_HUB_BASE = new Rectangle2d( + new Translation2d(11.318, 3.438), + new Translation2d(12.512, 4.631)); + /** + * Area on the field where you're allowed to shoot. + * + * Measured from the outside edge of the tape to the wall. + * + * Does not include the bump. + * + * G407: bumpers must overlap this zone, or major foul (15 pt). + */ + public static final Rectangle2d ALLIANCE_ZONE = new Rectangle2d( + new Translation2d(0.000, 0.000), + new Translation2d(4.029, FIELD_WIDTH)); + /** + * Middle zone of the field. + * + * Measured between the faces of each hub. + * + * Does not include the bump. + */ + public static final Rectangle2d NEUTRAL_ZONE = new Rectangle2d( + new Translation2d(5.223, 0.000), + new Translation2d(11.318, FIELD_WIDTH)); + + /** + * Alliance zone on the opposite side of the field. + * + * Measured from the outside edge of the tape to the wall. + * + * Does not include the bump. + */ + public static final Rectangle2d OPPONENT_ZONE = new Rectangle2d( + new Translation2d(12.512, 0.000), + new Translation2d(FIELD_LENGTH, FIELD_WIDTH)); +} diff --git a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java index ec7f6f3..e9951d3 100644 --- a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java +++ b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java @@ -16,4 +16,9 @@ * @param start starting pose */ public record AnnotatedCommand(Command command, Alliance alliance, Pose2d start) { + + /** For rotationally-symmetric fields */ + public static AnnotatedCommand eitherAlliance(Command c, Pose2d start) { + return new AnnotatedCommand(c, null, start); + } } diff --git a/lib/src/main/java/org/team100/lib/field/README.md b/lib/src/main/java/org/team100/lib/field/README.md deleted file mode 100644 index 1eaadb1..0000000 --- a/lib/src/main/java/org/team100/lib/field/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# lib.field - -This package contains constants representing field coordinates. - -There's game-specific stuff in here because it's used by the control API. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/hid/Buttons2025.java b/lib/src/main/java/org/team100/lib/hid/Buttons2025.java index 15a4cd9..2b3503a 100644 --- a/lib/src/main/java/org/team100/lib/hid/Buttons2025.java +++ b/lib/src/main/java/org/team100/lib/hid/Buttons2025.java @@ -1,7 +1,7 @@ package org.team100.lib.hid; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.field.FieldConstants.ReefPoint; import edu.wpi.first.wpilibj.GenericHID; diff --git a/lib/src/main/java/org/team100/lib/indicator/AutonAlerts.java b/lib/src/main/java/org/team100/lib/indicator/AutonAlerts.java new file mode 100644 index 0000000..9508c55 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/indicator/AutonAlerts.java @@ -0,0 +1,70 @@ +package org.team100.lib.indicator; + +import java.util.function.Consumer; +import java.util.function.Supplier; + +import org.team100.lib.config.AnnotatedCommand; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class AutonAlerts implements Runnable { + private final Supplier m_autons; + private final Consumer m_poseSetter; + private final Alert m_noStartingPosition; + private final Alert m_mismatchedAlliance; + + public AutonAlerts( + Supplier autons, + Alerts alerts, + Consumer poseSetter) { + m_autons = autons; + m_poseSetter = poseSetter; + m_noStartingPosition = alerts.add("No starting position!", AlertType.kWarning); + m_mismatchedAlliance = alerts.add("Wrong Alliance!", AlertType.kWarning); + } + + @Override + public void run() { + checkStart(); + checkAlliance(); + } + + private void checkStart() { + AnnotatedCommand cmd = m_autons.get(); + if (cmd == null) + return; + Pose2d start = cmd.start(); + if (start == null) { + m_noStartingPosition.set(true); + } else { + m_noStartingPosition.set(false); + m_poseSetter.accept(start); + } + } + + private void checkAlliance() { + AnnotatedCommand cmd = m_autons.get(); + if (cmd == null) + return; + Alliance alliance = cmd.alliance(); + if (alliance == null) { + // works for either + return; + } + Alliance dsAlliance = DriverStation.getAlliance().orElse(null); + if (dsAlliance == null) { + // not set yet + return; + } + if (alliance != dsAlliance) { + m_mismatchedAlliance.set(true); + } else { + m_mismatchedAlliance.set(false); + } + } + +} diff --git a/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java b/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java index 2e6113c..34bea2d 100644 --- a/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java +++ b/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java @@ -5,9 +5,9 @@ import java.util.Map; import java.util.function.DoubleFunction; +import org.team100.frc2025.field.FieldConstants2025; import org.team100.lib.coherence.Takt; import org.team100.lib.config.Camera; -import org.team100.lib.field.FieldConstants; import org.team100.lib.localization.SwerveHistory; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -86,9 +86,9 @@ public static Runnable get(LoggerFactory parent, SwerveHistory history) { Camera.CORAL_RIGHT), history, new Translation2d[] { - FieldConstants.CoralMark.LEFT.value, - FieldConstants.CoralMark.CENTER.value, - FieldConstants.CoralMark.RIGHT.value }); + FieldConstants2025.CoralMark.LEFT.value, + FieldConstants2025.CoralMark.CENTER.value, + FieldConstants2025.CoralMark.RIGHT.value }); return tsim::update; } diff --git a/lib/src/test/java/org/team100/frc2025/TrajectoryGallery.java b/lib/src/test/java/org/team100/frc2025/TrajectoryGallery.java index 5e84dc7..5ac585f 100644 --- a/lib/src/test/java/org/team100/frc2025/TrajectoryGallery.java +++ b/lib/src/test/java/org/team100/frc2025/TrajectoryGallery.java @@ -9,10 +9,10 @@ import org.junit.jupiter.api.Test; import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.Swerve.Auto.GoToCoralStation; +import org.team100.frc2025.field.FieldConstants2025; +import org.team100.frc2025.field.FieldConstants2025.CoralStation; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.field.FieldConstants; -import org.team100.lib.field.FieldConstants.CoralStation; -import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; @@ -113,7 +113,7 @@ private List series(DirectionSE2 m_course, WaypointSE2 m_goal) { private List series(CoralStation coralStation, ReefPoint reefPoint, ScoringLevel scoringLevel) { GoToCoralStation toStation = new GoToCoralStation(log, swerveKinodynamics, coralStation, 0.5); // the start is the goal from the previous maneuver - Pose2d start = FieldConstants.makeGoal(scoringLevel, reefPoint); + Pose2d start = FieldConstants2025.makeGoal(scoringLevel, reefPoint); TrajectorySE2 trajectory = toStation.apply(start); return new TrajectorySE2ToVectorSeries(0.1).convert(trajectory); } diff --git a/lib/src/test/java/org/team100/frc2025/field/SetpointsTest.java b/lib/src/test/java/org/team100/frc2025/field/SetpointsTest.java new file mode 100644 index 0000000..802578c --- /dev/null +++ b/lib/src/test/java/org/team100/frc2025/field/SetpointsTest.java @@ -0,0 +1,51 @@ +package org.team100.frc2025.field; + +import org.junit.jupiter.api.Test; +import org.team100.frc2025.field.FieldConstants2025.ReefPoint; + +import edu.wpi.first.math.geometry.Translation2d; + +/** + * Check the positions of the scoring points. + * + * See chart: + * + * https://docs.google.com/spreadsheets/d/1I3ZZU4iZdwebHROykgXosJXGss2YPVTT3JEtHqNjdpQ + */ +public class SetpointsTest { + private static final boolean DEBUG = false; + + void print(Translation2d t) { + if (DEBUG) + System.out.printf("%6.3f %6.3f\n", t.getX(), t.getY()); + } + + @Test + void testSetA() { + if (DEBUG) + System.out.println("x, y"); + print(FieldConstants2025.getScoringDestination(ReefPoint.A, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.AB, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.B, 1.4)); + + print(FieldConstants2025.getScoringDestination(ReefPoint.C, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.CD, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.D, 1.4)); + + print(FieldConstants2025.getScoringDestination(ReefPoint.E, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.EF, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.F, 1.4)); + + print(FieldConstants2025.getScoringDestination(ReefPoint.G, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.GH, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.H, 1.4)); + + print(FieldConstants2025.getScoringDestination(ReefPoint.I, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.IJ, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.J, 1.4)); + + print(FieldConstants2025.getScoringDestination(ReefPoint.K, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.KL, 1.4)); + print(FieldConstants2025.getScoringDestination(ReefPoint.L, 1.4)); + } +} diff --git a/lib/src/test/java/org/team100/lib/field/SetpointsTest.java b/lib/src/test/java/org/team100/lib/field/SetpointsTest.java deleted file mode 100644 index f471550..0000000 --- a/lib/src/test/java/org/team100/lib/field/SetpointsTest.java +++ /dev/null @@ -1,51 +0,0 @@ -package org.team100.lib.field; - -import org.junit.jupiter.api.Test; -import org.team100.lib.field.FieldConstants.ReefPoint; - -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Check the positions of the scoring points. - * - * See chart: - * - * https://docs.google.com/spreadsheets/d/1I3ZZU4iZdwebHROykgXosJXGss2YPVTT3JEtHqNjdpQ - */ -public class SetpointsTest { - private static final boolean DEBUG = false; - - void print(Translation2d t) { - if (DEBUG) - System.out.printf("%6.3f %6.3f\n", t.getX(), t.getY()); - } - - @Test - void testSetA() { - if (DEBUG) - System.out.println("x, y"); - print(FieldConstants.getScoringDestination(ReefPoint.A, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.AB, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.B, 1.4)); - - print(FieldConstants.getScoringDestination(ReefPoint.C, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.CD, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.D, 1.4)); - - print(FieldConstants.getScoringDestination(ReefPoint.E, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.EF, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.F, 1.4)); - - print(FieldConstants.getScoringDestination(ReefPoint.G, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.GH, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.H, 1.4)); - - print(FieldConstants.getScoringDestination(ReefPoint.I, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.IJ, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.J, 1.4)); - - print(FieldConstants.getScoringDestination(ReefPoint.K, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.KL, 1.4)); - print(FieldConstants.getScoringDestination(ReefPoint.L, 1.4)); - } -} From c1ed498554a401ab3f514fde58f83a586afe4d3d Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 11:24:56 -0800 Subject: [PATCH 2/6] WIP --- .../team100/lib/config/AnnotatedCommand.java | 29 ++++++++++++------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java index e9951d3..9204ab7 100644 --- a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java +++ b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java @@ -7,18 +7,25 @@ /** * A command with annotations that are checked against ground truth while * disabled. - * - * If you don't care about the annotations at all, e.g. because the same command - * works on both sides from anywhere, use null. - * - * @param command command to run - * @param alliance red or blue - * @param start starting pose */ -public record AnnotatedCommand(Command command, Alliance alliance, Pose2d start) { +public interface AnnotatedCommand { - /** For rotationally-symmetric fields */ - public static AnnotatedCommand eitherAlliance(Command c, Pose2d start) { - return new AnnotatedCommand(c, null, start); + /** + * command to run + */ + Command command(); + + /** + * red or blue, null if it works for both. + */ + default Alliance alliance() { + return null; + } + + /** + * starting pose, null if it doesn't matter. + */ + default Pose2d start() { + return null; } } From ee10b4d536e8b7ac0aff065425f6fcea57a4688f Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 11:53:53 -0800 Subject: [PATCH 3/6] WIP --- .../org/team100/frc2026/auton/AllAutons.java | 19 ++--- .../org/team100/frc2026/auton/DoNothing.java | 16 ++++- .../frc2026/auton/RightTrenchLeave.java | 71 ++++++++++++------- .../team100/frc2025/robot/AllAutons2025.java | 48 +++++++------ .../team100/lib/config/AnnotatedCommand.java | 11 ++- .../lib/config/AnnotatedCommandImpl.java | 48 +++++++++++++ .../org/team100/lib/config/AutonChooser.java | 26 +++++-- 7 files changed, 168 insertions(+), 71 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/config/AnnotatedCommandImpl.java diff --git a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java index 80f533b..66399fd 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -24,19 +24,12 @@ public class AllAutons { public AllAutons(Machinery machinery, ControllerSE2 controller) { m_autonChooser = new AutonChooser(); LoggerFactory log = Logging.instance().rootLogger.name("Auton"); - m_autonChooser.add("Do Nothing", - new AnnotatedCommand( - new DoNothing().withName("Nothing from right bump"), - null, - StartingPositions.RIGHT_BUMP)); - m_autonChooser.add("Right Trench Leave", - AnnotatedCommand.eitherAlliance( - new RightTrenchLeave().get( - log, - machinery.m_swerveKinodynamics, - controller, - machinery), - StartingPositions.RIGHT_TRENCH)); + m_autonChooser.add(new DoNothing()); + m_autonChooser.add(new RightTrenchLeave( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() { diff --git a/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java index 82202c4..651d289 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java +++ b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java @@ -1,8 +1,22 @@ package org.team100.frc2026.auton; +import org.team100.lib.config.AnnotatedCommand; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; /** An auton that does nothing at all. */ -public class DoNothing extends Command { +public class DoNothing implements AnnotatedCommand { + + @Override + public Command command() { + return Commands.idle().withName("Nothing from right bump"); + } + + @Override + public Pose2d start() { + return StartingPositions.RIGHT_BUMP; + } } diff --git a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java index cd65b3f..fb34832 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java +++ b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java @@ -1,9 +1,9 @@ package org.team100.frc2026.auton; import java.util.List; -import java.util.function.Function; import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; @@ -21,42 +21,59 @@ import edu.wpi.first.wpilibj2.command.Command; /** Drives a short distance from the right trench starting point */ -public class RightTrenchLeave { +public class RightTrenchLeave implements AnnotatedCommand { + private final LoggerFactory log; + private final ControllerSE2 controller; + private final Machinery machinery; + private final List constraints; + private final TrajectorySE2Factory trajectoryFactory; + private final PathSE2Factory pathFactory; + private final TrajectorySE2Planner planner; - public Command get( - LoggerFactory parent, + public RightTrenchLeave(LoggerFactory parent, SwerveKinodynamics kinodynamics, ControllerSE2 controller, Machinery machinery) { - LoggerFactory log = parent.name("RightTrenchLeave"); - List constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); - TrajectorySE2Factory trajectoryFactory = new TrajectorySE2Factory(constraints); - PathSE2Factory pathFactory = new PathSE2Factory(); - TrajectorySE2Planner planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); - - Function trajectoryFn = (p) -> { - List waypoints = List.of( - new WaypointSE2( - p, - new DirectionSE2(1, 0, 0), - 1), - new WaypointSE2( - new Pose2d(), - new DirectionSE2(1, 0, 0), - 1)); - return planner.restToRest(waypoints); - }; + log = parent.name("RightTrenchLeave"); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + trajectoryFactory = new TrajectorySE2Factory(constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Right Trench Leave"; + } + + TrajectorySE2 trajectory(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2( + startingPose, + new DirectionSE2(1, 0, 0), + 1), + new WaypointSE2( + new Pose2d(), + new DirectionSE2(1, 0, 0), + 1)); + return planner.restToRest(waypoints); + } + @Override + public Command command() { DriveWithTrajectoryFunction navigator = new DriveWithTrajectoryFunction( log, machinery.m_drive, controller, machinery.m_trajectoryViz, - trajectoryFn); - - return navigator - .until(navigator::isDone); - + this::trajectory); + return navigator.until(navigator::isDone); } + @Override + public Pose2d start() { + return StartingPositions.RIGHT_TRENCH; + } } diff --git a/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java b/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java index 8d4fd9a..3da7cb7 100644 --- a/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java +++ b/lib/src/main/java/org/team100/frc2025/robot/AllAutons2025.java @@ -3,7 +3,7 @@ import java.util.List; import org.team100.frc2025.field.FieldConstants2025.ReefPoint; -import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.config.AnnotatedCommandImpl; import org.team100.lib.config.AutonChooser; import org.team100.lib.config.ElevatorUtil.ScoringLevel; import org.team100.lib.controller.se2.ControllerFactorySE2; @@ -47,29 +47,37 @@ public AllAutons2025(Machinery2025 machinery) { // WARNING! The glass widget will override the default, so check it! // Run the auto in pre-match testing! - m_autonChooser.addAsDefault("Lollipop", - new AnnotatedCommand( - new LolipopAuto(autoLog, machinery, profile, controller, planner).get(), null, null)); + m_autonChooser.addAsDefault(new AnnotatedCommandImpl( + "Lollipop", + new LolipopAuto(autoLog, machinery, profile, controller, planner).get())); DriveAndScore driveAndScore = new DriveAndScore(autoLog, machinery, profile, controller); - m_autonChooser.add("Coral 1 left", - new AnnotatedCommand(driveAndScore.get(ScoringLevel.L4, ReefPoint.J), null, null)); - m_autonChooser.add("Coral 1 mid", - new AnnotatedCommand(driveAndScore.get(ScoringLevel.L4, ReefPoint.H), null, null)); - m_autonChooser.add("Coral 1 right", - new AnnotatedCommand(driveAndScore.get(ScoringLevel.L4, ReefPoint.F), null, null)); + m_autonChooser.add(new AnnotatedCommandImpl( + "Coral 1 left", + driveAndScore.get(ScoringLevel.L4, ReefPoint.J))); + m_autonChooser.add(new AnnotatedCommandImpl( + "Coral 1 mid", + driveAndScore.get(ScoringLevel.L4, ReefPoint.H))); + m_autonChooser.add(new AnnotatedCommandImpl( + "Coral 1 right", + driveAndScore.get(ScoringLevel.L4, ReefPoint.F))); Auton2025 auton = new Auton2025(autoLog, machinery, profile, controller); - m_autonChooser.add("Left Preload Only", new AnnotatedCommand( - auton.leftPreloadOnly(), null, null)); - m_autonChooser.add("Center Preload Only", new AnnotatedCommand( - auton.centerPreloadOnly(), null, null)); - m_autonChooser.add("Right Preload Only", new AnnotatedCommand( - auton.rightPreloadOnly(), null, null)); - m_autonChooser.add("Left Three Coral", new AnnotatedCommand( - auton.left(), null, null)); - m_autonChooser.add("Right Three Coral", new AnnotatedCommand( - auton.right(), null, null)); + m_autonChooser.add(new AnnotatedCommandImpl( + "Left Preload Only", + auton.leftPreloadOnly())); + m_autonChooser.add(new AnnotatedCommandImpl( + "Center Preload Only", + auton.centerPreloadOnly())); + m_autonChooser.add(new AnnotatedCommandImpl( + "Right Preload Only", + auton.rightPreloadOnly())); + m_autonChooser.add(new AnnotatedCommandImpl( + "Left Three Coral", + auton.left())); + m_autonChooser.add(new AnnotatedCommandImpl( + "Right Three Coral", + auton.right())); } public Command get() { diff --git a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java index 9204ab7..8c536ed 100644 --- a/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java +++ b/lib/src/main/java/org/team100/lib/config/AnnotatedCommand.java @@ -11,19 +11,24 @@ public interface AnnotatedCommand { /** - * command to run + * Must be unique. + */ + String name(); + + /** + * Command to run */ Command command(); /** - * red or blue, null if it works for both. + * Red or blue, null if it works for both. */ default Alliance alliance() { return null; } /** - * starting pose, null if it doesn't matter. + * Starting pose, null if it doesn't matter. */ default Pose2d start() { return null; diff --git a/lib/src/main/java/org/team100/lib/config/AnnotatedCommandImpl.java b/lib/src/main/java/org/team100/lib/config/AnnotatedCommandImpl.java new file mode 100644 index 0000000..9710d89 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/config/AnnotatedCommandImpl.java @@ -0,0 +1,48 @@ +package org.team100.lib.config; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; + +/** Legacy implementation */ +public class AnnotatedCommandImpl implements AnnotatedCommand { + private final String name; + private final Command command; + private final Alliance alliance; + private final Pose2d start; + + public AnnotatedCommandImpl( + String name, + Command command, + Alliance alliance, + Pose2d start) { + this.name = name; + this.command = command; + this.alliance = alliance; + this.start = start; + } + + public AnnotatedCommandImpl(String name, Command command) { + this(name, command, null, null); + } + + @Override + public String name() { + return name; + } + + @Override + public Command command() { + return command; + } + + @Override + public Alliance alliance() { + return alliance; + } + + @Override + public Pose2d start() { + return start; + } +} diff --git a/lib/src/main/java/org/team100/lib/config/AutonChooser.java b/lib/src/main/java/org/team100/lib/config/AutonChooser.java index 67069f1..c5e2ac9 100644 --- a/lib/src/main/java/org/team100/lib/config/AutonChooser.java +++ b/lib/src/main/java/org/team100/lib/config/AutonChooser.java @@ -1,25 +1,31 @@ package org.team100.lib.config; +import java.util.HashSet; +import java.util.Set; + import org.team100.lib.util.NamedChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class AutonChooser { - private final NamedChooser m_chooser = new NamedChooser<>("Auton Command"); + private final NamedChooser m_chooser; + /** duplicate names are not allowed. */ + private final Set m_names; public AutonChooser() { - m_chooser.setDefaultOption("NONE", new AnnotatedCommand( - null, null, null)); + m_chooser = new NamedChooser<>("Auton Command"); + m_names = new HashSet<>(); + addAsDefault(new AnnotatedCommandImpl("NONE", null, null, null)); SmartDashboard.putData(m_chooser); } - public void addAsDefault(String name, AnnotatedCommand cmd) { - m_chooser.setDefaultOption(name, cmd); + public void addAsDefault(AnnotatedCommand cmd) { + m_chooser.setDefaultOption(unique(cmd.name()), cmd); } - public void add(String name, AnnotatedCommand command) { - m_chooser.addOption(name, command); + public void add(AnnotatedCommand cmd) { + m_chooser.addOption(unique(cmd.name()), cmd); } public AnnotatedCommand get() { @@ -29,4 +35,10 @@ public AnnotatedCommand get() { public void close() { m_chooser.close(); } + + private String unique(String name) { + if (!m_names.add(name)) + throw new IllegalArgumentException("Duplicate name " + name); + return name; + } } From beacee1ff7864ed457fac15a7b985c101bcac00e Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 12:00:34 -0800 Subject: [PATCH 4/6] WIP --- comp/src/main/java/org/team100/frc2026/auton/DoNothing.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java index 651d289..f1d38d4 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java +++ b/comp/src/main/java/org/team100/frc2026/auton/DoNothing.java @@ -9,6 +9,11 @@ /** An auton that does nothing at all. */ public class DoNothing implements AnnotatedCommand { + @Override + public String name() { + return "Do Nothing"; + } + @Override public Command command() { return Commands.idle().withName("Nothing from right bump"); @@ -18,5 +23,4 @@ public Command command() { public Pose2d start() { return StartingPositions.RIGHT_BUMP; } - } From 44c42cf7dbc9ec73b32747d1e7a2b165e682857e Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 14:05:56 -0800 Subject: [PATCH 5/6] fix simulation auton pose resetting --- comp/src/main/java/org/team100/frc2026/Robot.java | 3 +-- .../team100/frc2026/auton/RightTrenchLeave.java | 14 ++++++++++---- .../java/org/team100/frc2026/robot/Machinery.java | 13 +++++++++++++ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/Robot.java b/comp/src/main/java/org/team100/frc2026/Robot.java index c31a550..6175d2f 100644 --- a/comp/src/main/java/org/team100/frc2026/Robot.java +++ b/comp/src/main/java/org/team100/frc2026/Robot.java @@ -13,7 +13,6 @@ import org.team100.lib.indicator.Alerts; import org.team100.lib.indicator.AutonAlerts; import org.team100.lib.logging.RobotLog; -import org.team100.lib.uncertainty.IsotropicNoiseSE2; import org.team100.lib.util.Banner; import edu.wpi.first.networktables.NetworkTableInstance; @@ -66,7 +65,7 @@ public Robot() { m_autonAlerts = new AutonAlerts( m_allAutons::getAnnotated, m_alerts, - (p) -> m_machinery.m_drive.resetPose(p, IsotropicNoiseSE2.high())); + m_machinery::resetPose); Prewarmer.init(m_machinery); } diff --git a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java index fb34832..c35ac5b 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java +++ b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java @@ -18,9 +18,15 @@ import org.team100.lib.trajectory.path.PathSE2Factory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -/** Drives a short distance from the right trench starting point */ +/** + * Drives a short distance from the right trench starting point. + * + * There are no points for leaving the starting line, as there sometimes has + * been in other years. This is just a demo. + */ public class RightTrenchLeave implements AnnotatedCommand { private final LoggerFactory log; private final ControllerSE2 controller; @@ -52,11 +58,11 @@ TrajectorySE2 trajectory(Pose2d startingPose) { List waypoints = List.of( new WaypointSE2( startingPose, - new DirectionSE2(1, 0, 0), + new DirectionSE2(-1, 0, 0), 1), new WaypointSE2( - new Pose2d(), - new DirectionSE2(1, 0, 0), + new Pose2d(2, 4, Rotation2d.kZero), + new DirectionSE2(0, 1, 0), 1)); return planner.restToRest(waypoints); } diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 35624ba..081029c 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -1,6 +1,7 @@ package org.team100.frc2026.robot; import java.io.IOException; +import java.util.function.Consumer; import java.util.function.UnaryOperator; import org.team100.frc2026.Climber; @@ -62,6 +63,7 @@ public class Machinery { private final Runnable m_groundTruthViz; private final SwerveModuleCollection m_modules; private final Runnable m_simulatedTagDetector; + private final Consumer m_groundTruthResetter; public final TrajectoryVisualization m_trajectoryViz; public final SwerveKinodynamics m_swerveKinodynamics; @@ -189,6 +191,8 @@ public Machinery() { }; m_simulatedTagDetector = () -> { }; + m_groundTruthResetter = (p) -> { + }; } else { // This is all for simulation only. final LoggerFactory simLog = logger.name("Simulation"); @@ -215,6 +219,7 @@ public Machinery() { simLog, m_swerveKinodynamics, groundTruthGyro, groundTruthHistory, m_modules::positions, UnaryOperator.identity()); + m_groundTruthResetter = (p) -> groundTruthUpdater.reset(p, IsotropicNoiseSE2.high()); GroundTruthCache groundTruthCache = new GroundTruthCache( groundTruthUpdater, groundTruthHistory); @@ -240,6 +245,14 @@ public Machinery() { m_beeper = new Beeper(m_drive); } + /** + * Purge the history and assert the given pose as the current estimate. + */ + public void resetPose(Pose2d p) { + m_drive.resetPose(p, IsotropicNoiseSE2.high()); + m_groundTruthResetter.accept(p); + } + public void periodic() { // publish the simulated tag sightings. m_simulatedTagDetector.run(); From 8a113b62e21f711aced9d4a8e800f677883618c8 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 13 Feb 2026 15:01:36 -0800 Subject: [PATCH 6/6] add sequence example auton --- .../org/team100/frc2026/auton/AllAutons.java | 5 + .../frc2026/auton/LeftSequenceExample.java | 109 ++++++++++++++++++ .../frc2026/auton/RightTrenchLeave.java | 3 +- 3 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java diff --git a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java index 66399fd..7977b2a 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -30,6 +30,11 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) { machinery.m_swerveKinodynamics, controller, machinery)); + m_autonChooser.add(new LeftSequenceExample( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() { diff --git a/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java b/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java new file mode 100644 index 0000000..af361ab --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java @@ -0,0 +1,109 @@ +package org.team100.frc2026.auton; + +import static edu.wpi.first.wpilibj2.command.Commands.sequence; +import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; + +import java.util.List; + +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.trajectory.TrajectorySE2; +import org.team100.lib.trajectory.TrajectorySE2Factory; +import org.team100.lib.trajectory.TrajectorySE2Planner; +import org.team100.lib.trajectory.constraint.TimingConstraint; +import org.team100.lib.trajectory.constraint.TimingConstraintFactory; +import org.team100.lib.trajectory.path.PathSE2Factory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example of a simple sequence */ +public class LeftSequenceExample implements AnnotatedCommand { + private final LoggerFactory log; + private final ControllerSE2 controller; + private final Machinery machinery; + private final List constraints; + private final TrajectorySE2Factory trajectoryFactory; + private final PathSE2Factory pathFactory; + private final TrajectorySE2Planner planner; + + public LeftSequenceExample( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name("RightTrenchLeave"); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + trajectoryFactory = new TrajectorySE2Factory(constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Left Sequence Example"; + } + + // go to the middle while spinning 90 degrees + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2( + startingPose, + new DirectionSE2(-1, 0, 0), + 1), + new WaypointSE2( + new Pose2d(2, 4, Rotation2d.kCW_Pi_2), + new DirectionSE2(0, -1, 0), + 1)); + return planner.restToRest(waypoints); + } + + // go back where we started. + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2( + startingPose, + new DirectionSE2(0, 1, 0), + 1), + new WaypointSE2( + StartingPositions.LEFT_TRENCH, + new DirectionSE2(1, 0, 0), + 1)); + return planner.restToRest(waypoints); + } + + @Override + public Command command() { + DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction( + log, + machinery.m_drive, + controller, + machinery.m_trajectoryViz, + this::t1); + DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction( + log, + machinery.m_drive, + controller, + machinery.m_trajectoryViz, + this::t2); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + n2.until(n2::isDone)); + } + + @Override + public Pose2d start() { + return StartingPositions.LEFT_TRENCH; + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java index c35ac5b..f16e777 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java +++ b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java @@ -36,7 +36,8 @@ public class RightTrenchLeave implements AnnotatedCommand { private final PathSE2Factory pathFactory; private final TrajectorySE2Planner planner; - public RightTrenchLeave(LoggerFactory parent, + public RightTrenchLeave( + LoggerFactory parent, SwerveKinodynamics kinodynamics, ControllerSE2 controller, Machinery machinery) {