From ee120a848ce869e6d00892d44895b0d1d0c2a712 Mon Sep 17 00:00:00 2001 From: Jason Xu <40355221+JasonBrave@users.noreply.github.com> Date: Sun, 23 May 2021 16:08:11 -0400 Subject: [PATCH] Add Limelight Simulation --- README.md | 2 +- .../java/viking/{ => vision}/Limelight.java | 127 +++++++++++++----- 2 files changed, 95 insertions(+), 34 deletions(-) rename src/main/java/viking/{ => vision}/Limelight.java (60%) diff --git a/README.md b/README.md index 16a07e4..5156c31 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ Includes support for X-Box One Controller with pre-made inverts for proper value Read paths from CSV files and also create logs in CSV format for debugging. ### Limelight -`viking.Limelight` +`viking.vision.Limelight` A wrapper for the Limelight using NetworkTables. diff --git a/src/main/java/viking/Limelight.java b/src/main/java/viking/vision/Limelight.java similarity index 60% rename from src/main/java/viking/Limelight.java rename to src/main/java/viking/vision/Limelight.java index 72272c9..acb9634 100644 --- a/src/main/java/viking/Limelight.java +++ b/src/main/java/viking/vision/Limelight.java @@ -1,5 +1,9 @@ -package viking; +package viking.vision; +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimInt; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -7,7 +11,7 @@ public class Limelight { private static Limelight instance = null; private NetworkTable limelight = NetworkTableInstance.getDefault().getTable("limelight"); - + public static enum LightMode { DEFAULT, OFF, @@ -19,51 +23,88 @@ public static enum LightMode { private double cameraAngle = 0; // Unit in degrees private double targetHeight = 0; // Height in meters + //simulation specific + private SimDevice sim_device; + private SimBoolean sim_valid_target; + private SimInt sim_led_mode; + private SimDouble sim_target_x, sim_target_y, sim_target_a; + private SimBoolean sim_cammode; + private SimDouble sim_pipeline; + + public Limelight(){ + sim_device = SimDevice.create("Limelight"); + if(sim_device != null){ + sim_valid_target = sim_device.createBoolean("valid_target", SimDevice.Direction.kInput, false); + sim_led_mode = sim_device.createInt("led_mode", SimDevice.Direction.kOutput, 0); + sim_target_x = sim_device.createDouble("target_x", SimDevice.Direction.kInput, 0.0); + sim_target_y = sim_device.createDouble("target_y", SimDevice.Direction.kInput, 0.0); + sim_target_a = sim_device.createDouble("target_a", SimDevice.Direction.kInput, 0.0); + sim_cammode = sim_device.createBoolean("cammode", SimDevice.Direction.kOutput, false); + sim_pipeline = sim_device.createDouble("pipeline", SimDevice.Direction.kOutput, 0); + } + } + /** * Whether the limelight has any valid targets (0 or 1) * @return Returns true if vision target is found */ public boolean validTargets(){ + if(sim_device != null){ + return sim_valid_target.get(); + } + double value = limelight.getEntry("tv").getDouble(0); - + if(value >= 1){ - return true; + return true; } - + return false; } - + /** - * Horizontal Offset From Crosshair To Target + * Horizontal Offset From Crosshair To Target * (LL1: -27 degrees to 27 degrees | LL2: -29.8 to 29.8 degrees) * @return x value of target relative to the crosshair */ public double targetX() { + if(sim_device != null){ + return sim_target_x.get(); + } return limelight.getEntry("tx").getDouble(0); } - + /** - * Vertical Offset From Crosshair To Target + * Vertical Offset From Crosshair To Target * (LL1: -20.5 degrees to 20.5 degrees | LL2: -24.85 to 24.85 degrees) * @return y value of target relative to the crosshair */ public double targetY() { + if(sim_device != null){ + return sim_target_y.get(); + } return limelight.getEntry("ty").getDouble(0); } - + /** * Target Area (0% of image to 100% of image) * @return target area in percentage */ public double targetA() { + if(sim_device != null){ + return sim_target_a.get(); + } return limelight.getEntry("ty").getDouble(0); } - + /** * True active pipeline index of the camera (0 .. 9) * @return active pipeline currently used by the Limelight */ public double getPipeline() { + if(sim_device != null){ + return sim_pipeline.get(); + } return limelight.getEntry("getpipe").getDouble(0); } @@ -72,19 +113,27 @@ public double getPipeline() { * @param value the value to set the camMode to */ public void setDriverMode(boolean value) { - if (value == true) { - limelight.getEntry("camMode").setDouble(1); + if(sim_device != null){ + sim_cammode.set(value); + return; } - else { - limelight.getEntry("camMode").setDouble(0); + + if (value == true) { + limelight.getEntry("camMode").setDouble(1); + } else { + limelight.getEntry("camMode").setDouble(0); } } - + /** * Get the current camMode * @return 0 means normal and 1 means driver mode */ public double driverMode() { + if(sim_device != null){ + return sim_cammode.get() ? 1 : 0; + } + return limelight.getEntry("camMode").getDouble(0); } @@ -93,26 +142,37 @@ public double driverMode() { * @param mode set the mode to either DEFAULT, OFF, BLINK, or ON */ public void setLEDMode(LightMode mode) { + int ledmode = 0; switch(mode) { - case DEFAULT: - limelight.getEntry("ledMode").setNumber(0); - break; - case OFF: - limelight.getEntry("ledMode").setNumber(1); - break; - case BLINK: - limelight.getEntry("ledMode").setNumber(2); - break; - case ON: - limelight.getEntry("ledMode").setNumber(3); - break; + case DEFAULT: + ledmode = 0; + break; + case OFF: + ledmode = 1; + break; + case BLINK: + ledmode = 2; + break; + case ON: + ledmode = 3; + break; } + + if(sim_device != null){ + sim_led_mode.set(ledmode); + return; + } + + limelight.getEntry("ledMode").setNumber(ledmode); } - + /** * Set the vision pipeline */ public void setPipeline(int pipelineID) { + if(sim_device != null){ + sim_pipeline.set(pipelineID); + } limelight.getEntry("pipeline").setNumber(pipelineID); } @@ -139,7 +199,7 @@ public void setCameraAngle(double angle) { public void setTargetHeight(double height) { targetHeight = height; } - + /** * Uses current targetY to calculate the distance to the target * @return the distance to the target in inches (estimation) @@ -147,7 +207,7 @@ public void setTargetHeight(double height) { public double getDistanceFromTarget() { return (targetHeight - cameraHeight) / Math.tan(cameraAngle - targetY()); } - + /** * Uses the FOV and the current targetX to calculate the X angle to the target * @return @@ -158,8 +218,9 @@ public double getHorzAngle() { } public static Limelight getInstance () { - if (instance == null) - instance = new Limelight(); + if (instance == null){ + instance = new Limelight(); + } return instance; } }