diff --git a/Main/src/main/java/Classes/SettingsManager.java b/Main/src/main/java/Classes/SettingsManager.java index 3cb700d9c..964776d56 100644 --- a/Main/src/main/java/Classes/SettingsManager.java +++ b/Main/src/main/java/Classes/SettingsManager.java @@ -37,12 +37,13 @@ public class SettingsManager { public static HashMap UsbCameras = new HashMap(); public static HashMap USBCamerasInfo = new HashMap(); public static Objects.GeneralSettings GeneralSettings; - public static HashMap CameraPort = new HashMap(); - public static HashMap CamerasCurrentPipeline = new HashMap(); + public static HashMap CamerasCurrentPipeline = new HashMap(); + public static HashMap CameraPort = new HashMap(); private Path SettingsPath = Paths.get(System.getProperty("user.dir"),"Settings"); private Path CamsPath = Paths.get(SettingsPath.toString(),"Cams"); + private void InitiateGeneralSettings(){ CheckPath(SettingsPath); try { diff --git a/Main/src/main/java/Handlers/Vision/Camera.java b/Main/src/main/java/Handlers/Vision/Camera.java deleted file mode 100644 index 278951580..000000000 --- a/Main/src/main/java/Handlers/Vision/Camera.java +++ /dev/null @@ -1,4 +0,0 @@ -package Handlers.Vision; - -public class Camera { -} diff --git a/Main/src/main/java/Handlers/Vision/CameraProcess.java b/Main/src/main/java/Handlers/Vision/CameraProcess.java new file mode 100644 index 000000000..123eba1ad --- /dev/null +++ b/Main/src/main/java/Handlers/Vision/CameraProcess.java @@ -0,0 +1,56 @@ +package Handlers.Vision; + +import Classes.SettingsManager; +import Objects.Pipeline; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.cameraserver.CameraServer; +import org.opencv.core.Mat; +import org.apache.commons.math3.fraction.Fraction; +import org.apache.commons.math3.util.FastMath; +import org.opencv.core.MatOfPoint; + +import java.util.List; + +public class CameraProcess implements Runnable{ + private String CameraName; + public CameraProcess(String CameraName){ + this.CameraName = CameraName; + } + + @Override + public void run() { + //calling all classes + CameraServer cs = CameraServer.getInstance(); + NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); + SettingsManager manager = SettingsManager.getInstance(); + VisionProcess visionProcess = new VisionProcess(); + //Setting up camera and network table + var Table = networkTableInstance.getTable("/Chameleon-Vision/" + CameraName); + var PipeLineEntry = Table.getEntry("Pipeline"); + var DriverModeEntry = Table.getEntry("Driver_Mode"); + var cv_sink = cs.getVideo(manager.UsbCameras.get(CameraName)); + int Width = manager.Cameras.get(CameraName).camVideoMode.width; + int Height = manager.Cameras.get(CameraName).camVideoMode.heigh; + var cv_publish = cs.putVideo(CameraName,Width,Height); + //initial math setup for camera + double DiagonalView = FastMath.toRadians(manager.Cameras.get(CameraName).FOV); + Fraction AspectFraction = new Fraction(Width,Height); + int HorizontalRatio = AspectFraction.getNumerator(); + int VerticalRatio = AspectFraction.getDenominator(); + double HorizontalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (HorizontalRatio / DiagonalView)) * 2; + double VerticalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (VerticalRatio / DiagonalView)) * 2; + double H_FOCAL_LENGTH = Width / (2 * FastMath.tan(HorizontalView /2)); + double V_FOCAL_LENGTH = Width / (2 * FastMath.tan(VerticalView /2)); + Mat mat = new Mat(); + long time; + + while (!Thread.interrupted()){ + Pipeline pipeline = manager.Cameras.get(CameraName).pipelines.get(manager.CamerasCurrentPipeline.get(CameraName)); + time = cv_sink.grabFrame(mat); + Mat HSVImage = visionProcess.HSVThreshold(pipeline.hue,pipeline.saturation,pipeline.value,mat,pipeline.erode,pipeline.dilate); + List Contours = visionProcess.FindContours(HSVImage); + } + + } +} diff --git a/Main/src/main/java/Handlers/Vision/VisionProcess.java b/Main/src/main/java/Handlers/Vision/VisionProcess.java index 2664efff1..d1d3d3372 100644 --- a/Main/src/main/java/Handlers/Vision/VisionProcess.java +++ b/Main/src/main/java/Handlers/Vision/VisionProcess.java @@ -1,4 +1,32 @@ package Handlers.Vision; +import org.jetbrains.annotations.NotNull; +import org.opencv.core.*; +import org.opencv.imgproc.*; + +import java.util.ArrayList; +import java.util.List; + public class VisionProcess { + private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + + public Mat HSVThreshold(@NotNull List hue, @NotNull List saturation , @NotNull List value, Mat image, boolean IsErode, boolean IsDilate){ + Mat hsv = new Mat(); + Imgproc.cvtColor(image,hsv,Imgproc.COLOR_BGR2HSV,3); + new Scalar(hue.get(0),saturation.get(0),value.get(0)); + Mat threshold = new Mat(); + Core.inRange(threshold,new Scalar(hue.get(0),saturation.get(0),value.get(0)),new Scalar(hue.get(1),saturation.get(1),value.get(1)),threshold); + if (IsErode){ + Imgproc.erode(threshold,threshold, Kernel); + } + if (IsDilate){ + Imgproc.dilate(threshold,threshold, Kernel); + } + return threshold; + } + public List FindContours(Mat BinaryImage){ + List Contours = new ArrayList<>(); + Imgproc.findContours(BinaryImage,Contours,new Mat(),Imgproc.RETR_EXTERNAL,Imgproc.CHAIN_APPROX_TC89_L1); + return Contours; + } } diff --git a/Main/src/main/java/Main.java b/Main/src/main/java/Main.java index 8246bef58..078c07dda 100644 --- a/Main/src/main/java/Main.java +++ b/Main/src/main/java/Main.java @@ -1,8 +1,19 @@ import Classes.SettingsManager; +import Handlers.Vision.CameraProcess; import Handlers.Web.Server; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +import java.util.Map; + public class Main { public static void main(String [] args) { SettingsManager manager = SettingsManager.getInstance(); + NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number); + for (Map.Entry entry: SettingsManager.UsbCameras.entrySet()){ + new Thread(new CameraProcess(entry.getKey())); + } Server.main(8888); } }