add fuel depth functions

Author: Owen-Morgan825

src/main/java/frc/robot/subsystems/fuelDetector/FuelCluster.java

@@ -4,6 +4,7 @@
 
 public class FuelCluster {
     public ArrayList<FuelSquare> fuelSquares = new ArrayList<>(0);
+    public double clusterDepth;
 
     public FuelCluster() {
 
@@ -21,4 +22,14 @@ public void addFuelSquare(FuelSquare square) {
         fuelSquares.add(square);
         square.addToCluster(this);
     }
+
+    public double averageSquareDepth() {
+        int size = fuelSquares.size();
+        double sum = 0;
+        for(int i = 0; i < size; i++) {
+            sum += fuelSquares.get(i).averageDepth;
+        }
+        sum /= size;
+        return sum;
+    }
 }

src/main/java/frc/robot/subsystems/fuelDetector/FuelCoordinates.java

@@ -10,6 +10,7 @@ public class FuelCoordinates {
     public double boxY2;
     public double width;
     public double height;
+    public double depth;
 
     public FuelCoordinates(double x, double y, double boxWidth, double boxHeight, double c) {
         boxX = x;
@@ -19,6 +20,7 @@ public FuelCoordinates(double x, double y, double boxWidth, double boxHeight, do
         computeCenterPoint(boxX, boxY, boxX2, boxY2);
         width = boxWidth;
         height = boxHeight;
+        computeDepth(45); //TODO: replace 45 with constant for camera FOV
         chance = c;
     }
     private static double pointFromDistance(double point, double length) {
@@ -34,4 +36,8 @@ public void assignSelfToFuelSquare(int gridWidth, int gridHeight, FuelSquare[][]
         int squareY = (int) Math.round(centerY / gridHeight);
         squareArray[squareX][squareY].addFuel(this);
     }
+    //TODO: implement depth function
+    public double computeDepth(double cameraFOV) {
+        return 0.0;
+    }
 }

src/main/java/frc/robot/subsystems/fuelDetector/FuelDetector.java

@@ -13,6 +13,7 @@ public void periodic() {
 
         //Implement when Limelight/camera detection is actually working
     }
+
     public ArrayList<FuelCoordinates> filterByHighChance(FuelCoordinates[] inputs) {
         //There's probably an easier and shorter way of doing this, but this is simple. Feel free to change it as long as the output doesn't change.
         ArrayList<FuelCoordinates> output = new ArrayList<>(0);
@@ -23,6 +24,7 @@ public ArrayList<FuelCoordinates> filterByHighChance(FuelCoordinates[] inputs) {
         }
         return output;
     }
+
     public FuelSquare[][] divideIntoSquares(ArrayList<FuelCoordinates> fuelCoords, int gridWidth, int gridHeight) {
         FuelSquare[][] output = new FuelSquare[gridWidth][gridHeight];
         for(int w = 0; w < output.length; w++) {
@@ -35,6 +37,7 @@ public FuelSquare[][] divideIntoSquares(ArrayList<FuelCoordinates> fuelCoords, i
         }
         return output;
     }
+
     public ArrayList<FuelCluster> getFuelClusters(FuelSquare[][] fuelSquares) {
         ArrayList<FuelCluster> clusters = new ArrayList<>(0);
         int width = fuelSquares.length;
@@ -53,7 +56,7 @@ public ArrayList<FuelCluster> getFuelClusters(FuelSquare[][] fuelSquares) {
                         fuelSquares[w][h - 1].cluster.addFuelSquare(fuelSquare);
                     } else {
                         FuelCluster c = new FuelCluster(fuelSquare);
-                        clusters.add(new FuelCluster(fuelSquare));
+                        clusters.add(c);
                     }
                 }
             }

src/main/java/frc/robot/subsystems/fuelDetector/FuelSquare.java

@@ -13,6 +13,8 @@ public class FuelSquare {
     public boolean isInFuelCluster = false;
     public FuelCluster cluster;
 
+    public double averageDepth;
+
     private int fuelCount = 0;
 
     public FuelSquare(int x, int y, double width, double height) {
@@ -42,4 +44,13 @@ public void addToCluster(FuelCluster c) {
         isInFuelCluster = true;
         cluster = c;
     }
+    public double averageFuelDepth() {
+        int size = fuelList.size();
+        double sum = 0;
+        for(int i = 0; i < size; i++) {
+            sum += fuelList.get(i).depth;
+        }
+        sum /= size;
+        return sum;
+    }
 }