Fuel classes, fuel detection algorithm

Author: Owen-Morgan825

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

@@ -0,0 +1,37 @@
+package frc.robot.subsystems.fuelDetector;
+
+public class FuelCoordinates {
+    public double chance;
+    public double centerX;
+    public double centerY;
+    public double boxX;
+    public double boxY;
+    public double boxX2;
+    public double boxY2;
+    public double width;
+    public double height;
+
+    public FuelCoordinates(double x, double y, double boxWidth, double boxHeight, double c) {
+        boxX = x;
+        boxY = y;
+        boxX2 = FuelCoordinates.pointFromDistance(x, boxWidth);
+        boxY2 = FuelCoordinates.pointFromDistance(y, boxHeight);
+        computeCenterPoint(boxX, boxY, boxX2, boxY2);
+        width = boxWidth;
+        height = boxHeight;
+        chance = c;
+    }
+    private static double pointFromDistance(double point, double length) {
+        //Get a second point from one point and a distance
+        return point + length;
+    }
+    public void computeCenterPoint(double x, double y, double x2, double y2) {
+        centerX = (x2 - x) / 2;
+        centerY = (y2 - y) / 2;
+    }
+    public void assignSelfToFuelSquare(int gridWidth, int gridHeight, FuelSquare[][] squareArray) {
+        int squareX = (int) Math.round(centerX / gridWidth);
+        int squareY = (int) Math.round(centerY / gridHeight);
+        squareArray[squareX][squareY].addFuel(this);
+    }
+}

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

@@ -1,5 +1,8 @@
 package frc.robot.subsystems.fuelDetector;
 
+import java.lang.reflect.Array;
+import java.util.ArrayList;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class FuelDetector extends SubsystemBase {
@@ -14,4 +17,26 @@ public double[][] getFuelClusters() {
         };
         return output;
     }
+    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<>();
+        for(int i = 0; i < inputs.length; i++) {
+            if(inputs[i].chance >= 80.0) {
+                output.add(inputs[i]);
+            }
+        }
+        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++) {
+            for(int h = 0; h < output[w].length; h++) {
+                output[w][h] = new FuelSquare();
+            }
+        }
+        for(int i = 0; i < fuelCoords.size(); i++) {
+            fuelCoords.get(i).assignSelfToFuelSquare(gridWidth, gridHeight, output);
+        }
+        return output;
+    }
 }

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

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.fuelDetector;
+
+import java.util.ArrayList;
+
+public class FuelSquare {
+    public ArrayList<FuelCoordinates> fuelList = new ArrayList<>();
+
+    public int gridX;
+    public int gridY;
+    public double squareWidth;
+    public double squareHeight;
+
+    public FuelSquare(int x, int y, double width, double height) {
+        gridX = x;
+        gridY = y;
+        squareWidth = width;
+        squareHeight = height;
+    }
+    public FuelSquare() {
+        
+    }
+
+    public void addFuel(FuelCoordinates fuel) {
+        fuelList.add(fuel);
+    }
+}