Update PurpleSwerve to reflect latest changes

build.gradle

@@ -97,7 +97,7 @@ dependencies {
   nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
   simulationRelease wpi.sim.enableRelease()
 
-  implementation 'com.github.lasarobotics:PurpleLib:2024.1.8'
+  implementation 'com.github.lasarobotics:PurpleLib:2024.3.2'
 
   implementation 'org.apache.commons:commons-math3:3.+'
 

src/main/java/frc/robot/Constants.java

@@ -13,15 +13,16 @@
 import org.lasarobotics.led.LEDStrip;
 import org.lasarobotics.utils.PIDConstants;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
 import frc.robot.subsystems.drive.PurplePathPose;
-import frc.robot.subsystems.vision.VisionCamera.Resolution;
-
+import frc.robot.subsystems.vision.AprilTagCamera.Resolution;
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
  * numerical or boolean
@@ -36,41 +37,45 @@
  */
 public final class Constants {
   public static class Field {
-    public static final double FIELD_WIDTH = 8.1026;
-    public static final double FIELD_LENGTH = 16.4846;
+    public static final double FIELD_WIDTH = 8.21;
+    public static final double FIELD_LENGTH = 16.54;
 
     public static final Translation2d CENTER = new Translation2d(FIELD_LENGTH / 2, FIELD_WIDTH / 2);
-
-    public static final PurplePathPose SUBSTATION = new PurplePathPose(new Pose2d(15.72, 6.15, Rotation2d.fromDegrees(0.0)), new Pose2d(0.85, 6.15, Rotation2d.fromDegrees(180.0)), 2.0);
-    public static final PurplePathPose GRID_1 = new PurplePathPose(new Pose2d(1.85, 4.93, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 4.93, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_2 = new PurplePathPose(new Pose2d(1.85, 4.45, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 4.45, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_3 = new PurplePathPose(new Pose2d(1.85, 3.90, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 3.90, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_4 = new PurplePathPose(new Pose2d(1.85, 3.30, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 3.30, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_5 = new PurplePathPose(new Pose2d(1.85, 2.75, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 2.75, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_6 = new PurplePathPose(new Pose2d(1.85, 2.20, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 2.20, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_7 = new PurplePathPose(new Pose2d(1.85, 1.65, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 1.65, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_8 = new PurplePathPose(new Pose2d(1.85, 1.10, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 1.10, Rotation2d.fromDegrees(0.0)), 0.15);
-    public static final PurplePathPose GRID_9 = new PurplePathPose(new Pose2d(1.85, 0.52, Rotation2d.fromDegrees(180.0)), new Pose2d(14.70, 0.52, Rotation2d.fromDegrees(0.0)), 0.15);
+    public static final Pair<Integer,Translation2d> BLUE_SPEAKER = new Pair<Integer,Translation2d>(7, new Translation2d(0.00, 5.55));
+    public static final Pair<Integer,Translation2d> RED_SPEAKER = new Pair<Integer,Translation2d>(4, new Translation2d(15.64, 5.55));
+
+    public static final PurplePathPose AMP = new PurplePathPose(
+      new Pose2d(Units.Meters.of(1.85), Units.Meters.of(7.77), Rotation2d.fromDegrees(-90.0)),
+      new Pose2d(Units.Meters.of(14.66), Units.Meters.of(7.77), Rotation2d.fromDegrees(-90.0)),
+      Units.Meters.of(0.5),
+      true
+    );
+    public static final PurplePathPose SOURCE = new PurplePathPose(
+      new Pose2d(Units.Meters.of(15.48), Units.Meters.of(0.84), Rotation2d.fromDegrees(+120.00)),
+      new Pose2d(Units.Meters.of(1.07), Units.Meters.of(0.82), Rotation2d.fromDegrees(+60.0)),
+      Units.Meters.of(0.5),
+      true
+    );
   }
 
   public static class HID {
     public static final int PRIMARY_CONTROLLER_PORT = 0;
     public static final int SECONDARY_CONTROLLER_PORT = 1;
-    public static final double CONTROLLER_DEADBAND = 0.10;
+    public static final double CONTROLLER_DEADBAND = 0.1;
   }
 
   public static class Drive {
-    public static final PIDConstants DRIVE_TURN_PID = new PIDConstants(30.0, 0.0, 0.3, 0.0);
-    public static final double DRIVE_SLIP_RATIO = 0.08;
-    public static final double DRIVE_TURN_SCALAR = 30.0;
-    public static final double DRIVE_LOOKAHEAD = 3;
+    public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(7.0, 0.0, 0.1, 0.0);
+    public static final double DRIVE_SLIP_RATIO = 0.11;
+    public static final double DRIVE_TURN_SCALAR = 60.0;
+    public static final double DRIVE_LOOKAHEAD = 6;
 
     public static final ControlCentricity DRIVE_CONTROL_CENTRICITY = ControlCentricity.FIELD_CENTRIC;
 
     private static final double DRIVE_THROTTLE_INPUT_CURVE_X[] = { 0.0, 0.100, 0.200, 0.300, 0.400, 0.500, 0.600, 0.700, 0.800, 0.900, 1.000 };
-    private static final double DRIVE_THROTTLE_INPUT_CURVE_Y[] = { 0.0, 0.042, 0.168, 0.378, 0.672, 1.050, 1.512, 2.508, 2.688, 3.402, 4.300 };
+    private static final double DRIVE_THROTTLE_INPUT_CURVE_Y[] = { 0.0, 0.052, 0.207, 0.465, 0.827, 1.293, 1.862, 2.534, 3.310, 4.189, 5.172 };
     private static final double DRIVE_TURN_INPUT_CURVE_X[] = { 0.0, 0.100, 0.200, 0.300, 0.400, 0.500, 0.600, 0.700, 0.800, 0.900, 1.0 };
-    private static final double DRIVE_TURN_INPUT_CURVE_Y[] = { 0.0, 0.008, 0.032, 0.072, 0.128, 0.200, 0.288, 0.392, 0.512, 0.768, 1.0 };
+    private static final double DRIVE_TURN_INPUT_CURVE_Y[] = { 0.0, 0.010, 0.050, 0.100, 0.150, 0.200, 0.250, 0.300, 0.400, 0.600, 1.0 };
 
     private static final SplineInterpolator SPLINE_INTERPOLATOR = new SplineInterpolator();
     public static final PolynomialSplineFunction DRIVE_THROTTLE_INPUT_CURVE = SPLINE_INTERPOLATOR.interpolate(DRIVE_THROTTLE_INPUT_CURVE_X, DRIVE_THROTTLE_INPUT_CURVE_Y);
@@ -93,29 +98,29 @@ public static class DriveHardware {
   }
 
   public static class VisionHardware {
-    public static final String CAMERA_A_NAME = "cameraA";
+    public static final String CAMERA_A_NAME = "Arducam_OV9782_USB_Camera_A";
     public static final Transform3d CAMERA_A_LOCATION = new Transform3d(
-      new Translation3d(0.0, 0.0, 0.5),
-      new Rotation3d(0.0, 0.0, 0.0)
+      new Translation3d(0.381, 0.133, 0.102),
+      new Rotation3d(0.0, Math.toRadians(-20.0), 0.0)
     );
     public static final Resolution CAMERA_A_RESOLUTION = Resolution.RES_1280_720;
     public static final Rotation2d CAMERA_A_FOV = Rotation2d.fromDegrees(79.7);
 
-    public static final String CAMERA_B_NAME = "cameraB";
+    public static final String CAMERA_B_NAME = "Arducam_OV9782_USB_Camera_B";
     public static final Transform3d CAMERA_B_LOCATION = new Transform3d(
-      new Translation3d(0.0, 0.0, 0.5),
-      new Rotation3d(0.0, 0.0, Math.toRadians(+120.0))
+      new Translation3d(0.148, 0.2667, 0.47),
+      new Rotation3d(0.0, Math.toRadians(-25.0), Math.toRadians(+180.0))
     );
     public static final Resolution CAMERA_B_RESOLUTION = Resolution.RES_1280_720;
     public static final Rotation2d CAMERA_B_FOV = Rotation2d.fromDegrees(79.7);
 
-    public static final String CAMERA_C_NAME = "cameraC";
-    public static final Transform3d CAMERA_C_LOCATION = new Transform3d(
-      new Translation3d(0.0, 0.0, 0.5),
-      new Rotation3d(0.0, 0.0, Math.toRadians(-120.0))
+    public static final String CAMERA_OBJECT_NAME = "Arducam_OV9782_USB_Camera_C";
+    public static final Transform3d CAMERA_OBJECT_LOCATION = new Transform3d(
+      new Translation3d(0.3, 0.0, 0.5),
+      new Rotation3d(0, Math.toRadians(+15.0), 0)
     );
-    public static final Resolution CAMERA_C_RESOLUTION = Resolution.RES_1280_720;
-    public static final Rotation2d CAMERA_C_FOV = Rotation2d.fromDegrees(79.7);
+    public static final Resolution CAMERA_OBJECT_RESOLUTION = Resolution.RES_1280_720;
+    public static final Rotation2d CAMERA_OBJECT_FOV = Rotation2d.fromDegrees(79.7);
   }
 
   public static class SmartDashboard {

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 
 import com.revrobotics.REVPhysicsSim;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -14,14 +15,13 @@
 public class RobotContainer {
   private static final DriveSubsystem DRIVE_SUBSYSTEM = new DriveSubsystem(
     DriveSubsystem.initializeHardware(),
-    Constants.Drive.DRIVE_TURN_PID,
+    Constants.Drive.DRIVE_ROTATE_PID,
     Constants.Drive.DRIVE_CONTROL_CENTRICITY,
+    Constants.Drive.DRIVE_THROTTLE_INPUT_CURVE,
+    Constants.Drive.DRIVE_TURN_INPUT_CURVE,
     Constants.Drive.DRIVE_TURN_SCALAR,
     Constants.HID.CONTROLLER_DEADBAND,
-    Constants.Drive.DRIVE_LOOKAHEAD,
-    Constants.Drive.DRIVE_SLIP_RATIO,
-    Constants.Drive.DRIVE_THROTTLE_INPUT_CURVE,
-    Constants.Drive.DRIVE_TURN_INPUT_CURVE
+    Constants.Drive.DRIVE_LOOKAHEAD
   );
 
   private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
@@ -44,19 +44,20 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
+    // Start button - toggle traction control
     PRIMARY_CONTROLLER.start().onTrue(DRIVE_SUBSYSTEM.toggleTractionControlCommand());
-    PRIMARY_CONTROLLER.leftBumper().whileTrue(
-      DRIVE_SUBSYSTEM.aimAtPointCommand(
-        () -> PRIMARY_CONTROLLER.getLeftY(),
-        () -> PRIMARY_CONTROLLER.getLeftX(),
-        Constants.Field.CENTER
+
+    // A button - go to amp
+    PRIMARY_CONTROLLER.a().whileTrue(
+      DRIVE_SUBSYSTEM.goToPoseCommand(
+        Constants.Field.AMP
       )
     );
 
-    PRIMARY_CONTROLLER.rightBumper().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SUBSTATION));
-    PRIMARY_CONTROLLER.a().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.GRID_5));
-    PRIMARY_CONTROLLER.b().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.GRID_8));
-    PRIMARY_CONTROLLER.x().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.GRID_2));
+    // B button - go to source
+    PRIMARY_CONTROLLER.b().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SOURCE));
+
+    PRIMARY_CONTROLLER.povLeft().onTrue(DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
   }
 
   /**

src/main/java/frc/robot/subsystems/drive/AutoTrajectory.java

@@ -4,157 +4,70 @@
 
 package frc.robot.subsystems.drive;
 
-import java.util.HashMap;
 import java.util.List;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.EventMarker;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
-import com.pathplanner.lib.path.PathPoint;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 
 public class AutoTrajectory {
   DriveSubsystem m_driveSubsystem;
   Command m_swerveCommand;
-  PathPlannerPath m_path;
+  Pair<String,List<PathPlannerPath>> m_auto;
 
   /**
    * Create new path trajectory using PathPlanner path
    * @param driveSubsystem DriveSubsystem to drive the robot
-   * @param pathName PathPlanner path name
-   * @param maxVelocity Maximum velocity of robot during path (m/s)
-   * @param maxAcceleration Maximum acceleration of robot during path (m/s^2)
+   * @param autoName PathPlanner auto name
    */
-  public AutoTrajectory(DriveSubsystem driveSubsystem, String pathName) {
+  public AutoTrajectory(DriveSubsystem driveSubsystem, String autoName) {
     this.m_driveSubsystem = driveSubsystem;
 
     // Get path
-    m_path = PathPlannerPath.fromPathFile(pathName);
+    m_auto = new Pair<String, List<PathPlannerPath>>(autoName, PathPlannerAuto.getPathGroupFromAutoFile(autoName));
   }
 
   /**
    * Creates new path trajectory using a physical x,y coordinate points
    * @param driveSubsystem DriveSubsystem required for drivetrain movement
-   * @param waypoints list of x, y coordinate pairs in trajectory
-   * @param reversed whether the trajectory followed should be in reverse
-   * @param maxVelocity Maximum velocity of robot during path (m/s)
-   * @param maxAcceleration Maximum acceleration of robot during path (m/s^2)
+   * @param waypoints List of x, y coordinate pairs in trajectory
+   * @param pathConstraints Path following constraints
    */
-  public AutoTrajectory(DriveSubsystem driveSubsystem, List<PathPoint> waypoints, PathConstraints pathConstraints) {
+  public AutoTrajectory(DriveSubsystem driveSubsystem, List<Pose2d> waypoints, PathConstraints pathConstraints) {
     this.m_driveSubsystem = driveSubsystem;
 
     // Generate path from waypoints
-    m_path = PathPlannerPath.fromPathPoints(
-      waypoints,
+    m_auto = new Pair<String, List<PathPlannerPath>>("", List.of(new PathPlannerPath(
+      PathPlannerPath.bezierFromPoses(waypoints),
       pathConstraints,
-      new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).rotationTarget.getTarget())
-    );
+      new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).getRotation())
+    )));
   }
 
-  /**
-   * Reset drive odometry to beginning of this path
-   */
-  private void resetOdometry() {
-    m_driveSubsystem.resetPose(getInitalPose());
-  }
-
-  /**
-   * Get markers of path
-   * @return A list of markers within the path
-   */
-  public List<EventMarker> getEventMarkers() {
-    return m_path.getEventMarkers();
-  }
-
-  /**
-   * Get initial pose for path
-   * @return Path initial pose
-   */
-  public Pose2d getInitalPose() {
-    if (m_path == null) return new Pose2d();
-    return new Pose2d(m_path.getPoint(0).position, m_path.getPoint(0).rotationTarget.getTarget());
-  }
-
-  /**
-   * Get Ramsete command to run
-   * @return Ramsete command that will stop when complete
-   */
-  public Command getCommandAndStop() {
-    return AutoBuilder.followPath(m_path)
-            .andThen(() -> m_driveSubsystem.resetRotatePID())
-            .andThen(m_driveSubsystem.lockCommand())
-            .andThen(m_driveSubsystem.stopCommand());
-  }
-
-  /**
-   * Get auto command to execute path, resetting odometry first
-   * @param isFirstPath true if path is the first one in autonomous
-   * @return Ramsete command that will stop when complete
-   */
-  public Command getCommandAndStop(boolean isFirstPath) {
-    if (isFirstPath) {
-      return Commands.runOnce(() -> resetOdometry())
-              .andThen(AutoBuilder.followPath(m_path))
-              .andThen(() -> m_driveSubsystem.resetRotatePID())
-              .andThen(m_driveSubsystem.lockCommand())
-              .andThen(m_driveSubsystem.stopCommand());
-    } else return getCommandAndStop();
-  }
-
-  /**
-   * Get auto command to execute path and events along the way
-   * @param eventMap Map of event marker names to the commands that should run when reaching that
-   *     marker. This SHOULD NOT contain any commands requiring the same subsystems as the path
-   *     following command.
-   * @return Command to execute actions in autonomous
-   */
-  public Command getCommandAndStop(HashMap<String, Command> eventMap) {
-    return AutoBuilder.followPath(m_path)
-            .andThen(() -> m_driveSubsystem.resetRotatePID())
-            .andThen(m_driveSubsystem.lockCommand())
-            .andThen(m_driveSubsystem.stopCommand());
-  }
-
-  /**
-   * Get auto command to execute path and events along the way, resetting odometry first
-   * @param isFirstPath true if path is the first one in autonomous
-   * @param eventMap Map of event marker names to the commands that should run when reaching that
-   *     marker. This SHOULD NOT contain any commands requiring the same subsystems as the path
-   *     following command.
-   * @return Command to execute actions in autonomous
-   */
-  public Command getCommandAndStop(boolean isFirstPath, HashMap<String, Command> eventMap) {
-    if (isFirstPath) {
-      return AutoBuilder.followPath(m_path)
-            .andThen(() -> m_driveSubsystem.resetRotatePID())
-            .andThen(m_driveSubsystem.lockCommand())
-            .andThen(m_driveSubsystem.stopCommand());
-    } else return getCommandAndStop(eventMap);
+  /** Return initial pose */
+  public Pose2d getInitialPose() {
+    return m_auto.getSecond().get(0).getPreviewStartingHolonomicPose();
   }
 
   /**
    * Get auto command to execute path
-   * @return Ramsete command that does NOT stop when complete
+   * @return Auto command group that will stop when complete
    */
   public Command getCommand() {
-    return AutoBuilder.followPath(m_path).andThen(() -> m_driveSubsystem.resetRotatePID());
-  }
-
-  /**
-   * Get auto command to execute path, resetting odometry first
-   * @param isFirstPath true if path is first one in autonomous
-   * @return Ramsete command that does NOT stop when complete
-   */
-  public Command getCommand(boolean isFirstPath) {
-    if (isFirstPath) {
-      return Commands.runOnce(() -> resetOdometry())
-             .andThen(AutoBuilder.followPath(m_path))
-             .andThen(() -> m_driveSubsystem.resetRotatePID());
-    } else return getCommand();
+    Command autoCommand = m_auto.getSecond().size() == 1
+      ? AutoBuilder.followPath(m_auto.getSecond().get(0))
+      : new PathPlannerAuto(m_auto.getFirst());
+
+    return m_driveSubsystem.resetPoseCommand(() -> getInitialPose())
+      .andThen(autoCommand)
+      .andThen(() -> m_driveSubsystem.resetRotatePID())
+      .andThen(m_driveSubsystem.stopCommand())
+      .andThen(m_driveSubsystem.lockCommand());
   }
 }