Replace PhotonCamera Wrapper with code from Iron Maple

customAssets/Robot_Pumbaa/config.json

@@ -12,7 +12,77 @@
         0.0,
         0.0
     ],
-    "cameras": [],
+    "cameras": [
+        {
+            "name": "front-left",
+            "position": [
+                0.43179999999999996,
+                0.1016,
+                0.381
+            ],
+            "fov": 68.0,
+            "rotations": [
+                {
+                    "axis": "y",
+                    "degrees": 0.0
+                },
+                {
+                    "axis": "z",
+                    "degrees": 12.0
+                }
+            ],
+            "resolution": [
+                1280,
+                800
+            ]
+        },
+        {
+            "name": "front-right",
+            "position": [
+                0.43179999999999996,
+                -0.127,
+                0.381
+            ],
+            "fov": 68.0,
+            "rotations": [
+                {
+                    "axis": "y",
+                    "degrees": 0.0
+                },
+                {
+                    "axis": "z",
+                    "degrees": -6.000000000000001
+                }
+            ],
+            "resolution": [
+                1600,
+                1200
+            ]
+        },
+        {
+            "name": "back-left",
+            "position": [
+                -0.3302,
+                0.35559999999999997,
+                0.0
+            ],
+            "fov": 68.0,
+            "rotations": [
+                {
+                    "axis": "y",
+                    "degrees": 0.0
+                },
+                {
+                    "axis": "z",
+                    "degrees": -176.00000000000003
+                }
+            ],
+            "resolution": [
+                1600,
+                1200
+            ]
+        }
+    ],
     "components": [
         {
             "zeroedRotations": [

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

@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2023 FRC 6328 "Mechanical Advantage"
+ * https://github.com/Mechanical-Advantage/RobotCode2023/blob/main/src/main/java/org/
+ * littletonrobotics/frc2023/util/Alert.java 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.util;
+
+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;
+import org.littletonrobotics.junction.Logger;
+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;
+
+/**
+ * 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 setActivated(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:
+                    Logger.recordOutput("INFO/", 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 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/util/CustomMaths/Statistics.java

@@ -0,0 +1,59 @@
+package frc.lib.util.CustomMaths;
+
+public final class Statistics {
+    public static double getMean(double[] dataSet) {
+        double sum = 0;
+        for (double data : dataSet)
+            sum += data;
+        return sum / dataSet.length;
+    }
+
+    public static double getMedian(double[] dataSet) {
+        if (dataSet.length % 2 == 0)
+            return (dataSet[dataSet.length / 2 - 1] + dataSet[dataSet.length / 2]) / 2;
+
+        return dataSet[dataSet.length / 2];
+    }
+
+    public static double getStandardDeviation(double[] dataSet) {
+        final double mean = getMean(dataSet);
+        double varianceSum = 0;
+        for (double data : dataSet)
+            varianceSum += (data - mean) * (data - mean);
+        return Math.sqrt(varianceSum / (dataSet.length - 1));
+    }
+
+    public static double[] getStandardizedScores(double[] dataSet) {
+        if (dataSet.length <= 1)
+            throw new IllegalArgumentException("data too short!!!");
+        final double[] standardizedScores = new double[dataSet.length];
+        final double mean = getMean(dataSet);
+        final double standardDeviation = getStandardDeviation(dataSet);
+        for (int i = 0; i < dataSet.length; i++)
+            standardizedScores[i] = (dataSet[i] - mean) / standardDeviation;
+        return standardizedScores;
+    }
+
+    public static double getCorrelationCoefficient(double[] dataSet1, double[] dataSet2) {
+        if (dataSet1.length != dataSet2.length)
+            throw new IllegalArgumentException("data set length unmatched");
+        final double[] standardizedScores1 = getStandardizedScores(dataSet1),
+            standardizedScores2 = getStandardizedScores(dataSet2);
+        double productSum = 0;
+        for (int i = 0; i < dataSet2.length; i++)
+            productSum += standardizedScores1[i] * standardizedScores2[i];
+        return productSum / (dataSet1.length - 1);
+    }
+
+    public static double getBestFitLineSlope(double[] dataSetX, double[] dataSetY) {
+        final double standardizedDeviationX = getStandardDeviation(dataSetX),
+            standardizedDeviationY = getStandardDeviation(dataSetY);
+        return getCorrelationCoefficient(dataSetX, dataSetY) * standardizedDeviationY
+            / standardizedDeviationX;
+    }
+
+    public static double getBestFitLineIntersect(double[] dataSetX, double[] dataSetY) {
+        final double slope = getBestFitLineSlope(dataSetX, dataSetY);
+        return getMean(dataSetY) - slope * getMean(dataSetX);
+    }
+}

src/main/java/frc/lib/util/TimeUtils.java

@@ -0,0 +1,32 @@
+package frc.lib.util;
+
+import org.littletonrobotics.junction.Logger;
+
+public class TimeUtils {
+
+    public static void delay(double seconds) {
+        try {
+            // Convert seconds to total milliseconds
+            long totalMillis = (long) (seconds * 1000);
+            // Calculate the remaining nanoseconds
+            int nanoPart = (int) ((seconds * 1000 - totalMillis) * 1000000);
+
+            // Pause the thread for the specified duration in milliseconds and nanoseconds
+            Thread.sleep(totalMillis, nanoPart);
+        } catch (InterruptedException e) {
+            // Restore the interrupted status
+            Thread.currentThread().interrupt();
+
+            // Optionally, handle the interruption, e.g. logging or throwing a runtime exception
+            System.err.println("The sleep was interrupted");
+        }
+    }
+
+    public static double getLogTimeSeconds() {
+        return Logger.getTimestamp() / 1_000_000.0;
+    }
+
+    public static double getRealTimeSeconds() {
+        return Logger.getRealTimestamp() / 1_000_000.0;
+    }
+}