Groundwork for april tags done

src/main/java/frc/lib/utils/Alert.java

@@ -0,0 +1,147 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.lib.utils;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.function.Predicate;
+
+/** Class for managing persistent alerts to be sent over NetworkTables. */
+public class Alert {
+  private static Map<String, SendableAlerts> groups = new HashMap<String, SendableAlerts>();
+
+  private final AlertType type;
+  private boolean active = false;
+  private double activeStartTime = 0.0;
+  private String text;
+
+  /**
+   * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated,
+   * the appropriate entries will be added to NetworkTables.
+   *
+   * @param text Text to be displayed when the alert is active.
+   * @param type Alert level specifying urgency.
+   */
+  public Alert(String text, AlertType type) {
+    this("Alerts", text, type);
+  }
+
+  /**
+   * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate
+   * entries will be added to NetworkTables.
+   *
+   * @param group Group identifier, also used as NetworkTables title
+   * @param text Text to be displayed when the alert is active.
+   * @param type Alert level specifying urgency.
+   */
+  public Alert(String group, String text, AlertType type) {
+    if (!groups.containsKey(group)) {
+      groups.put(group, new SendableAlerts());
+      SmartDashboard.putData(group, groups.get(group));
+    }
+
+    this.text = text;
+    this.type = type;
+    groups.get(group).alerts.add(this);
+  }
+
+  /**
+   * Sets whether the alert should currently be displayed. When activated, the alert text will also
+   * be sent to the console.
+   */
+  public void set(boolean active) {
+    if (active && !this.active) {
+      activeStartTime = Timer.getFPGATimestamp();
+      switch (type) {
+        case ERROR:
+          DriverStation.reportError(text, false);
+          break;
+        case WARNING:
+          DriverStation.reportWarning(text, false);
+          break;
+        case INFO:
+          System.out.println(text);
+          break;
+      }
+    }
+    this.active = active;
+  }
+
+  /** Updates current alert text. */
+  public void setText(String text) {
+    if (active && !text.equals(this.text)) {
+      switch (type) {
+        case ERROR:
+          DriverStation.reportError(text, false);
+          break;
+        case WARNING:
+          DriverStation.reportWarning(text, false);
+          break;
+        case INFO:
+          System.out.println(text);
+          break;
+      }
+    }
+    this.text = text;
+  }
+
+  private static class SendableAlerts implements Sendable {
+    public final List<Alert> alerts = new ArrayList<>();
+
+    public String[] getStrings(AlertType type) {
+      Predicate<Alert> activeFilter = (Alert x) -> x.type == type && x.active;
+      Comparator<Alert> timeSorter =
+          (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
+      return alerts.stream()
+          .filter(activeFilter)
+          .sorted(timeSorter)
+          .map((Alert a) -> a.text)
+          .toArray(String[]::new);
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+      builder.setSmartDashboardType("Alerts");
+      builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
+      builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
+      builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
+    }
+  }
+
+  /** Represents an alert's level of urgency. */
+  public static enum AlertType {
+    /**
+     * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type
+     * for problems which will seriously affect the robot's functionality and thus require immediate
+     * attention.
+     */
+    ERROR,
+
+    /**
+     * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this
+     * type for problems which could affect the robot's functionality but do not necessarily require
+     * immediate attention.
+     */
+    WARNING,
+
+    /**
+     * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type
+     * for problems which are unlikely to affect the robot's functionality, or any other alerts
+     * which do not fall under "ERROR" or "WARNING".
+     */
+    INFO
+  }
+}

src/main/java/frc/lib/utils/AllianceFlipUtil.java

@@ -0,0 +1,72 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.lib.utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Constants.FieldConstants;
+
+/** Utility functions for flipping from the blue to red alliance. */
+public class AllianceFlipUtil {
+  /** Flips an x coordinate to the correct side of the field based on the current alliance color. */
+  public static double apply(double xCoordinate) {
+    if (shouldFlip()) {
+      return FieldConstants.kFieldLength - xCoordinate;
+    } else {
+      return xCoordinate;
+    }
+  }
+
+  /** Flips a translation to the correct side of the field based on the current alliance color. */
+  public static Translation2d apply(Translation2d translation) {
+    if (shouldFlip()) {
+      return new Translation2d(apply(translation.getX()), translation.getY());
+    } else {
+      return translation;
+    }
+  }
+
+  /** Flips a rotation based on the current alliance color. */
+  public static Rotation2d apply(Rotation2d rotation) {
+    if (shouldFlip()) {
+      return new Rotation2d(-rotation.getCos(), rotation.getSin());
+    } else {
+      return rotation;
+    }
+  }
+
+  /** Flips a pose to the correct side of the field based on the current alliance color. */
+  public static Pose2d apply(Pose2d pose) {
+    if (shouldFlip()) {
+      return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation()));
+    } else {
+      return pose;
+    }
+  }
+
+  public static Translation3d apply(Translation3d translation3d) {
+    if (shouldFlip()) {
+      return new Translation3d(
+          apply(translation3d.getX()), translation3d.getY(), translation3d.getZ());
+    } else {
+      return translation3d;
+    }
+  }
+
+  /**
+   * Flips a trajectory state to the correct side of the field based on the current alliance color.
+   */
+  public static boolean shouldFlip() {
+    return DriverStation.getAlliance().isPresent()
+        && DriverStation.getAlliance().get() == Alliance.Red;
+  }
+}

