From b0718d6198a7bf789430c97ab51dbd5cb935af37 Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Sat, 14 Feb 2026 14:30:24 -0800 Subject: [PATCH 1/6] Added a "test" auton --- comp/simgui-ds.json | 16 +-- .../org/team100/frc2026/auton/AllAutons.java | 5 + .../team100/frc2026/auton/AutonPositions.java | 13 +++ .../org/team100/frc2026/auton/AutonTest.java | 109 ++++++++++++++++++ 4 files changed, 135 insertions(+), 8 deletions(-) create mode 100644 comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/AutonTest.java diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index a2e3cce..5729c19 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -4,9 +4,9 @@ "visible": false } }, - "Joysticks": { + "Keyboard 0 Settings": { "window": { - "visible": false + "visible": true } }, "System Joysticks": { @@ -18,8 +18,8 @@ { "axisConfig": [ { - "decKey": 69, - "incKey": 82 + "decKey": 81, + "incKey": 69 }, {}, { @@ -27,13 +27,13 @@ "keyRate": 0.009999999776482582 }, {}, - { - "decKey": 65, - "incKey": 68 - }, { "decKey": 87, "incKey": 83 + }, + { + "decKey": 68, + "incKey": 65 } ], "axisCount": 6, 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 7977b2a..e9c51fa 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -35,6 +35,11 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) { machinery.m_swerveKinodynamics, controller, machinery)); + m_autonChooser.add(new AutonTest( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() { diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java new file mode 100644 index 0000000..859c73c --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java @@ -0,0 +1,13 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * Areas that might be useful for autons + */ +public class AutonPositions { + public static final Pose2d ABOVE_BALL_FIELD = new Pose2d(7.5, 7, Rotation2d.kCW_90deg); + + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java new file mode 100644 index 0000000..670c455 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.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 AutonTest 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 AutonTest( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name(name()); + 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 "Auton Test"; + } + + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, 0, 0), 1), + new WaypointSE2(new Pose2d(2, 4, Rotation2d.kZero), + new DirectionSE2(-1, 1, 0), 1)); + return planner.restToRest(waypoints); + } + + + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(1, 1, 0), 1), + new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD, + new DirectionSE2(1, 0, 0), 1)); + return planner.restToRest(waypoints); + } + + // go back where we started. + TrajectorySE2 t3(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-0.4, -1, 0), 1), + new WaypointSE2(StartingPositions.RIGHT_BUMP, + 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); + DriveWithTrajectoryFunction n3 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t3); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + n2.until(n2::isDone), + waitSeconds(1), + n3.until(n3::isDone)); + } + + @Override + public Pose2d start() { + return StartingPositions.RIGHT_BUMP; + } + +} From 751c6d1f35296d9d48d6b52f5286836557227b03 Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Sun, 15 Feb 2026 13:56:03 -0800 Subject: [PATCH 2/6] Added a new auton --- .../org/team100/frc2026/auton/AllAutons.java | 5 + .../org/team100/frc2026/auton/Auton1.java | 117 ++++++++++++++++++ .../team100/frc2026/auton/AutonPositions.java | 6 +- .../org/team100/frc2026/auton/AutonTest.java | 7 +- .../org/team100/frc2026/auton/BumpZones.java | 31 +++++ 5 files changed, 162 insertions(+), 4 deletions(-) create mode 100644 comp/src/main/java/org/team100/frc2026/auton/Auton1.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/BumpZones.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 e9c51fa..381442b 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -40,6 +40,11 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) { machinery.m_swerveKinodynamics, controller, machinery)); + m_autonChooser.add(new Auton1( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() { diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java new file mode 100644 index 0000000..c137c0e --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -0,0 +1,117 @@ +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.auton.BumpZones; +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.constraint.VelocityLimitRegionConstraint; +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 Auton1 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 Auton1( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name(name()); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + // In meters/second + double maxBumpVelocity = 1; + // create a new VelocityRegionContstraint `slow_bump_zone` + VelocityLimitRegionConstraint slow_bump_zone = new VelocityLimitRegionConstraint(log, BumpZones.BLUE_BUMP_LEFT, maxBumpVelocity); + // TODO: can't modify `constraints` by adding `slow_bump_zone`, but it doesn' crash + // constraints.add(slow_bump_zone); + trajectoryFactory = new TrajectorySE2Factory(constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Auton 1"; + } + + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD, + new DirectionSE2(1, 1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(0, -1, 0), 1), + new WaypointSE2(AutonPositions.MIDDLE_BALL_FIELD, + new DirectionSE2(0, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t3(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, 1, 0), 1), + new WaypointSE2(StartingPositions.LEFT_BUMP, + new DirectionSE2(-1, 0, 0), 1), + new WaypointSE2(AutonPositions.CLIMB_LEFT, + new DirectionSE2(-1, -1, 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); + DriveWithTrajectoryFunction n3 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t3); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + n2.until(n2::isDone), + waitSeconds(1), + n3.until(n3::isDone)); + } + + @Override + public Pose2d start() { + return StartingPositions.LEFT_BUMP; + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java index 859c73c..6e8a37b 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java @@ -7,7 +7,11 @@ * Areas that might be useful for autons */ public class AutonPositions { - public static final Pose2d ABOVE_BALL_FIELD = new Pose2d(7.5, 7, Rotation2d.kCW_90deg); + public static final Pose2d ABOVE_BALL_FIELD = new Pose2d(7.75, 7, Rotation2d.kCW_90deg); + public static final Pose2d MIDDLE_BALL_FIELD = new Pose2d(7.75, 4, Rotation2d.kCW_90deg); + public static final Pose2d BELOW_BALL_FIELD = new Pose2d(7.75, 1, Rotation2d.kCW_90deg); + public static final Pose2d CLIMB_LEFT = new Pose2d(1.175, 4.25, new Rotation2d(-135 * (Math.PI / 180))); + public static final Pose2d CLIMB_RIGHT = new Pose2d(1.175, 3.1, new Rotation2d(135 * (Math.PI / 180))); } diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java index 670c455..57f1229 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java @@ -62,13 +62,14 @@ TrajectorySE2 t1(Pose2d startingPose) { return planner.restToRest(waypoints); } - TrajectorySE2 t2(Pose2d startingPose) { List waypoints = List.of( new WaypointSE2(startingPose, new DirectionSE2(1, 1, 0), 1), new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD, - new DirectionSE2(1, 0, 0), 1)); + new DirectionSE2(1, 1, 0), 1), + new WaypointSE2(AutonPositions.BELOW_BALL_FIELD, + new DirectionSE2(0, -1, 0), 1)); return planner.restToRest(waypoints); } @@ -76,7 +77,7 @@ TrajectorySE2 t2(Pose2d startingPose) { TrajectorySE2 t3(Pose2d startingPose) { List waypoints = List.of( new WaypointSE2(startingPose, - new DirectionSE2(-0.4, -1, 0), 1), + new DirectionSE2(0, 1, 0), 1), new WaypointSE2(StartingPositions.RIGHT_BUMP, new DirectionSE2(-1, 0, 0), 1)); return planner.restToRest(waypoints); diff --git a/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java b/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java new file mode 100644 index 0000000..5646319 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java @@ -0,0 +1,31 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class BumpZones { + public static final Rectangle2d BLUE_BUMP_LEFT = + new Rectangle2d( + new Translation2d(5.2, 6.43), + new Translation2d(4.1, 4.6) + ); + + public static final Rectangle2d BLUE_BUMP_RIGHT = + new Rectangle2d( + new Translation2d(5.2, 3.2), + new Translation2d(4.1, 1.64) + ); + + public static final Rectangle2d RED_BUMP_RIGHT = + new Rectangle2d( + new Translation2d(12.5, 6.43), + new Translation2d(11.32, 4.6) + ); + + public static final Rectangle2d RED_BUMP_LEFT = + new Rectangle2d( + new Translation2d(12.5, 3.2), + new Translation2d(11.32, 1.64) + ); + +} From daabbaca320bf6fd24c61f216471abdd6e01ca9a Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Sun, 15 Feb 2026 14:04:15 -0800 Subject: [PATCH 3/6] Added Slow Bump Zone constraint to the auton --- comp/src/main/java/org/team100/frc2026/auton/Auton1.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java index c137c0e..44491c0 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -4,6 +4,7 @@ import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; import java.util.List; +import java.util.ArrayList; import org.team100.frc2026.auton.BumpZones; import org.team100.frc2026.robot.Machinery; @@ -47,11 +48,14 @@ public Auton1( constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); // In meters/second double maxBumpVelocity = 1; - // create a new VelocityRegionContstraint `slow_bump_zone` + List new_constraints = new ArrayList<>(constraints); + + // create a new VelocityRegionContstraint `slow_bu mp_zone` VelocityLimitRegionConstraint slow_bump_zone = new VelocityLimitRegionConstraint(log, BumpZones.BLUE_BUMP_LEFT, maxBumpVelocity); + new_constraints.add(slow_bump_zone); // TODO: can't modify `constraints` by adding `slow_bump_zone`, but it doesn' crash // constraints.add(slow_bump_zone); - trajectoryFactory = new TrajectorySE2Factory(constraints); + trajectoryFactory = new TrajectorySE2Factory(new_constraints); pathFactory = new PathSE2Factory(); planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); } From 548a7651040df4bfedc16444ff776fa15e1849b8 Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Sun, 15 Feb 2026 15:00:18 -0800 Subject: [PATCH 4/6] Added a stop to score within auton 1 --- .../java/org/team100/frc2026/auton/Auton1.java | 18 ++++++++++++++++-- .../team100/frc2026/auton/AutonPositions.java | 2 ++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java index 44491c0..d502710 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -6,6 +6,7 @@ import java.util.List; import java.util.ArrayList; +import org.team100.frc2026.Intake; import org.team100.frc2026.auton.BumpZones; import org.team100.frc2026.robot.Machinery; import org.team100.lib.config.AnnotatedCommand; @@ -53,7 +54,6 @@ public Auton1( // create a new VelocityRegionContstraint `slow_bu mp_zone` VelocityLimitRegionConstraint slow_bump_zone = new VelocityLimitRegionConstraint(log, BumpZones.BLUE_BUMP_LEFT, maxBumpVelocity); new_constraints.add(slow_bump_zone); - // TODO: can't modify `constraints` by adding `slow_bump_zone`, but it doesn' crash // constraints.add(slow_bump_zone); trajectoryFactory = new TrajectorySE2Factory(new_constraints); pathFactory = new PathSE2Factory(); @@ -89,6 +89,15 @@ TrajectorySE2 t3(Pose2d startingPose) { new DirectionSE2(-1, 1, 0), 1), new WaypointSE2(StartingPositions.LEFT_BUMP, new DirectionSE2(-1, 0, 0), 1), + new WaypointSE2(AutonPositions.SHOOT_LEFT, + new DirectionSE2(-1, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t4(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, -1, 0), 1), new WaypointSE2(AutonPositions.CLIMB_LEFT, new DirectionSE2(-1, -1, 0), 1)); return planner.restToRest(waypoints); @@ -105,12 +114,17 @@ public Command command() { DriveWithTrajectoryFunction n3 = new DriveWithTrajectoryFunction( log, machinery.m_drive, controller, machinery.m_trajectoryViz, this::t3); + DriveWithTrajectoryFunction n4 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t4); return sequence( n1.until(n1::isDone), waitSeconds(1), n2.until(n2::isDone), waitSeconds(1), - n3.until(n3::isDone)); + n3.until(n3::isDone), + waitSeconds(1), + n4.until(n4::isDone)); } @Override diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java index 6e8a37b..1eccdec 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java @@ -14,4 +14,6 @@ public class AutonPositions { public static final Pose2d CLIMB_LEFT = new Pose2d(1.175, 4.25, new Rotation2d(-135 * (Math.PI / 180))); public static final Pose2d CLIMB_RIGHT = new Pose2d(1.175, 3.1, new Rotation2d(135 * (Math.PI / 180))); + public static final Pose2d SHOOT_LEFT = new Pose2d(2.3, 5.1, new Rotation2d(-35 * (Math.PI / 180))); + public static final Pose2d SHOOT_RIGHT = new Pose2d(2.3, 2.7, new Rotation2d(35 * (Math.PI / 180))); } From 1bfffa641b46765dc178d9bba60bbfe530b113db Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Mon, 16 Feb 2026 10:24:51 -0800 Subject: [PATCH 5/6] Started integrating intake into auton 1 --- comp/src/main/java/org/team100/frc2026/auton/Auton1.java | 5 ++++- comp/src/main/java/org/team100/frc2026/robot/Machinery.java | 4 ++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java index d502710..c5f5fac 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -1,5 +1,6 @@ package org.team100.frc2026.auton; +import static edu.wpi.first.wpilibj2.command.Commands.parallel; import static edu.wpi.first.wpilibj2.command.Commands.sequence; import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; @@ -120,7 +121,9 @@ public Command command() { return sequence( n1.until(n1::isDone), waitSeconds(1), - n2.until(n2::isDone), + parallel( + n2.until(n2::isDone), + machinery.m_intake.intake()), waitSeconds(1), n3.until(n3::isDone), waitSeconds(1), 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 081029c..4651b0f 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -71,8 +71,8 @@ public class Machinery { public final SwerveDriveSubsystem m_drive; final Beeper m_beeper; final Shooter m_shooter; - final Intake m_intake; - final IntakeExtend m_extender; + public final Intake m_intake; + public final IntakeExtend m_extender; final Serializer m_serializer; final ClimberExtension m_ClimberExtension; final Climber m_Climber; From 4435b3ba0d4238f62f29308ab718bcf2d7bb8cd1 Mon Sep 17 00:00:00 2001 From: Malcolm <824706@seq.org> Date: Mon, 16 Feb 2026 10:55:49 -0800 Subject: [PATCH 6/6] Auton 1 now spins the intake --- comp/src/main/java/org/team100/frc2026/Intake.java | 3 ++- comp/src/main/java/org/team100/frc2026/auton/Auton1.java | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/Intake.java b/comp/src/main/java/org/team100/frc2026/Intake.java index 963b919..864e671 100644 --- a/comp/src/main/java/org/team100/frc2026/Intake.java +++ b/comp/src/main/java/org/team100/frc2026/Intake.java @@ -53,7 +53,7 @@ public void periodic() { public Command intake() { return run(this::fullSpeed).withName("Intake Full Speed"); - } + } public Command stop() { return run(this::stopMotor).withName("Intake Stop"); @@ -69,6 +69,7 @@ private void fullSpeed() { double velocityRad_S = 450; m_motor.setVelocity(velocityRad_S, 0, 0); // m_motor.setDutyCycle(1); + System.out.println(velocityRad_S + "wow"); } } diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java index c5f5fac..4c16771 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -122,8 +122,9 @@ public Command command() { n1.until(n1::isDone), waitSeconds(1), parallel( - n2.until(n2::isDone), - machinery.m_intake.intake()), + n2, + machinery.m_intake.intake() + ).until(n2::isDone), waitSeconds(1), n3.until(n3::isDone), waitSeconds(1),