Started work on autons

comp/simgui-ds.json

@@ -4,9 +4,9 @@
       "visible": false
     }
   },
-  "Joysticks": {
+  "Keyboard 0 Settings": {
     "window": {
-      "visible": false
+      "visible": true
     }
   },
   "System Joysticks": {
@@ -18,22 +18,22 @@
     {
       "axisConfig": [
         {
-          "decKey": 69,
-          "incKey": 82
+          "decKey": 81,
+          "incKey": 69
         },
         {},
         {
           "decayRate": 0.0,
           "keyRate": 0.009999999776482582
         },
         {},
-        {
-          "decKey": 65,
-          "incKey": 68
-        },
         {
           "decKey": 87,
           "incKey": 83
+        },
+        {
+          "decKey": 68,
+          "incKey": 65
         }
       ],
       "axisCount": 6,

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");
     }
 
 }

comp/src/main/java/org/team100/frc2026/auton/AllAutons.java

@@ -35,6 +35,16 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) {
                 machinery.m_swerveKinodynamics,
                 controller,
                 machinery));
+        m_autonChooser.add(new AutonTest(
+                log,
+                machinery.m_swerveKinodynamics,
+                controller,
+                machinery));
+        m_autonChooser.add(new Auton1(
+                log,
+                machinery.m_swerveKinodynamics,
+                controller,
+                machinery));
     }
 
     public Command get() {

comp/src/main/java/org/team100/frc2026/auton/Auton1.java

@@ -0,0 +1,139 @@
+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;
+
+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;
+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<TimingConstraint> 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;
+        List<TimingConstraint> 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);
+        // constraints.add(slow_bump_zone);
+        trajectoryFactory = new TrajectorySE2Factory(new_constraints);
+        pathFactory = new PathSE2Factory();
+        planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory);
+    }
+
+    @Override
+    public String name() {
+        return "Auton 1";
+    }
+
+    TrajectorySE2 t1(Pose2d startingPose) {
+        List<WaypointSE2> 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<WaypointSE2> 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<WaypointSE2> 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.SHOOT_LEFT,
+                        new DirectionSE2(-1, -1, 0), 1));
+        return planner.restToRest(waypoints);
+    }
+
+    TrajectorySE2 t4(Pose2d startingPose) {
+        List<WaypointSE2> 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);
+    }
+
+    @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);
+        DriveWithTrajectoryFunction n4 = new DriveWithTrajectoryFunction(
+                log, machinery.m_drive, controller,
+                machinery.m_trajectoryViz, this::t4);
+        return sequence(
+                n1.until(n1::isDone),
+                waitSeconds(1),
+                parallel(
+                    n2,
+                   machinery.m_intake.intake()
+                ).until(n2::isDone),
+                waitSeconds(1),
+                n3.until(n3::isDone),
+                waitSeconds(1),
+                n4.until(n4::isDone));
+    }
+
+    @Override
+    public Pose2d start() {
+        return StartingPositions.LEFT_BUMP;
+    }
+
+}

comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java

@@ -0,0 +1,19 @@
+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.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)));
+
+    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)));
+}

comp/src/main/java/org/team100/frc2026/auton/AutonTest.java

@@ -0,0 +1,110 @@
+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<TimingConstraint> 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<WaypointSE2> 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<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(startingPose,
+                        new DirectionSE2(1, 1, 0), 1),
+                new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD,
+                        new DirectionSE2(1, 1, 0), 1),
+                new WaypointSE2(AutonPositions.BELOW_BALL_FIELD,
+                        new DirectionSE2(0, -1, 0), 1));
+        return planner.restToRest(waypoints);
+    }
+
+  // go back where we started.
+    TrajectorySE2 t3(Pose2d startingPose) {
+        List<WaypointSE2> waypoints = List.of(      
+                new WaypointSE2(startingPose,
+                        new DirectionSE2(0, 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;
+    }
+
+}

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)
+        );
+
+}

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;