src/main/java/frc/lib/utils/GeomUtil.java

@@ -0,0 +1,151 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.lib.utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
+public class GeomUtil {
+  /**
+   * Creates a pure translating transform
+   *
+   * @param translation The translation to create the transform with
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Translation2d translation) {
+    return new Transform2d(translation, new Rotation2d());
+  }
+
+  /**
+   * Creates a pure rotating transform
+   *
+   * @param rotation The rotation to create the transform with
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Rotation2d rotation) {
+    return new Transform2d(new Translation2d(), rotation);
+  }
+
+  /**
+   * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+   *
+   * @param pose The pose that will represent the transform
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Pose2d pose) {
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  public static Pose2d inverse(Pose2d pose) {
+    Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+    return new Pose2d(
+        pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+  }
+
+  /**
+   * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Transform2d transform) {
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Creates a pure translated pose
+   *
+   * @param translation The translation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Translation2d translation) {
+    return new Pose2d(translation, new Rotation2d());
+  }
+
+  /**
+   * Creates a pure rotated pose
+   *
+   * @param rotation The rotation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Rotation2d rotation) {
+    return new Pose2d(new Translation2d(), rotation);
+  }
+
+  /**
+   * Multiplies a twist by a scaling factor
+   *
+   * @param twist The twist to multiply
+   * @param factor The scaling factor for the twist components
+   * @return The new twist
+   */
+  public static Twist2d multiply(Twist2d twist, double factor) {
+    return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+  }
+
+  /**
+   * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+   *
+   * @param pose The pose that will represent the transform
+   * @return The resulting transform
+   */
+  public static Transform3d toTransform3d(Pose3d pose) {
+    return new Transform3d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose3d toPose3d(Transform3d transform) {
+    return new Pose3d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+   *
+   * @param speeds The original translation
+   * @return The resulting translation
+   */
+  public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+    return new Twist2d(
+        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Creates a new pose from an existing one using a different translation value.
+   *
+   * @param pose The original pose
+   * @param translation The new translation to use
+   * @return The new pose with the new translation and original rotation
+   */
+  public static Pose2d withTranslation(Pose2d pose, Translation2d translation) {
+    return new Pose2d(translation, pose.getRotation());
+  }
+
+  /**
+   * Creates a new pose from an existing one using a different rotation value.
+   *
+   * @param pose The original pose
+   * @param rotation The new rotation to use
+   * @return The new pose with the original translation and new rotation
+   */
+  public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) {
+    return new Pose2d(pose.getTranslation(), rotation);
+  }
+}