From f6915b14cf5191a167dbc7b6ec8ac17e0efa7a6d Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:17:48 -0400 Subject: [PATCH 01/28] updated limelight code and added more tests --- .../common/limelight/LimeLightVision.java | 127 ------------------ .../common/limelight/TestLimelight.java | 84 +++++++++--- 2 files changed, 68 insertions(+), 143 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java diff --git a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java deleted file mode 100644 index c4fcb72..0000000 --- a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java +++ /dev/null @@ -1,127 +0,0 @@ -package org.usfirst.frc4048.common.limelight; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class LimeLightVision { - public static final int LED_ON = 3; - public static final int LED_BLINK = 2; - public static final int LED_OFF = 1; - - private double cameraHeight; - private double targetHeight; - private double cameraAngle; - - NetworkTable table; - NetworkTableEntry tv; - NetworkTableEntry tx; - NetworkTableEntry ty; - NetworkTableEntry ta; - NetworkTableEntry ts; - NetworkTableEntry tl; - NetworkTableEntry stream; - - /** - * Utility class to calculate distances based off of LimeLIght camera angle calculation. - * @param cameraHeight height of Limelight camera, in inches - * @param targetHeight height of field target, in inches - * @param cameraAngle camera mounting angle, in degrees, where 0 is horizontal and down is negative - */ - public LimeLightVision(double cameraHeight, double targetHeight, double cameraAngle) { - this.cameraHeight = cameraHeight; - this.targetHeight = targetHeight; - this.cameraAngle = cameraAngle; - table = NetworkTableInstance.getDefault().getTable("limelight"); - tv = table.getEntry("tv"); - tx = table.getEntry("tx"); - ty = table.getEntry("ty"); - ta = table.getEntry("ta"); - ts = table.getEntry("ts"); - tl = table.getEntry("tl"); - stream = table.getEntry("stream"); - } - - // This method is required in order to allow tests to be written since NetworkTable is final and cannot be mocked - LimeLightVision(boolean DO_NOT_USE_FOR_TESTING_ONLY, double cameraHeight, double targetHeight, double cameraAngle) { - this.cameraHeight = cameraHeight; - this.targetHeight = targetHeight; - this.cameraAngle = cameraAngle; - } - - /** - * Calculate and return the distances from the camera. - * The calculated distances are in inches, as calculated by the triangulation of the vertical distance between the - * camera and target and the detected angles. - * forward is the "straight" distance from the camera to the target - * sideways is the "lateral" distance from the camera to the target - * @return Camera distance calculated from the limelight, null if no target acquired - */ - public CameraDistance getTargetDistance() { - double validTarget = tv.getDouble(0.0); - if ( validTarget != 1.0 ) { - return null; - } - - double x = tx.getDouble(0.0); - double y = ty.getDouble(0.0); - - return calcCameraDistance(x, y, targetHeight, cameraHeight, cameraAngle); - } - - /** - * Calculate and return the camera detected angles - * The angles are returned by the Limelight camera and are in degrees. - * tx is the horizontal angle between the target center and the camera crosshairs - * ty is the vertical angle between the target center and the camera crosshairs - * @return the X and Y angles detected by the camera, Null in case no target is acquired - */ - public CameraAngles getCameraAngles() { - double validTarget = tv.getDouble(0.0); - if ( validTarget != 1.0 ) { - return null; - } - - return new CameraAngles(tx.getDouble(0.0), ty.getDouble(0.0)); - } - - public void setLedOn() { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_ON); - } - - public void setLedOff() { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_OFF); - } - - public void setLedBlink() { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_BLINK); - } - - /** - * The Limelight camera has 3 streaming modes if a second USB camera is plugged in. - * 0 = both, side by side - * 1 = Limelight stream with USB camera in the corner - * 2 = USB camera stream with Limelight in the corner - */ - public void setStream(double option) { - if (option >= 0.0 && option <= 2.0) - { - stream.setDouble(option); - } - } - - public double getStream() { - return stream.getDouble(0.0); - } - - /** - * Internal utility to calculate the distances, in inches, from the camera based off of the viewed angles - */ - CameraDistance calcCameraDistance(double angleX, double angleY, double targetHeight, double cameraHeight, double cameraAngle) { - double forwardDistance = (targetHeight - cameraHeight) / Math.tan(Math.toRadians(cameraAngle + angleY)); - double sidewaysDistance = forwardDistance * Math.tan(Math.toRadians(angleX)); - - return new CameraDistance(forwardDistance, sidewaysDistance); - } - -} diff --git a/src/test/java/org/usfirst/frc4048/common/limelight/TestLimelight.java b/src/test/java/org/usfirst/frc4048/common/limelight/TestLimelight.java index b3d0227..d95495f 100644 --- a/src/test/java/org/usfirst/frc4048/common/limelight/TestLimelight.java +++ b/src/test/java/org/usfirst/frc4048/common/limelight/TestLimelight.java @@ -14,110 +14,162 @@ public class TestLimelight { @BeforeClass public static void init() throws Exception { - classUnderTest = new LimeLightVision(true, CAMERA_HEIGHT, TARGET_HEIGT, CAMERA_ANGLE); + classUnderTest = new LimeLightVision(true,CAMERA_HEIGHT, TARGET_HEIGT, CAMERA_ANGLE); } + @Test + public void testCalcHorizontalDistanceToTarget1(){ + double horizDist = classUnderTest.calcHorizontalDistanceToTarget(0); + Assert.assertEquals(-30.1078093318, horizDist, 0.0001); + } + @Test + public void testCalcHorizontalDistanceToTarget2(){ + double horizDist = classUnderTest.calcHorizontalDistanceToTarget(10); + Assert.assertEquals(-133.006662526, horizDist, 0.0001); + } + @Test + public void testCalcHorizontalDistanceToTarget3(){ + double horizDist = classUnderTest.calcHorizontalDistanceToTarget(-10); + Assert.assertEquals(-12.4603677378, horizDist, 0.0001); + } + @Test + public void testCalcHorizontalDistanceToTarget4(){ + double horizDist = classUnderTest.calcHorizontalDistanceToTarget(-45); + Assert.assertEquals(3.99139381004, horizDist, 0.0001); + } + @Test + public void testCalcHorizontalDistanceToTarget5(){ + double horizDist = classUnderTest.calcHorizontalDistanceToTarget(30); + Assert.assertEquals(44.8741444409, horizDist, 0.0001); + } + //(relativeHeight)/Math.sin(Math.toRadians(cameraAngle+angleY)); + @Test + public void testCalcDirectDistanceToTarget1(){ + double horizDist = classUnderTest.calcDirectDistanceToTarget(0); + Assert.assertEquals(-41.3356549409, horizDist, 0.0001); + } + @Test + public void testCalcDirectDistanceToTarget2(){ + double horizDist = classUnderTest.calcDirectDistanceToTarget(10); + Assert.assertEquals(-143.355870221, horizDist, 0.0001); + } + @Test + public void testCalcDirectDistanceToTarget3(){ + double horizDist = classUnderTest.calcDirectDistanceToTarget(-10); + Assert.assertEquals(-24.5859333557, horizDist, 0.0001); + } + @Test + public void testCalcDirectDistanceToTarget4(){ + double horizDist = classUnderTest.calcDirectDistanceToTarget(-45); + Assert.assertEquals(-11.6663339722, horizDist, 0.0001); + } + @Test + public void testCalcDirectDistanceToTarget5(){ + double horizDist = classUnderTest.calcDirectDistanceToTarget(-10); + Assert.assertEquals(36.2795527854, horizDist, 0.0001); + } + @Test public void testCalcAngle1() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, 0, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, 0); Assert.assertEquals(36.0970284, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.0, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle2() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, 10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, 10); Assert.assertEquals(128.7059963, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.0, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle3() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -10); Assert.assertEquals(20.21433097, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.0, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle4() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -45, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -45); Assert.assertEquals(5.407745571, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.0, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle5() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -30, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(0, -30); Assert.assertEquals(9.31977274, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.0, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle6() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, 0, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, 0); Assert.assertEquals(36.0970284, cameraDistance.getForward(), 0.0001); Assert.assertEquals(6.364880031, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle7() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, 10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, 10); Assert.assertEquals(128.7059963, cameraDistance.getForward(), 0.0001); Assert.assertEquals(22.69433973, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle8() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -10); Assert.assertEquals(20.21433097, cameraDistance.getForward(), 0.0001); Assert.assertEquals(3.564331946, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle9() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -45, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -45); Assert.assertEquals(5.407745571, cameraDistance.getForward(), 0.0001); Assert.assertEquals(0.953531449, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle10() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -30, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(10, -30); Assert.assertEquals(9.31977274, cameraDistance.getForward(), 0.0001); Assert.assertEquals(1.643327403, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle11() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, 0, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, 0); Assert.assertEquals(36.0970284, cameraDistance.getForward(), 0.0001); Assert.assertEquals(-6.364880031, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle12() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, 10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, 10); Assert.assertEquals(128.7059963, cameraDistance.getForward(), 0.0001); Assert.assertEquals(-22.69433973, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle13() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -10, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -10); Assert.assertEquals(20.21433097, cameraDistance.getForward(), 0.0001); Assert.assertEquals(-3.564331946, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle14() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -45, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -45); Assert.assertEquals(5.407745571, cameraDistance.getForward(), 0.0001); Assert.assertEquals(-0.953531449, cameraDistance.getSideways(), 0.0001); } @Test public void testCalcAngle15() throws Exception { - CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -30, TARGET_HEIGT, CAMERA_HEIGHT, CAMERA_ANGLE); + CameraDistance cameraDistance = classUnderTest.calcCameraDistance(-10, -30); Assert.assertEquals(9.31977274, cameraDistance.getForward(), 0.0001); Assert.assertEquals(-1.643327403, cameraDistance.getSideways(), 0.0001); } From 20fea9fc3df17a4039a1cc379392fcff00b8a2e9 Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:18:23 -0400 Subject: [PATCH 02/28] updated motorUtils and shuffleboard code --- .../smartshuffleboard/SmartShuffleboard.java | 42 +++++++++- .../SmartShuffleboardTab.java | 3 +- .../frc4048/common/util/MotorUtils.java | 79 ++++++++++--------- 3 files changed, 82 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java index 0bc9483..ac9a48f 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; +import edu.wpi.first.wpilibj2.command.CommandBase; import java.util.HashMap; import java.util.Map; @@ -34,16 +35,48 @@ public class SmartShuffleboard { private final static Map smartTabMap = new HashMap(); - public static void put(String tabName, String fieldName, Object value) // value is primitive + public static SimpleWidget put(String tabName, String fieldName, Object value) // value is primitive { SmartShuffleboardTab smartTab = getOrCreateTab(tabName); - smartTab.put(fieldName, value); + return smartTab.put(fieldName, value); } - public static void put(String tabName, String layoutName, String fieldName, Object value) // value is primitive + public static SimpleWidget put(String tabName, String layoutName, String fieldName, Object value) // value is primitive { SmartShuffleboardTab smartTab = getOrCreateTab(tabName); - smartTab.put(fieldName, layoutName, value); + return smartTab.put(fieldName, layoutName, value); + } + + public static boolean getBoolean(String tabName, String fieldName, boolean defaultValue) { + SmartShuffleboardTab smartTab = smartTabMap.get(tabName); + if (smartTab == null) { + return defaultValue; + } + return smartTab.getBoolean(fieldName, defaultValue); + } + + public static double getDouble(String tabName, String fieldName, double defaultValue) { + SmartShuffleboardTab smartTab = smartTabMap.get(tabName); + if (smartTab == null) { + return defaultValue; + } + return smartTab.getDouble(fieldName, defaultValue); + } + + public static String getString(String tabName, String fieldName, String defaultValue) { + SmartShuffleboardTab smartTab = smartTabMap.get(tabName); + if (smartTab == null) { + return defaultValue; + } + return smartTab.getString(fieldName, defaultValue); + } + + public static NetworkTableValue getValue(String tabName, String fieldName) { + SmartShuffleboardTab smartTab = smartTabMap.get(tabName); + if (smartTab == null) { + return null; + } + return smartTab.getValue(fieldName); } public static void putCommand(String tabName, String fieldName, CommandBase cmd) // value is primitive @@ -89,4 +122,5 @@ private static SmartShuffleboardTab getOrCreateTab(String tabName) { } return smartTab; } + } diff --git a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java index 3f8ea1a..94cdfa5 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java @@ -7,10 +7,9 @@ package org.usfirst.frc4048.common.smartshuffleboard; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.shuffleboard.*; import edu.wpi.first.wpilibj2.command.CommandBase; - +import edu.wpi.first.networktables.NetworkTableEntry; import java.util.*; diff --git a/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java b/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java index 42ce170..be8404d 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java +++ b/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java @@ -3,46 +3,53 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.Timer; -import org.usfirst.frc4048.common.logging.Logging; +import frc.robot.utils.logging.Logging; /* * MotorStall object should be instantiated in init() method of a command * isFinished() should call isStalled() to determine if stalled for longer than allowed */ public class MotorUtils { - public static final double DEFAULT_TIMEOUT = 0.15; - private double timeout; - private double time; - final int PDPChannel; - final double currentThreshold; - private PowerDistribution pdp; - - public MotorUtils(PowerDistribution pdp, int PDPPort, double currentThreshold) { - this.pdp = pdp; - this.timeout = DEFAULT_TIMEOUT; - this.PDPChannel = PDPPort; - this.currentThreshold = currentThreshold; - time = Timer.getFPGATimestamp(); - } - - public MotorUtils(PowerDistribution pdp, int PDPPort, double currentThreshold, double timeout) { - this(pdp, PDPPort, currentThreshold); - this.timeout = timeout; - } - - public boolean isStalled() { - final double currentValue = pdp.getCurrent(PDPChannel); - final double now = Timer.getFPGATimestamp(); - - if (currentValue < currentThreshold) { - time = now; - } else { - DriverStation.reportError("Motor stall, PDP Channel=" + PDPChannel, false); - if (now - time > timeout) { - Logging.instance().traceMessage(Logging.MessageLevel.INFORMATION, "Motor stall, PDP channel =" + PDPChannel); - return true; - } - } - return false; - } + public static final double DEFAULT_TIMEOUT = 0.15; + private double timeout; + private double time; + private PowerDistribution powerDistPanel; + + final int PDPChannel; + final double currentThreshold; + + private boolean everStalled = false; + + public MotorUtils(int PDPPort, double currentThreshold, double timeout, PowerDistribution powerDistPanel ){ + this.timeout = timeout; + this.PDPChannel = PDPPort; + this.currentThreshold = currentThreshold; + this.powerDistPanel = powerDistPanel; + time = Timer.getFPGATimestamp(); + } + + public boolean isStalled() { + final double currentValue = powerDistPanel.getCurrent(PDPChannel); + final double now = Timer.getFPGATimestamp(); + + if (currentValue < currentThreshold) { + time = now; + } else { + DriverStation.reportError("Motor stall, PDP Channel=" + PDPChannel, false); + if (now - time > timeout) { + everStalled = true; + Logging.instance().traceMessage(Logging.MessageLevel.INFORMATION, "Motor stall, PDP channel =" + PDPChannel); + return true; + } + } + return false; + } + + public boolean everStalled() { + return everStalled; + } + + public void resetStall() { + everStalled = false; + } } From fc70b22d2e311bed31809fb70add6124bed0c9c5 Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:18:58 -0400 Subject: [PATCH 03/28] forgot to add this to limelight commit --- .../common/limelight/LimeLightVision.java | 148 ++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java diff --git a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java new file mode 100644 index 0000000..7f427f5 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java @@ -0,0 +1,148 @@ +package org.usfirst.frc4048.common.limelight; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class LimeLightVision { + public static final int LED_ON = 3; + public static final int LED_BLINK = 2; + public static final int LED_OFF = 1; + + /** The relative height of the target to the camera, for trig calculations */ + private double relativeHeight; + private double cameraAngle; + + NetworkTable table; + NetworkTableEntry tv; + NetworkTableEntry tx; + NetworkTableEntry ty; + NetworkTableEntry ta; + NetworkTableEntry ts; + NetworkTableEntry tl; + NetworkTableEntry stream; + + /** + * Utility class to calculate distances based off of LimeLIght camera angle calculation. + * @param cameraHeight height of Limelight camera, in inches + * @param targetHeight height of field target, in inches + * @param cameraAngle camera mounting angle, in degrees, where 0 is horizontal and down is negative + */ + public LimeLightVision(double cameraHeight, double targetHeight, double cameraAngle) { + this.relativeHeight = targetHeight - cameraHeight; + this.cameraAngle = cameraAngle; + table = NetworkTableInstance.getDefault().getTable("limelight"); + tv = table.getEntry("tv"); + tx = table.getEntry("tx"); + ty = table.getEntry("ty"); + ta = table.getEntry("ta"); + ts = table.getEntry("ts"); + tl = table.getEntry("tl"); + stream = table.getEntry("stream"); + } + + // This method is required in order to allow tests to be written since NetworkTable is final and cannot be mocked + public LimeLightVision(boolean DO_NOT_USE_FOR_TESTING_ONLY, double cameraHeight, double targetHeight, double cameraAngle) { + this.relativeHeight = targetHeight - cameraHeight; + this.cameraAngle = cameraAngle; + } + + /** + * Calculate and return the distances from the camera. + * The calculated distances are in inches, as calculated by the triangulation of the vertical distance between the + * camera and target and the detected angles. + * forward is the "straight" distance from the camera to the target + * sideways is the "lateral" distance from the camera to the target + * @return Camera distance calculated from the limelight, null if no target acquired + */ + public CameraDistance getTargetDistance() { + double validTarget = tv.getDouble(0.0); + if ( validTarget != 1.0 ) { + return null; + } + + double x = tx.getDouble(0.0); + double y = ty.getDouble(0.0); + + return calcCameraDistance(x, y); + } + + /** + * Return true if the limelight has a target + * + * @return true if the camera has a target + */ + public boolean hasTarget() { + return (tv.getDouble(0.0) > 0); + } + + /** + * Calculate and return the camera detected angles + * The angles are returned by the Limelight camera and are in degrees. + * tx is the horizontal angle between the target center and the camera crosshairs + * ty is the vertical angle between the target center and the camera crosshairs + * @return the X and Y angles detected by the camera, Null in case no target is acquired + */ + public CameraAngles getCameraAngles() { + double validTarget = tv.getDouble(0.0); + if ( validTarget != 1.0 ) { + return null; + } + + return new CameraAngles(tx.getDouble(0.0), ty.getDouble(0.0)); + } + + public void setLedOn() { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_ON); + } + + public void setLedOff() { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_OFF); + } + + public void setLedBlink() { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(LED_BLINK); + } + + public void setPipeline(int pipelineMode) { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineMode); + } + + /** + * The Limelight camera has 3 streaming modes if a second USB camera is plugged in. + * 0 = both, side by side + * 1 = Limelight stream with USB camera in the corner + * 2 = USB camera stream with Limelight in the corner + */ + public void setStream(double option) { + if (option >= 0.0 && option <= 2.0) + { + stream.setDouble(option); + } + } + + public double getStream() { + return stream.getDouble(0.0); + } + + /** + * Internal utility to calculate the distances, in inches, from the camera based off of the viewed angles + */ + private CameraDistance calcCameraDistance(double angleX, double angleY) { + double forwardDistance = (relativeHeight) / Math.tan(Math.toRadians(cameraAngle + angleY)); + double sidewaysDistance = forwardDistance * Math.tan(Math.toRadians(angleX)); + + return new CameraDistance(forwardDistance, sidewaysDistance); + } + + public double calcDirectDistanceToTarget(double angleY){ + return (relativeHeight)/Math.sin(Math.toRadians(cameraAngle+angleY)); + } + + public double calcHorizontalDistanceToTarget(double angleY){ + /*Assumes y offset from camera is in degrees*/ + //+10 Accounts for distance from camera to bumper and distance to center of hoop + return (relativeHeight)/Math.tan(Math.toRadians(cameraAngle+angleY)) + 10; + } + +} From c71d901462dfcb426d2dfb3aa0e34e3d07a45b0b Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:19:12 -0400 Subject: [PATCH 04/28] added LED panel --- .../usfirst/frc4048/common/util/LedPanel.java | 95 +++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/util/LedPanel.java diff --git a/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java new file mode 100644 index 0000000..1a147b1 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java @@ -0,0 +1,95 @@ +package org.usfirst.frc4048.common.util; + +public class LedPanel { + public enum PicID { + ZERO(false,false,false), + ONE(true,false,false), + TWO(false,true,false), + THREE(true,true,false), + FOUR(false,false,true), + FIVE(true,false,true), + SIX(false,true,true), + SEVEN(true,true,true); + private final boolean op1; + private final boolean op2; + private final boolean op3; + PicID(boolean op1, boolean op2, boolean op3) { + this.op1 = op1; + this.op2 = op2; + this.op3 = op3; + } + } + private PicID ID; + + private final DigitalOutput output1; + private final DigitalOutput output2; + private final DigitalOutput output3; + + public LedPanel(int digitalOut1, int digitalOut2, int digitalOut3) { + ID = PicID.ZERO; + output1 = new DigitalOutput(digitalOut1); + output2 = new DigitalOutput(digitalOut2); + output3 = new DigitalOutput(digitalOut3); + } + + public PicID getID() { + return ID; + } + + public void setID(PicID picID) { + ID = picID; + setPic(picID); + } + /* Output1 Output 2 Output 3 f(x) +True Values{ 1 2 4 x%2!=0 + 3 3 5 + 5 6 6 + 7 7 7 } + */ + + private void setPic(PicID picID) { + output1.set(picID.op1); + output2.set(picID.op2); + output3.set(picID.op3); +// if(picID == 0) { +// output1.set(false); +// output2.set(false); +// output3.set(false); +// } +// else if(picID == 1) { +// output1.set(true); +// output2.set(false); +// output3.set(false); +// } +// else if(picID == 2) { +// output1.set(false); +// output2.set(true); +// output3.set(false); +// } +// else if(picID == 3) { +// output1.set(true); +// output2.set(true); +// output3.set(false); +// } +// else if(picID == 4) { +// output1.set(false); +// output2.set(false); +// output3.set(true); +// } +// else if(picID == 5) { +// output1.set(true); +// output2.set(false); +// output3.set(true); +// } +// else if(picID == 6) { +// output1.set(false); +// output2.set(true); +// output3.set(true); +// } +// else if(picID == 7) { +// output1.set(true); +// output2.set(true); +// output3.set(true); +// } + } +} From 8b47f9686e21cd45f6cffdd2f1c826e78431dccb Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:19:21 -0400 Subject: [PATCH 05/28] added more diags --- .../common/diag/DiagDutyCycleEncoder.java | 30 +++++++++++ .../frc4048/common/diag/DiagPhotonVision.java | 54 +++++++++++++++++++ .../common/diag/DiagSparkMaxAbsEncoder.java | 30 +++++++++++ .../common/diag/DiagSparkMaxSwitch.java | 43 +++++++++++++++ .../frc4048/common/diag/DiagToFSensor.java | 30 +++++++++++ 5 files changed, 187 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/diag/DiagDutyCycleEncoder.java create mode 100644 src/main/java/org/usfirst/frc4048/common/diag/DiagPhotonVision.java create mode 100644 src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxAbsEncoder.java create mode 100644 src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxSwitch.java create mode 100644 src/main/java/org/usfirst/frc4048/common/diag/DiagToFSensor.java diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagDutyCycleEncoder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagDutyCycleEncoder.java new file mode 100644 index 0000000..a93c9a7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagDutyCycleEncoder.java @@ -0,0 +1,30 @@ +package org.usfirst.frc4048.common.diag; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has traveled at least a given + * distance from its initial position (measured at initialization or after a reset) + */ +public class DiagDutyCycleEncoder extends DiagDistanceTraveled { + + private DutyCycleEncoder encoder; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for success + * @param encoder - the encoder instance to test + */ + public DiagDutyCycleEncoder(String title, String name, double requiredTravel, DutyCycleEncoder encoder) { + super(title, name, requiredTravel); + this.encoder = encoder; + reset(); + } + + @Override + protected double getCurrentValue() { + return encoder.get(); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagPhotonVision.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagPhotonVision.java new file mode 100644 index 0000000..adf480d --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagPhotonVision.java @@ -0,0 +1,54 @@ +package org.usfirst.frc4048.common.diag; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Base class for Diagnosable that have an initial value (their "current" value) and that are required to change their + * value by a certain amount from that + */ +public abstract class DiagPhotonVision implements Diagnosable { + + private final String name; + private final String title; + private int firstTagId; + private double firstTimestamp; + protected GenericEntry networkTableEntry; + + public DiagPhotonVision(String title, String name) { + this.title = title; + this.name = name; + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = shuffleBoardTab.getLayout(title, BuiltInLayouts.kList).withSize(width, height).add(name, false).getEntry(); //getLayout(title, BuiltInLayouts.kList) + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + firstTagId = -1; + firstTimestamp = -1; + } + + boolean getDiagResult() { + if (firstTagId == -1 || firstTimestamp == -1){ + firstTimestamp = getTagTimestamp(); + firstTagId = getTagId(); + } else { + return getTagTimestamp() != firstTimestamp; + } + return false; + } + + protected abstract int getTagId(); + protected abstract double getTagTimestamp(); +} diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxAbsEncoder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxAbsEncoder.java new file mode 100644 index 0000000..5fe2cca --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxAbsEncoder.java @@ -0,0 +1,30 @@ +package org.usfirst.frc4048.common.diag; + +import com.ctre.phoenix.sensors.WPI_CANCoder; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has traveled at least a given + * distance from its initial position (measured at initialization or after a reset) + */ +public class DiagSparkMaxAbsEncoder extends DiagDistanceTraveled { + + private WPI_CANCoder canCoder; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for success + * @param canSparkMax - the encoder instance to test + */ + public DiagSparkMaxAbsEncoder(String title, String name, double requiredTravel, WPI_CANCoder canCoder) { + super(title, name, requiredTravel); + this.canCoder = canCoder; + reset(); + } + + @Override + protected double getCurrentValue() { + return canCoder.getAbsolutePosition(); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxSwitch.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxSwitch.java new file mode 100644 index 0000000..43c05a7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxSwitch.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4048.common.diag; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch.Type; + +/** + * Diagnostics class for digital switch connected directly to the talon SRX + * it is a DiagBoolean subclass. + */ +public class DiagSparkMaxSwitch extends DiagBoolean { + public DiagSparkMaxSwitch(String title, String name) { + super(title, name); + } + + public enum Direction {FORWARD, REVERSE}; + private CANSparkMax canSparkMax; + private Direction direction; + + /* + * Constructor + * + * @param name -the name of the unit. Will be used on the Shuffleboard + * @param talonSRX -the talon SRX to read the switch value from + */ + + public DiagSparkMaxSwitch(String title, String name, CANSparkMax canSparkMax, Direction direction) { + super(title, name); + this.canSparkMax = canSparkMax; + this.direction = direction; + } + @Override + protected boolean getValue() { + switch (direction) { + case FORWARD: + return canSparkMax.getForwardLimitSwitch(Type.kNormallyOpen).isPressed(); + case REVERSE: + return canSparkMax.getReverseLimitSwitch(Type.kNormallyOpen).isPressed(); + default: + return false; + } + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagToFSensor.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagToFSensor.java new file mode 100644 index 0000000..576b207 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagToFSensor.java @@ -0,0 +1,30 @@ +package org.usfirst.frc4048.common.diag; + +import com.revrobotics.Rev2mDistanceSensor; + +/** + * Diagnostics class for the Sonar. It is a DiagMinMax object. + * Give it a max and minimum distance to test. + */ +public class DiagToFSensor extends DiagMinMax { + + private Rev2mDistanceSensor sensor; + + /** + * Constructor + * + * @param name -Name of the sensor, to be used on Shuffleboard + * @param sonar -The Ultrasonic object to be tested. + * @param minDistance -The minimum testing distance for the sonar; the one that will be tested against. + * @param maxDistance -The maximum testing distance for the sonar; the one that will be tested against. + */ + public DiagToFSensor(String title, String name, Rev2mDistanceSensor sensor, double minDistance, double maxDistance){ + super(title, name, minDistance, maxDistance); + this.sensor = sensor; + } + + @Override + double getSensorReading() { + return sensor.getRange(); + } +} From b3cde29efbccc0affe2d3dba6f7f6a4f5e51cbca Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:41:22 -0400 Subject: [PATCH 06/28] got rid of maven and added gradle --- build.gradle | 100 +++++++ pom.xml | 129 --------- settings.gradle | 27 ++ vendordeps/NavX.json | 35 +++ vendordeps/Phoenix.json | 423 ++++++++++++++++++++++++++++ vendordeps/REV2mDistanceSensor.json | 55 ++++ vendordeps/REVLib.json | 73 +++++ vendordeps/WPILibNewCommands.json | 37 +++ vendordeps/photonlib.json | 41 +++ 9 files changed, 791 insertions(+), 129 deletions(-) create mode 100644 build.gradle delete mode 100644 pom.xml create mode 100644 settings.gradle create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/Phoenix.json create mode 100644 vendordeps/REV2mDistanceSensor.json create mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/WPILibNewCommands.json create mode 100644 vendordeps/photonlib.json diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..3fcd0d3 --- /dev/null +++ b/build.gradle @@ -0,0 +1,100 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2023.4.3" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = 4048 + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + testImplementation 'junit:junit:4.13.1' + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/pom.xml b/pom.xml deleted file mode 100644 index 2e610d6..0000000 --- a/pom.xml +++ /dev/null @@ -1,129 +0,0 @@ - - - - 4.0.0 - - org.usfirst.frc4048 - java-common - 1.0.0 - - java-common - - - 2022.2.1 - 5.19.4 - 4.0.442 - 2022.1.1 - - UTF-8 - 11 - 11 - - - - - edu.wpi.first.wpilibj - wpilibj-java - ${wpilib.version} - - - - edu.wpi.first.wpiutil - wpiutil-java - ${wpilib.version} - - - - edu.wpi.first.wpilibNewCommands - wpilibNewCommands-java - ${wpilib.version} - - - - edu.wpi.first.ntcore - ntcore-java - ${wpilib.version} - - - - edu.wpi.first.cscore - cscore-java - ${wpilib.version} - - - - edu.wpi.first.hal - hal-java - ${wpilib.version} - - - - com.ctre.phoenix - api-java - ${ctre.version} - - - - com.ctre.phoenix - wpiapi-java - ${ctre.version} - - - - com.revrobotics.frc - REVLib-java - ${REVLib-java.version} - - - - com.kauailabs.navx.frc - navx-java - ${navx-java.version} - - - - junit - junit - 4.13.1 - test - - - - org.mockito - mockito-core - 2.4.0 - test - - - - - - - - - wpilib-release - Wpilib Releases Repo - https://first.wpi.edu/FRC/roborio/maven/release/ - - - - ctre-release - CTRE Releases Repo - http://devsite.ctr-electronics.com/maven/release/ - - - - revrobotics-release - Rev Robotics Repo - https://maven.revrobotics.com/ - - - - local-wpilib-install - Local copy of libraries from wpilib installation - file://${user.home}/wpilib/2022/maven - - - - diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..48c039e --- /dev/null +++ b/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2023' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..785b8a9 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,35 @@ +{ + "fileName": "NavX.json", + "name": "KauaiLabs_navX_FRC", + "version": "2023.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2023/" + ], + "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2023.0.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2023.0.1", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..630eab0 --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,423 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.30.3", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.3" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.3", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.3", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.3", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.3", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.3", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.3", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..ecf0636 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,55 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "0.4.0", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "0.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "0.4.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..4ac44b0 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.2", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.2", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.2", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..bd535bf --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file From 7baddb8b803a10650d2aff7acbb2012c8f7b5f55 Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:42:02 -0400 Subject: [PATCH 07/28] made LimeLightVision method package-private so it is accessible through tests --- .../org/usfirst/frc4048/common/limelight/LimeLightVision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java index 7f427f5..8ceafd2 100644 --- a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java +++ b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java @@ -128,7 +128,7 @@ public double getStream() { /** * Internal utility to calculate the distances, in inches, from the camera based off of the viewed angles */ - private CameraDistance calcCameraDistance(double angleX, double angleY) { + CameraDistance calcCameraDistance(double angleX, double angleY) { double forwardDistance = (relativeHeight) / Math.tan(Math.toRadians(cameraAngle + angleY)); double sidewaysDistance = forwardDistance * Math.tan(Math.toRadians(angleX)); From 4df03c25357421bfee21ed1ec4a8195bf5199079 Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 15:43:51 -0400 Subject: [PATCH 08/28] fixed shuffleboard problem with support for the new network tables --- .../common/smartshuffleboard/SmartShuffleboardTab.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java index 94cdfa5..4345e52 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java @@ -7,6 +7,7 @@ package org.usfirst.frc4048.common.smartshuffleboard; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.*; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.networktables.NetworkTableEntry; @@ -47,7 +48,7 @@ public void put(String fieldName, Object value) //primitive SimpleWidget widget = widgetMap.get(fieldName); if (widget != null) { - NetworkTableEntry ntEntry= widget.getEntry(); + GenericEntry ntEntry= widget.getEntry(); ntEntry.setValue(value); } else @@ -70,7 +71,7 @@ public void put(String fieldName, String layoutName, Object value) //primitive SimpleWidget widget = widgetMap.get(fieldName); if (widget != null) { - NetworkTableEntry ntEntry= widget.getEntry(); + GenericEntry ntEntry= widget.getEntry(); ntEntry.setValue(value); } else From 3df2e56f3081976d90cee43de7604d9ff01e5280 Mon Sep 17 00:00:00 2001 From: Noah Date: Mon, 9 Oct 2023 16:07:00 -0400 Subject: [PATCH 09/28] fixed errors with changing shuffleboard and move to gradle --- build.gradle | 3 + .../frc4048/common/diag/DiagBoolean.java | 13 +++-- .../frc4048/common/diag/DiagColorSensor.java | 13 +++-- .../common/diag/DiagDistanceTraveled.java | 23 +++++--- .../frc4048/common/diag/DiagEncoder.java | 6 +- .../frc4048/common/diag/DiagMinMax.java | 13 +++-- .../usfirst/frc4048/common/diag/DiagNavX.java | 8 +-- .../common/diag/DiagOpticalRangeFinder.java | 4 +- .../common/diag/DiagOpticalSensor.java | 4 +- .../frc4048/common/diag/DiagPigeon.java | 8 +-- .../usfirst/frc4048/common/diag/DiagPot.java | 8 +-- .../frc4048/common/diag/DiagSonar.java | 4 +- .../common/diag/DiagSparkMaxEncoder.java | 11 ++-- .../diag/DiagSwerveEnclosureSparkMAX.java | 6 +- .../diag/DiagSwerveEnclosureTalonSRX.java | 6 +- .../frc4048/common/diag/DiagSwitch.java | 4 +- .../common/diag/DiagTalonSrxEncoder.java | 39 +++++-------- .../common/diag/DiagTalonSrxSwitch.java | 33 ++++------- .../frc4048/common/diag/Diagnosable.java | 2 +- .../frc4048/common/diag/Diagnostics.java | 5 +- .../frc4048/common/logging/Logging.java | 6 +- .../smartshuffleboard/SmartShuffleboard.java | 15 ++--- .../SmartShuffleboardTab.java | 57 ++++++++++++++++--- .../usfirst/frc4048/common/util/LedPanel.java | 2 + .../frc4048/common/util/MotorUtils.java | 2 +- .../frc4048/common/diag/TestDiagEncoder.java | 4 +- .../diag/TestDiagOpticalRangeFinder.java | 4 +- .../common/diag/TestDiagOpticalSensor.java | 4 +- .../frc4048/common/diag/TestDiagPigeon.java | 4 +- .../frc4048/common/diag/TestDiagPot.java | 4 +- .../frc4048/common/diag/TestDiagSonar.java | 4 +- .../common/diag/TestDiagSwerveEnclosure.java | 4 +- .../frc4048/common/diag/TestDiagSwitch.java | 4 +- 33 files changed, 180 insertions(+), 147 deletions(-) diff --git a/build.gradle b/build.gradle index 3fcd0d3..15546ce 100644 --- a/build.gradle +++ b/build.gradle @@ -69,6 +69,9 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + // https://mvnrepository.com/artifact/org.mockito/mockito-core + testImplementation 'org.mockito:mockito-core:2.1.0' + } test { diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagBoolean.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagBoolean.java index dc52e0f..3ea626f 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagBoolean.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagBoolean.java @@ -1,6 +1,7 @@ package org.usfirst.frc4048.common.diag; -import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; /** @@ -9,8 +10,9 @@ */ public abstract class DiagBoolean implements Diagnosable { + private String title; private String name; - private NetworkTableEntry networkTableEntry; + private GenericEntry networkTableEntry; private boolean seenFalse; private boolean seenTrue; @@ -20,15 +22,16 @@ public abstract class DiagBoolean implements Diagnosable { * * @param name the name of the unit. Will be used on the Shuffleboard */ - public DiagBoolean(String name) { + public DiagBoolean(String title, String name) { + this.title = title; this.name = name; reset(); } @Override - public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab) { - networkTableEntry = shuffleBoardTab.add(name, false).getEntry(); + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = shuffleBoardTab.getLayout(title, BuiltInLayouts.kList).withSize(width, height).add(name, false).getEntry(); } @Override diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java index 09b6e2d..50a279c 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java @@ -7,7 +7,8 @@ package org.usfirst.frc4048.common.diag; -import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import org.usfirst.frc4048.common.util.ColorSensor; @@ -21,19 +22,21 @@ public class DiagColorSensor implements Diagnosable { private ColorSensor colorsensor; private String name; - private NetworkTableEntry networkTableEntry; + private String title; + private GenericEntry networkTableEntry; private Map colorMap; - public DiagColorSensor(String name, ColorSensor colorsensor) { + public DiagColorSensor(String name, String title, ColorSensor colorsensor) { this.name = name; + this.title = title; this.colorsensor = colorsensor; colorMap = new HashMap(); reset(); } @Override - public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab) { - networkTableEntry = shuffleBoardTab.add(name, false).getEntry(); + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = shuffleBoardTab.getLayout(title, BuiltInLayouts.kList).withSize(width, height).add(name, false).getEntry(); } @Override diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagDistanceTraveled.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagDistanceTraveled.java index c238d44..debb343 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagDistanceTraveled.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagDistanceTraveled.java @@ -1,6 +1,7 @@ package org.usfirst.frc4048.common.diag; -import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; /** @@ -10,22 +11,26 @@ public abstract class DiagDistanceTraveled implements Diagnosable { protected String name; - protected int requiredTravel; - protected NetworkTableEntry networkTableEntry; - private int initialValue; + protected String title; + protected double requiredTravel; + protected GenericEntry networkTableEntry; + private double initialValue; private boolean traveledDistance; - public DiagDistanceTraveled(String name, int requiredTravel) { + public DiagDistanceTraveled(String title, String name, double requiredTravel) { + this.title = title; this.name = name; this.requiredTravel = requiredTravel; } @Override - public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab) { - networkTableEntry = shuffleBoardTab.add(name, false).getEntry(); + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = shuffleBoardTab.getLayout(title, BuiltInLayouts.kList).withSize(width, height).add(name, false).getEntry(); //getLayout(title, BuiltInLayouts.kList) } + + @Override public void refresh() { if (networkTableEntry != null) { @@ -40,7 +45,7 @@ public void reset() { } boolean getDiagResult() { - int currentValue = getCurrentValue(); + double currentValue = getCurrentValue(); if (Math.abs(currentValue - initialValue) >= requiredTravel) { traveledDistance = true; @@ -49,5 +54,5 @@ boolean getDiagResult() { return this.traveledDistance; } - protected abstract int getCurrentValue(); + protected abstract double getCurrentValue(); } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagEncoder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagEncoder.java index 23637c6..b0a9d2e 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagEncoder.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagEncoder.java @@ -17,14 +17,14 @@ public class DiagEncoder extends DiagDistanceTraveled { * @param requiredTravel - the required difference between the initial position to qualify for success * @param encoder - the encoder instance to test */ - public DiagEncoder(String name, int requiredTravel, Encoder encoder) { - super(name, requiredTravel); + public DiagEncoder(String title, String name, double requiredTravel, Encoder encoder) { + super(title, name, requiredTravel); this.encoder = encoder; reset(); } @Override - protected int getCurrentValue() { + protected double getCurrentValue() { return encoder.get(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagMinMax.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagMinMax.java index 3a0b089..4cebe95 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagMinMax.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagMinMax.java @@ -1,6 +1,7 @@ package org.usfirst.frc4048.common.diag; -import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; /** @@ -10,9 +11,10 @@ public abstract class DiagMinMax implements Diagnosable { private String name; + private String title; private double minValue; private double maxValue; - private NetworkTableEntry networkTableEntry; + private GenericEntry networkTableEntry; private boolean seenMinValue; private boolean seenMaxValue; @@ -24,8 +26,9 @@ public abstract class DiagMinMax implements Diagnosable { * @param minValue - the minimum value the object needs to hit to qualify for success * @param maxValue - the maximum value the object needs to hit to qualify for success */ - public DiagMinMax(String name, double minValue, double maxValue) { + public DiagMinMax(String title, String name, double minValue, double maxValue) { this.name = name; + this.title = title; this.minValue = minValue; this.maxValue = maxValue; @@ -33,8 +36,8 @@ public DiagMinMax(String name, double minValue, double maxValue) { } @Override - public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab) { - networkTableEntry = shuffleBoardTab.add(name, false).getEntry(); + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = shuffleBoardTab.getLayout(title, BuiltInLayouts.kList).withSize(width, height).add(name, false).getEntry(); } @Override diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagNavX.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagNavX.java index fd1b30a..b07a360 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagNavX.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagNavX.java @@ -15,15 +15,15 @@ public class DiagNavX extends DiagDistanceTraveled { private AHRS navX; - public DiagNavX(String name, int requiredTravel, AHRS navX) { - super(name, requiredTravel); + public DiagNavX(String title, String name, double requiredTravel, AHRS navX) { + super(title, name, requiredTravel); this.navX = navX; reset(); } @Override - protected int getCurrentValue() { - return (int)navX.getAngle(); + protected double getCurrentValue() { + return navX.getAngle(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalRangeFinder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalRangeFinder.java index a956272..815ee0c 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalRangeFinder.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalRangeFinder.java @@ -20,8 +20,8 @@ public class DiagOpticalRangeFinder extends DiagMinMax { * @param minDistance -The minimum testing distance for the optical range finder; the one that will be tested against. * @param maxDistance -The maximum testing distance for the optical range finder; the one that will be tested against. */ - public DiagOpticalRangeFinder(String name, OpticalRangeFinder opticalRangeFinder, double minDistance, double maxDistance){ - super(name, minDistance, maxDistance); + public DiagOpticalRangeFinder(String tittle, String name, OpticalRangeFinder opticalRangeFinder, double minDistance, double maxDistance){ + super(tittle,name, minDistance, maxDistance); this.opticalRangeFinder = opticalRangeFinder; } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalSensor.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalSensor.java index bc4bd0a..5e109d0 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalSensor.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagOpticalSensor.java @@ -15,8 +15,8 @@ public class DiagOpticalSensor extends DiagBoolean { * @param name - The sensor's name, which will be shown on Shuffleboard * @param digitalInput - The DigitalInput pin the sensor is connected to */ - public DiagOpticalSensor(String name, DigitalInput digitalInput){ - super(name); + public DiagOpticalSensor(String title, String name, DigitalInput digitalInput){ + super(title, name); this.digitalInput = digitalInput; } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagPigeon.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagPigeon.java index 0283f99..f27a451 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagPigeon.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagPigeon.java @@ -6,15 +6,15 @@ public class DiagPigeon extends DiagDistanceTraveled { private PigeonIMU pigeon; - public DiagPigeon(String name, int requiredTravel, PigeonIMU pigeon) { - super(name, requiredTravel); + public DiagPigeon(String title,String name, int requiredTravel, PigeonIMU pigeon) { + super(title, name, requiredTravel); this.pigeon = pigeon; reset(); } @Override - protected int getCurrentValue() { - return (int)pigeon.getFusedHeading(); + protected double getCurrentValue() { + return (double)pigeon.getFusedHeading(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagPot.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagPot.java index 118b51e..d824831 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagPot.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagPot.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.AnalogPotentiometer; /** - * A diagnostics class for analog potentiometer. It is a DiagMinMax object. + * A diagnostics class for analog potentiometer. It is a DiagMinMax object. * Give it the maximum and minimum voltages for testing with. */ public class DiagPot extends DiagMinMax { @@ -20,8 +20,8 @@ public class DiagPot extends DiagMinMax { * @param maxVoltage - the maximum value the pot needs to hit to qualify for success * @param pot - the pot instance to test */ - public DiagPot(String name, double minVoltage, double maxVoltage, AnalogPotentiometer pot) { - super(name, minVoltage, maxVoltage); + public DiagPot(String title, String name, double minVoltage, double maxVoltage, AnalogPotentiometer pot) { + super(title, name, minVoltage, maxVoltage); this.pot = pot; } @@ -29,4 +29,4 @@ public DiagPot(String name, double minVoltage, double maxVoltage, AnalogPotentio double getSensorReading(){ return pot.get(); } -} +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSonar.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSonar.java index 6635ada..f896636 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagSonar.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSonar.java @@ -18,8 +18,8 @@ public class DiagSonar extends DiagMinMax { * @param minDistance -The minimum testing distance for the sonar; the one that will be tested against. * @param maxDistance -The maximum testing distance for the sonar; the one that will be tested against. */ - public DiagSonar(String name, Ultrasonic sonar, double minDistance, double maxDistance){ - super(name, minDistance, maxDistance); + public DiagSonar(String title, String name, Ultrasonic sonar, double minDistance, double maxDistance){ + super(title, name, minDistance, maxDistance); this.sonar = sonar; } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxEncoder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxEncoder.java index fcf6bc0..a9d5264 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxEncoder.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSparkMaxEncoder.java @@ -3,8 +3,7 @@ import com.revrobotics.CANSparkMax; /** - * A diagnostics class for digital encoder that is connected directly to the SparkMAX. - * The diagnostics will turn green once the encoder has traveled at least a given + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has traveled at least a given * distance from its initial position (measured at initialization or after a reset) */ public class DiagSparkMaxEncoder extends DiagDistanceTraveled { @@ -18,14 +17,14 @@ public class DiagSparkMaxEncoder extends DiagDistanceTraveled { * @param requiredTravel - the required difference between the initial position to qualify for success * @param canSparkMax - the encoder instance to test */ - public DiagSparkMaxEncoder(String name, int requiredTravel, CANSparkMax canSparkMax) { - super(name, requiredTravel); + public DiagSparkMaxEncoder(String title, String name, double requiredTravel, CANSparkMax canSparkMax) { + super(title, name, requiredTravel); this.canSparkMax = canSparkMax; reset(); } @Override - protected int getCurrentValue() { - return (int)canSparkMax.getEncoder().getPosition(); + protected double getCurrentValue() { + return canSparkMax.getEncoder().getPosition(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureSparkMAX.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureSparkMAX.java index d41af16..b09c1b8 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureSparkMAX.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureSparkMAX.java @@ -6,13 +6,13 @@ public class DiagSwerveEnclosureSparkMAX extends DiagDistanceTraveled { private SparkMAXSwerveEnclosure enclosure; - public DiagSwerveEnclosureSparkMAX(String name, int requiredTravel, SparkMAXSwerveEnclosure enclosure) { - super(name, requiredTravel); + public DiagSwerveEnclosureSparkMAX(String title,String name, int requiredTravel, SparkMAXSwerveEnclosure enclosure) { + super(title,name, requiredTravel); this.enclosure = enclosure; } @Override - protected int getCurrentValue() { + protected double getCurrentValue() { return enclosure.getLastEncPosition(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureTalonSRX.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureTalonSRX.java index a536e99..858f55c 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureTalonSRX.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwerveEnclosureTalonSRX.java @@ -6,13 +6,13 @@ public class DiagSwerveEnclosureTalonSRX extends DiagDistanceTraveled { private CanTalonSwerveEnclosure enclosure; - public DiagSwerveEnclosureTalonSRX(String name, int requiredTravel, CanTalonSwerveEnclosure enclosure) { - super(name, requiredTravel); + public DiagSwerveEnclosureTalonSRX(String tittle, String name, int requiredTravel, CanTalonSwerveEnclosure enclosure) { + super(tittle,name, requiredTravel); this.enclosure = enclosure; } @Override - protected int getCurrentValue() { + protected double getCurrentValue() { return enclosure.getLastEncPosition(); } } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwitch.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwitch.java index daa1b96..82e435e 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagSwitch.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagSwitch.java @@ -15,8 +15,8 @@ public class DiagSwitch extends DiagBoolean { * @param name the name of the unit. Will be used on the Shuffleboard * @param digitalInput - the DigitalInput the switch is connected to */ - public DiagSwitch(String name, DigitalInput digitalInput) { - super(name); + public DiagSwitch(String title, String name, DigitalInput digitalInput) { + super(title, name); this.digitalInput = digitalInput; } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxEncoder.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxEncoder.java index 17d9cce..9300e58 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxEncoder.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxEncoder.java @@ -8,38 +8,27 @@ * distance from its initial position (measured at initialization or after a reset) */ public class DiagTalonSrxEncoder extends DiagDistanceTraveled { - public DiagTalonSrxEncoder(String name, int requiredTravel) { - super(name, requiredTravel); + public DiagTalonSrxEncoder(String title, String name, double requiredTravel) { + super(title, name, requiredTravel); } - @Override - protected int getCurrentValue() { - return 0; - } - - /*** - * The following code is commented out since the WPI_TalonSRX has a build issue (It is using - * Sendable from the wrong package). - * - - private WPI_TalonSRX talonSRX; - - /** - * Constructor - * - * @param name - the name of the unit. Will be used on the Shuffleboard - * @param requiredTravel - the required difference between the initial position to qualify for success - * @param talonSRX - the encoder instance to test - * / - public DiagTalonSrxEncoder(String name, int requiredTravel, WPI_TalonSRX talonSRX) { - super(name, requiredTravel); + private WPI_TalonSRX talonSRX; + + /* + Constructor + + @param name - the name of the unit. Will be used on the Shuffleboard + @param requiredTravel - the required difference between the initial position to qualify for success + @param talonSRX - the encoder instance to test + */ + public DiagTalonSrxEncoder(String title, String name, double requiredTravel, WPI_TalonSRX talonSRX) { + super(title, name, requiredTravel); this.talonSRX = talonSRX; reset(); } @Override - protected int getCurrentValue() { + protected double getCurrentValue() { return talonSRX.getSelectedSensorPosition(); } - ***/ } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxSwitch.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxSwitch.java index e06d582..1810961 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxSwitch.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagTalonSrxSwitch.java @@ -7,37 +7,26 @@ * it is a DiagBoolean subclass. */ public class DiagTalonSrxSwitch extends DiagBoolean { - public DiagTalonSrxSwitch(String name) { - super(name); + public DiagTalonSrxSwitch(String title, String name) { + super(title, name); } - @Override - protected boolean getValue() { - return false; - } - - /*** - * The following code is commented out since the WPI_TalonSRX has a build issue (It is using - * Sendable from the wrong package). - * - - public enum Direction {FORWARD, REVERSE}; - + public enum Direction {FORWARD, REVERSE}; private WPI_TalonSRX talonSRX; private Direction direction; - / ** + /* * Constructor * - * @param name the name of the unit. Will be used on the Shuffleboard - * @param talonSRX the talon SRX to read the switch value from - * / - public DiagTalonSrxSwitch(String name, WPI_TalonSRX talonSRX, Direction direction) { - super(name); + * @param name -the name of the unit. Will be used on the Shuffleboard + * @param talonSRX -the talon SRX to read the switch value from + */ + + public DiagTalonSrxSwitch(String title, String name, WPI_TalonSRX talonSRX, Direction direction) { + super(title, name); this.talonSRX = talonSRX; this.direction = direction; } - @Override protected boolean getValue() { switch (direction) { @@ -49,5 +38,5 @@ protected boolean getValue() { return false; } } - ***/ + } diff --git a/src/main/java/org/usfirst/frc4048/common/diag/Diagnosable.java b/src/main/java/org/usfirst/frc4048/common/diag/Diagnosable.java index 5d54e69..d454222 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/Diagnosable.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/Diagnosable.java @@ -8,7 +8,7 @@ public interface Diagnosable { * Set the tab for the diagnosable (this is done by the diagnostics infrastructure) * @param shuffleBoardTab the tab to set */ - void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab); + void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height); /** * A method called periodically that will test the diagnosable value and update the display diff --git a/src/main/java/org/usfirst/frc4048/common/diag/Diagnostics.java b/src/main/java/org/usfirst/frc4048/common/diag/Diagnostics.java index 82776dc..8a00431 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/Diagnostics.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/Diagnostics.java @@ -16,6 +16,7 @@ public class Diagnostics extends SubsystemBase { public static final String SHUFFLEBOARD_TAB_NAME = "Diagnostics"; + public static final int width = 1, height = 4; private ShuffleboardTab shuffleBoardTab; @@ -30,7 +31,6 @@ public Diagnostics() { addDiagnosable(new DiagSwitch("Switch1", new DigitalInput(1), shuffleBoardTab)); addDiagnosable(new DiagPot("Pot1", 0.1, 0.9, new AnalogPotentiometer(0), shuffleBoardTab)); addDiagnosable(new DiagEncoder("Encoder1", 100, new Encoder(5,6), shuffleBoardTab)); - addDiagnosable(new DiagOpticalSensor("OpticalSensor1", new DigitalInput(2), shuffleBoardTab)); addDiagnosable(new DiagOpticalRangeFinder("OpticalRangeFinder1", new OpticalRangeFinder(new AnalogInput(1)), shuffleBoardTab, 3.0, 12.0)); addDiagnosable(new DiagSonar("Sonar1", new Ultrasonic(3, 4), shuffleBoardTab, 3.0, 12.0)); @@ -38,10 +38,11 @@ public Diagnostics() { } public void addDiagnosable(Diagnosable diagnosable) { - diagnosable.setShuffleBoardTab(shuffleBoardTab); + diagnosable.setShuffleBoardTab(shuffleBoardTab, width, height); diagnosables.add(diagnosable); } + /** * Refresh the display with current values. * This would go through the range of components and test their state, and then refresh the shuffleboard display diff --git a/src/main/java/org/usfirst/frc4048/common/logging/Logging.java b/src/main/java/org/usfirst/frc4048/common/logging/Logging.java index c8ee87c..821db9a 100644 --- a/src/main/java/org/usfirst/frc4048/common/logging/Logging.java +++ b/src/main/java/org/usfirst/frc4048/common/logging/Logging.java @@ -119,12 +119,12 @@ private final void writeTitles() { private final void writeData() { counter += 1; - if ((DriverStation.getInstance().isEnabled() && (counter % LOGGING_FREQ == 0)) || writeTitles) { + if ((DriverStation.isEnabled() && (counter % LOGGING_FREQ == 0)) || writeTitles) { final double now = Timer.getFPGATimestamp(); sb.setLength(0); sb.append(df3.format(now)); sb.append(COMMA); - if (DriverStation.getInstance().isDisabled()) + if (DriverStation.isDisabled()) sb.append(0); else sb.append(df3.format(now - startTime)); @@ -187,7 +187,7 @@ public void traceMessage(MessageLevel ml, String... vals) { final StringBuilder sb = new StringBuilder(); sb.append(df3.format(now)); sb.append(COMMA); - if (DriverStation.getInstance().isDisabled()) + if (DriverStation.isDisabled()) sb.append(0); else sb.append(df3.format(now - startTime)); diff --git a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java index ac9a48f..2e20106 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboard.java @@ -1,13 +1,10 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. package org.usfirst.frc4048.common.smartshuffleboard; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.networktables.NetworkTableValue; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -21,13 +18,13 @@ * Adding and updating a value is done in the same manner, and there's no need to handle networkTable entries. * Data can be displayed in its own widget or items can be grouped into a list widget. * Example: - * + * * tab name list name field name value * SmartShuffleboard.put("Drive", "encoders", "Front Right", steerFR.getSelectedSensorPosition(0)); * SmartShuffleboard.put("Drive", "encoders", "Front Left", steerFL.getSelectedSensorPosition(0)); * SmartShuffleboard.put("Drive", "encoders", "Rear Right", steerRR.getSelectedSensorPosition(0)); * SmartShuffleboard.put("Drive", "encoders", "Rear Left", steerRL.getSelectedSensorPosition(0)); - * + * * tab name field name value * SmartShuffleboard.put("Drive", "Gyro", getGyro()); */ diff --git a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java index 4345e52..af9e8cf 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java @@ -8,9 +8,9 @@ package org.usfirst.frc4048.common.smartshuffleboard; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.NetworkTableValue; import edu.wpi.first.wpilibj.shuffleboard.*; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.networktables.NetworkTableEntry; import java.util.*; @@ -21,12 +21,12 @@ public class SmartShuffleboardTab { private Map widgetMap = new HashMap(); private Set commandSet = new HashSet<>(); private ShuffleboardTab tab; - - SmartShuffleboardTab(String tabName) + + SmartShuffleboardTab(String tabName) { tab = Shuffleboard.getTab(tabName); } - + public SimpleWidget getWidget(String fieldName) // return widget handle { return widgetMap.get(fieldName); @@ -36,14 +36,14 @@ public ShuffleboardLayout getLayout(String layoutName) // return layout hand { try { return tab.getLayout(layoutName); - } + } catch (Exception noSuchElementException) { return null; } } - - public void put(String fieldName, Object value) //primitive + + public SimpleWidget put(String fieldName, Object value) //primitive { SimpleWidget widget = widgetMap.get(fieldName); if (widget != null) @@ -56,9 +56,10 @@ public void put(String fieldName, Object value) //primitive widget = tab.add(fieldName, value); widgetMap.put(fieldName, widget); } + return widget; } - - public void put(String fieldName, String layoutName, Object value) //primitive + + public SimpleWidget put(String fieldName, String layoutName, Object value) //primitive { ShuffleboardLayout layout; try { @@ -79,7 +80,45 @@ public void put(String fieldName, String layoutName, Object value) //primitive widget = layout.add(fieldName, value); widgetMap.put(fieldName, widget); } + return widget; + } + + public boolean getBoolean(String fieldName, boolean defaultValue) { + SimpleWidget widget = widgetMap.get(fieldName); + if (widget == null) { + return defaultValue; + } else { + return widget.getEntry().getBoolean(defaultValue); + } } + + public double getDouble(String fieldName, double defaultValue) { + SimpleWidget widget = widgetMap.get(fieldName); + if (widget == null) { + return defaultValue; + } else { + return widget.getEntry().getDouble(defaultValue); + } + } + + public String getString(String fieldName, String defaultValue) { + SimpleWidget widget = widgetMap.get(fieldName); + if (widget == null) { + return defaultValue; + } else { + return widget.getEntry().getString(defaultValue); + } + } + + public NetworkTableValue getValue(String fieldName) { + SimpleWidget widget = widgetMap.get(fieldName); + if (widget == null) { + return null; + } else { + return widget.getEntry().get(); + } + } + public void putCommand(String fieldName, CommandBase cmd) { if (!commandSet.contains(fieldName)) diff --git a/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java index 1a147b1..119c3d2 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java +++ b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java @@ -1,5 +1,7 @@ package org.usfirst.frc4048.common.util; +import edu.wpi.first.wpilibj.DigitalOutput; + public class LedPanel { public enum PicID { ZERO(false,false,false), diff --git a/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java b/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java index be8404d..4122197 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java +++ b/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.Timer; -import frc.robot.utils.logging.Logging; +import org.usfirst.frc4048.common.logging.Logging; /* * MotorStall object should be instantiated in init() method of a command diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagEncoder.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagEncoder.java index 0e3de71..fbba28c 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagEncoder.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagEncoder.java @@ -14,7 +14,7 @@ public void testPotInitially() throws Exception { Encoder mockEncoder = Mockito.mock(Encoder.class); when(mockEncoder.get()).thenReturn(1); - DiagEncoder classUnderTest = new DiagEncoder("encoder", 100, mockEncoder); + DiagEncoder classUnderTest = new DiagEncoder("","encoder", 100, mockEncoder); when(mockEncoder.get()).thenReturn(51); Assert.assertFalse(classUnderTest.getDiagResult()); @@ -34,7 +34,7 @@ public void testEncoderAfterReset() throws Exception { Encoder mockEncoder = Mockito.mock(Encoder.class); when(mockEncoder.get()).thenReturn(1); - DiagEncoder classUnderTest = new DiagEncoder("encoder", 100, mockEncoder); + DiagEncoder classUnderTest = new DiagEncoder("Diags","encoder", 100, mockEncoder); when(mockEncoder.get()).thenReturn(101); Assert.assertTrue(classUnderTest.getDiagResult()); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalRangeFinder.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalRangeFinder.java index 92f1790..fe034e6 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalRangeFinder.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalRangeFinder.java @@ -13,7 +13,7 @@ public class TestDiagOpticalRangeFinder{ public void testOpticalRangeFinderInitially() throws Exception { OpticalRangeFinder mockOpticalRangeFinder = Mockito.mock(OpticalRangeFinder.class); - DiagOpticalRangeFinder classUnderTest = new DiagOpticalRangeFinder("Optical Range Finder", mockOpticalRangeFinder,3.0, 12.0); + DiagOpticalRangeFinder classUnderTest = new DiagOpticalRangeFinder("Diags","Optical Range Finder", mockOpticalRangeFinder,3.0, 12.0); when(mockOpticalRangeFinder.getDistanceInInches()).thenReturn(7.0); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); @@ -29,7 +29,7 @@ public void testOpticalRangeFinderInitially() throws Exception { public void testOpticalRangeFinderAfterReset() throws Exception { OpticalRangeFinder mockOpticalRangeFinder = Mockito.mock(OpticalRangeFinder.class); - DiagOpticalRangeFinder classUnderTest = new DiagOpticalRangeFinder("Optical Range Finder", mockOpticalRangeFinder, 3.0, 12.0); + DiagOpticalRangeFinder classUnderTest = new DiagOpticalRangeFinder("Diags","Optical Range Finder", mockOpticalRangeFinder, 3.0, 12.0); when(mockOpticalRangeFinder.getDistanceInInches()).thenReturn(7.0); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalSensor.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalSensor.java index 452207e..2ca3a9a 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalSensor.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagOpticalSensor.java @@ -13,7 +13,7 @@ public class TestDiagOpticalSensor{ public void testOpticalSensorInitially() throws Exception { DigitalInput mockInput = Mockito.mock(DigitalInput.class); - DiagOpticalSensor classUnderTest = new DiagOpticalSensor("Optical Sensor", mockInput); + DiagOpticalSensor classUnderTest = new DiagOpticalSensor("Diags","Optical Sensor", mockInput); when(mockInput.get()).thenReturn(true); Assert.assertFalse(classUnderTest.getDiagResult()); @@ -26,7 +26,7 @@ public void testOpticalSensorInitially() throws Exception { public void testOpticalSensorAfterReset() throws Exception { DigitalInput mockInput = Mockito.mock(DigitalInput.class); - DiagOpticalSensor classUnderTest = new DiagOpticalSensor("Optical Sensor", mockInput); + DiagOpticalSensor classUnderTest = new DiagOpticalSensor("Diags","Optical Sensor", mockInput); when(mockInput.get()).thenReturn(true); Assert.assertFalse(classUnderTest.getDiagResult()); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPigeon.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPigeon.java index d19aa06..7ca6969 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPigeon.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPigeon.java @@ -14,7 +14,7 @@ public void testPigeonInitially() throws Exception { PigeonIMU mockPigeon = Mockito.mock(PigeonIMU.class); when(mockPigeon.getFusedHeading()).thenReturn(1.0); - DiagPigeon classUnderTest = new DiagPigeon("pigeon", 100, mockPigeon); + DiagPigeon classUnderTest = new DiagPigeon("Diags","pigeon", 100, mockPigeon); when(mockPigeon.getFusedHeading()).thenReturn(51.0); Assert.assertFalse(classUnderTest.getDiagResult()); @@ -34,7 +34,7 @@ public void testPigeonAfterReset() throws Exception { PigeonIMU mockPigeon = Mockito.mock(PigeonIMU.class); when(mockPigeon.getFusedHeading()).thenReturn(1.0); - DiagPigeon classUnderTest = new DiagPigeon("pigeon", 100, mockPigeon); + DiagPigeon classUnderTest = new DiagPigeon("Diags","pigeon", 100, mockPigeon); when(mockPigeon.getFusedHeading()).thenReturn(101.0); Assert.assertTrue(classUnderTest.getDiagResult()); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPot.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPot.java index bfa584e..4cbb097 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPot.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagPot.java @@ -13,7 +13,7 @@ public class TestDiagPot { public void testPotInitially() throws Exception { AnalogPotentiometer mockPot = Mockito.mock(AnalogPotentiometer.class); - DiagPot classUnderTest = new DiagPot("pot", 1.0, 2.0, mockPot); + DiagPot classUnderTest = new DiagPot("Diags","pot", 1.0, 2.0, mockPot); when(mockPot.get()).thenReturn(1.5); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); @@ -29,7 +29,7 @@ public void testPotInitially() throws Exception { public void testPotAfterReset() throws Exception { AnalogPotentiometer mockPot = Mockito.mock(AnalogPotentiometer.class); - DiagPot classUnderTest = new DiagPot("pot", 1.0, 2.0,mockPot); + DiagPot classUnderTest = new DiagPot("Diags","pot", 1.0, 2.0,mockPot); when(mockPot.get()).thenReturn(1.0); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSonar.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSonar.java index cf03be4..2c7d1f9 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSonar.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSonar.java @@ -14,7 +14,7 @@ public class TestDiagSonar{ public void testSonarInitially() throws Exception { Ultrasonic mockUltasonic = Mockito.mock(Ultrasonic.class); - DiagSonar classUnderTest = new DiagSonar("switch", mockUltasonic, 3.0, 12.0); + DiagSonar classUnderTest = new DiagSonar("Diags","switch", mockUltasonic, 3.0, 12.0); when(mockUltasonic.getRangeInches()).thenReturn(7.0); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); @@ -30,7 +30,7 @@ public void testSonarInitially() throws Exception { public void testSonarAfterReset() throws Exception { Ultrasonic mockUltasonic = Mockito.mock(Ultrasonic.class); - DiagSonar classUnderTest = new DiagSonar("switch", mockUltasonic, 3.0, 12.0); + DiagSonar classUnderTest = new DiagSonar("Diags","switch", mockUltasonic, 3.0, 12.0); when(mockUltasonic.getRangeInches()).thenReturn(7.0); Assert.assertFalse(classUnderTest.getDiagResult(classUnderTest.getSensorReading())); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwerveEnclosure.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwerveEnclosure.java index 11c71d5..8e0c046 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwerveEnclosure.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwerveEnclosure.java @@ -14,7 +14,7 @@ public void testSwerveInitially() throws Exception { SparkMAXSwerveEnclosure mockEnclosure = Mockito.mock(SparkMAXSwerveEnclosure.class); when(mockEnclosure.getLastEncPosition()).thenReturn(1); - DiagSwerveEnclosureSparkMAX classUnderTest = new DiagSwerveEnclosureSparkMAX("enclosure", 100, mockEnclosure); + DiagSwerveEnclosureSparkMAX classUnderTest = new DiagSwerveEnclosureSparkMAX("Diags","enclosure", 100, mockEnclosure); when(mockEnclosure.getLastEncPosition()).thenReturn(51); Assert.assertFalse(classUnderTest.getDiagResult()); @@ -34,7 +34,7 @@ public void testSwerveAfterReset() throws Exception { SparkMAXSwerveEnclosure mockEnclosure = Mockito.mock(SparkMAXSwerveEnclosure.class); when(mockEnclosure.getLastEncPosition()).thenReturn(1); - DiagSwerveEnclosureSparkMAX classUnderTest = new DiagSwerveEnclosureSparkMAX("enclosure", 100, mockEnclosure); + DiagSwerveEnclosureSparkMAX classUnderTest = new DiagSwerveEnclosureSparkMAX("Diags","enclosure", 100, mockEnclosure); when(mockEnclosure.getLastEncPosition()).thenReturn(101); Assert.assertTrue(classUnderTest.getDiagResult()); diff --git a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwitch.java b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwitch.java index 656f070..2eca59b 100644 --- a/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwitch.java +++ b/src/test/java/org/usfirst/frc4048/common/diag/TestDiagSwitch.java @@ -13,7 +13,7 @@ public class TestDiagSwitch { public void testSwitchInitially() throws Exception { DigitalInput mockInput = Mockito.mock(DigitalInput.class); - DiagSwitch classUnderTest = new DiagSwitch("switch", mockInput); + DiagSwitch classUnderTest = new DiagSwitch("Diags","switch", mockInput); when(mockInput.get()).thenReturn(true); Assert.assertFalse(classUnderTest.getDiagResult()); @@ -26,7 +26,7 @@ public void testSwitchInitially() throws Exception { public void testSwitchAfterReset() throws Exception { DigitalInput mockInput = Mockito.mock(DigitalInput.class); - DiagSwitch classUnderTest = new DiagSwitch("switch", mockInput); + DiagSwitch classUnderTest = new DiagSwitch("Diags","switch", mockInput); when(mockInput.get()).thenReturn(true); Assert.assertFalse(classUnderTest.getDiagResult()); From c0b45c095d8f41d6ec31f2ae7f5d7f3ea8a91ae1 Mon Sep 17 00:00:00 2001 From: Noah Date: Sun, 5 Nov 2023 19:35:52 -0500 Subject: [PATCH 10/28] added swerve to common code (not tested and a lot of abstraction, need a second opinion) --- .../common/swervev2/SwerveDrivetrain.java | 80 +++++++++++++++++++ .../components/EncodedSwerveSparkMax.java | 10 +++ .../components/GenericEncodedSwerve.java | 79 ++++++++++++++++++ .../swervev2/components/SwerveMotor.java | 8 ++ .../components/SwerveMotorEncoder.java | 21 +++++ .../swervev2/type/GenericSwerveModule.java | 65 +++++++++++++++ .../common/swervev2/type/PidSwerve.java | 8 ++ .../swervev2/type/SparkMaxSwerveModule.java | 12 +++ .../common/swervev2/type/StatedSwerve.java | 10 +++ .../org/usfirst/frc4048/common/util/Gain.java | 23 ++++++ .../org/usfirst/frc4048/common/util/PID.java | 29 +++++++ 11 files changed, 345 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotor.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/type/GenericSwerveModule.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/type/PidSwerve.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/type/SparkMaxSwerveModule.java create mode 100644 src/main/java/org/usfirst/frc4048/common/swervev2/type/StatedSwerve.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/Gain.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/PID.java diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java new file mode 100644 index 0000000..be485b8 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java @@ -0,0 +1,80 @@ +package org.usfirst.frc4048.common.swervev2; + +import com.ctre.phoenix.sensors.WPI_CANCoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import org.usfirst.frc4048.common.swervev2.components.EncodedSwerveSparkMax; +import org.usfirst.frc4048.common.swervev2.type.SparkMaxSwerveModule; +import org.usfirst.frc4048.common.util.Gain; +import org.usfirst.frc4048.common.util.PID; + +public class SwerveDrivetrain { + private final CANSparkMax m_frontLeftDrive; + private final CANSparkMax m_frontLeftTurn; + + private final WPI_CANCoder frontLeftCanCoder; + + private final Translation2d m_frontLeftLocation = new Translation2d(0,0); + + private final SparkMaxSwerveModule m_frontLeft; + + + private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation); + + + + public SwerveDrivetrain() { + + m_frontLeftDrive = new CANSparkMax(0, CANSparkMaxLowLevel.MotorType.kBrushless); + + m_frontLeftTurn = new CANSparkMax(0, CANSparkMaxLowLevel.MotorType.kBrushless); + + frontLeftCanCoder = new WPI_CANCoder(0); + double driveVelConvFactor = (2 * 0) / (0 * 60); + double drivePosConvFactor = (2 * 0) / (0); + double steerPosConvFactor = (2 * Math.PI / 0); + EncodedSwerveSparkMax encodedSwerveSparkMax = new EncodedSwerveSparkMax(m_frontLeftDrive, m_frontLeftTurn, frontLeftCanCoder, driveVelConvFactor, drivePosConvFactor, steerPosConvFactor); + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(18 * 4, 2 * Math.PI * 10); + m_frontLeft = new SparkMaxSwerveModule(encodedSwerveSparkMax, PID.of(0,0,0),PID.of(0,0,0), Gain.of(0,0),Gain.of(0,0),constraints); + m_frontLeftDrive.setInverted(true); + } + + + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + SwerveModuleState[] swerveModuleStates = m_kinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, new Rotation2d(Math.toRadians(0))) + : new ChassisSpeeds(xSpeed, ySpeed, rot)); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, 0); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + m_frontLeft.setDesiredState(desiredStates[0]); + } + + + public void stopMotor() { + m_frontLeftTurn.set(0.0); + } + + + public void setPower(double value){ + m_frontLeftTurn.set(value); + } + + public void SetRelEncZero(){ + m_frontLeft.getSwerveMotor().resetRelEnc(); + } + + + public double getRelEnc(){ + return m_frontLeft.getSwerveMotor().getDriveEncPosition(); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java new file mode 100644 index 0000000..efe1220 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java @@ -0,0 +1,10 @@ +package org.usfirst.frc4048.common.swervev2.components; + +import com.ctre.phoenix.sensors.WPI_CANCoder; +import com.revrobotics.CANSparkMax; + +public class EncodedSwerveSparkMax extends GenericEncodedSwerve { + public EncodedSwerveSparkMax(CANSparkMax driveMotor, CANSparkMax steerMotor, WPI_CANCoder absEncoder, double driveVelFactor, double drivePosFactor, double steerPosFactor) { + super(driveMotor, steerMotor, absEncoder, driveMotor.getEncoder(), steerMotor.getEncoder(), driveVelFactor, drivePosFactor, steerPosFactor); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java new file mode 100644 index 0000000..0bd100a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java @@ -0,0 +1,79 @@ +package org.usfirst.frc4048.common.swervev2.components; + +import com.ctre.phoenix.sensors.WPI_CANCoder; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +public class GenericEncodedSwerve implements SwerveMotor, SwerveMotorEncoder { + private final MotorController driveMotor; + private final MotorController steerMotor; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder steerEncoder; + private double steerOffset = 0; + private final WPI_CANCoder absEncoder; + + public GenericEncodedSwerve(MotorController driveMotor, MotorController steerMotor, WPI_CANCoder absEncoder, RelativeEncoder driveEncoder, RelativeEncoder steerEncoder, + double driveVelFactor, double drivePosFactor, double steerPosFactor) { + this.driveMotor = driveMotor; + this.steerMotor = steerMotor; + this.absEncoder = absEncoder; + this.driveEncoder = driveEncoder; + this.steerEncoder = steerEncoder; + resetRelEnc(); + driveEncoder.setVelocityConversionFactor(driveVelFactor); + driveEncoder.setPositionConversionFactor(drivePosFactor); + steerEncoder.setPositionConversionFactor(steerPosFactor); + } + + @Override + public MotorController getSteerMotor() { + return steerMotor; + } + + @Override + public MotorController getDriveMotor() { + return driveMotor; + } + + @Override + public double getSteerEncPosition() { + double value = steerEncoder.getPosition() - getSteerOffset(); + value %= 2 * Math.PI; + if (value < 0) { + value += 2 * Math.PI; + } + return (value); + } + + @Override + public double getDriveEncPosition() { + return driveEncoder.getPosition(); + } + + @Override + public void resetRelEnc() { + steerEncoder.setPosition(0); + driveEncoder.setPosition(0); + } + + @Override + public double getDriveEncVel() { + return driveEncoder.getVelocity(); + } + + @Override + public double getSteerOffset() { + return steerOffset; + } + + @Override + public void setSteerOffset(double zeroAbs) { + steerEncoder.setPosition(0); + steerOffset = Math.toRadians(zeroAbs - absEncoder.getAbsolutePosition()); + steerOffset %= 2 * Math.PI; + if (steerOffset < 0) { + steerOffset += 2 * Math.PI; + } + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotor.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotor.java new file mode 100644 index 0000000..6826cd5 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotor.java @@ -0,0 +1,8 @@ +package org.usfirst.frc4048.common.swervev2.components; + +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +public interface SwerveMotor { + MotorController getSteerMotor(); + MotorController getDriveMotor(); +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java new file mode 100644 index 0000000..7d07194 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java @@ -0,0 +1,21 @@ +package org.usfirst.frc4048.common.swervev2.components; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; + +public interface SwerveMotorEncoder { + double getSteerEncPosition(); + double getDriveEncPosition(); + void resetRelEnc(); + + double getDriveEncVel(); + + double getSteerOffset(); + + void setSteerOffset(double zeroAbs); + + default SwerveModulePosition getPosition(){ + return new SwerveModulePosition(getDriveEncPosition(),new Rotation2d(getSteerEncPosition())); + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/type/GenericSwerveModule.java b/src/main/java/org/usfirst/frc4048/common/swervev2/type/GenericSwerveModule.java new file mode 100644 index 0000000..7233f66 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/type/GenericSwerveModule.java @@ -0,0 +1,65 @@ +package org.usfirst.frc4048.common.swervev2.type; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import org.usfirst.frc4048.common.swervev2.components.GenericEncodedSwerve; +import org.usfirst.frc4048.common.util.Gain; +import org.usfirst.frc4048.common.util.PID; + +public class GenericSwerveModule implements StatedSwerve, PidSwerve { + private final PIDController drivePIDController; + private final ProfiledPIDController turningPIDController; + private final SimpleMotorFeedforward driveFeedforward; + private final SimpleMotorFeedforward turnFeedforward; + private final GenericEncodedSwerve swerveMotor; + + public GenericSwerveModule(GenericEncodedSwerve swerveMotor, PID drivePid, PID turnPid, Gain driveGain, Gain turnGain, TrapezoidProfile.Constraints goalConstraint) { + this.swerveMotor = swerveMotor; + drivePIDController = new PIDController(drivePid.getP(),drivePid.getI(),drivePid.getD()); + turningPIDController = new ProfiledPIDController(turnPid.getP(),turnPid.getI(),turnPid.getD(),goalConstraint); + driveFeedforward = new SimpleMotorFeedforward(driveGain.getS(),driveGain.getV()); + turnFeedforward = new SimpleMotorFeedforward(turnGain.getS(),turnGain.getV()); + turningPIDController.enableContinuousInput(0, Math.PI * 2); + } + + @Override + public SwerveModuleState getState() { + return new SwerveModuleState(swerveMotor.getDriveEncVel(),new Rotation2d(swerveMotor.getSteerEncPosition())); + } + + @Override + public void setDesiredState(SwerveModuleState desiredState) { + SwerveModuleState state = SwerveModuleState.optimize(desiredState, new Rotation2d(swerveMotor.getSteerEncPosition())); + double driveSpeed = calcDrivePidOut(state.speedMetersPerSecond) + calcDriveFeedForward(state.speedMetersPerSecond); + double turnSpeed = calcSteerPidOut(state.angle.getRadians()) + calcSteerFeedForward(); + swerveMotor.getDriveMotor().setVoltage(driveSpeed); + swerveMotor.getSteerMotor().set(turnSpeed); + } + + @Override + public double calcDrivePidOut(double setpoint) { + return drivePIDController.calculate(swerveMotor.getDriveEncVel(), setpoint); + } + + @Override + public double calcSteerPidOut(double goal) { + return turningPIDController.calculate(swerveMotor.getSteerEncPosition(), goal); + } + + @Override + public double calcDriveFeedForward(double velocity) { + return driveFeedforward.calculate(velocity); + } + + @Override + public double calcSteerFeedForward() { + return turnFeedforward.calculate(turningPIDController.getSetpoint().velocity); + } + public GenericEncodedSwerve getSwerveMotor(){ + return swerveMotor; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/type/PidSwerve.java b/src/main/java/org/usfirst/frc4048/common/swervev2/type/PidSwerve.java new file mode 100644 index 0000000..a15685a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/type/PidSwerve.java @@ -0,0 +1,8 @@ +package org.usfirst.frc4048.common.swervev2.type; + +public interface PidSwerve { + double calcDrivePidOut(double setpoint); + double calcSteerPidOut(double goal); + double calcDriveFeedForward(double velocity); + double calcSteerFeedForward(); +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/type/SparkMaxSwerveModule.java b/src/main/java/org/usfirst/frc4048/common/swervev2/type/SparkMaxSwerveModule.java new file mode 100644 index 0000000..e21550c --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/type/SparkMaxSwerveModule.java @@ -0,0 +1,12 @@ +package org.usfirst.frc4048.common.swervev2.type; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import org.usfirst.frc4048.common.swervev2.components.EncodedSwerveSparkMax; +import org.usfirst.frc4048.common.util.Gain; +import org.usfirst.frc4048.common.util.PID; + +public class SparkMaxSwerveModule extends GenericSwerveModule { + public SparkMaxSwerveModule(EncodedSwerveSparkMax swerveMotors, PID drivePid, PID turnPid, Gain driveGain, Gain turnGain, TrapezoidProfile.Constraints goalConstraint) { + super(swerveMotors, drivePid, turnPid, driveGain, turnGain, goalConstraint); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/type/StatedSwerve.java b/src/main/java/org/usfirst/frc4048/common/swervev2/type/StatedSwerve.java new file mode 100644 index 0000000..afba48a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/type/StatedSwerve.java @@ -0,0 +1,10 @@ +package org.usfirst.frc4048.common.swervev2.type; + +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public interface StatedSwerve { + + SwerveModuleState getState(); + + void setDesiredState(SwerveModuleState desiredState); +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/Gain.java b/src/main/java/org/usfirst/frc4048/common/util/Gain.java new file mode 100644 index 0000000..c1ba548 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/Gain.java @@ -0,0 +1,23 @@ +package org.usfirst.frc4048.common.util; + +public class Gain { + private final double v; + private final double s; + + public Gain(double v, double s) { + this.v = v; + this.s = s; + } + + public double getV() { + return v; + } + + public double getS() { + return s; + } + + public static Gain of(double v, double s){ + return new Gain(v,s); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/PID.java b/src/main/java/org/usfirst/frc4048/common/util/PID.java new file mode 100644 index 0000000..be1e173 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/PID.java @@ -0,0 +1,29 @@ +package org.usfirst.frc4048.common.util; + +public class PID { + private final double p; + private final double i; + private final double d; + + public PID(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + } + + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public static PID of(double p, double i, double d){ + return new PID(p,i,d); + } +} From e932576aa71a991f5c3715915c7d48ee49ca0d67 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 5 Nov 2023 22:15:05 -0500 Subject: [PATCH 11/28] got rid of default interface method --- .../common/swervev2/components/GenericEncodedSwerve.java | 5 +++++ .../common/swervev2/components/SwerveMotorEncoder.java | 4 ---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java index 0bd100a..3628b26 100644 --- a/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java @@ -2,6 +2,8 @@ import com.ctre.phoenix.sensors.WPI_CANCoder; import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.motorcontrol.MotorController; public class GenericEncodedSwerve implements SwerveMotor, SwerveMotorEncoder { @@ -76,4 +78,7 @@ public void setSteerOffset(double zeroAbs) { steerOffset += 2 * Math.PI; } } + public SwerveModulePosition getPosition(){ + return new SwerveModulePosition(getDriveEncPosition(),new Rotation2d(getSteerEncPosition())); + } } diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java index 7d07194..964f314 100644 --- a/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java @@ -14,8 +14,4 @@ public interface SwerveMotorEncoder { void setSteerOffset(double zeroAbs); - default SwerveModulePosition getPosition(){ - return new SwerveModulePosition(getDriveEncPosition(),new Rotation2d(getSteerEncPosition())); - } - } From f374f9990d79af16e4b9ab770a119eb66fe6d599 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 5 Nov 2023 22:19:02 -0500 Subject: [PATCH 12/28] cleaned up reference Drivetrain --- .../usfirst/frc4048/common/swervev2/SwerveDrivetrain.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java index be485b8..f03af71 100644 --- a/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java @@ -14,6 +14,7 @@ import org.usfirst.frc4048.common.util.Gain; import org.usfirst.frc4048.common.util.PID; +//TODO change to test public class SwerveDrivetrain { private final CANSparkMax m_frontLeftDrive; private final CANSparkMax m_frontLeftTurn; @@ -36,9 +37,9 @@ public SwerveDrivetrain() { m_frontLeftTurn = new CANSparkMax(0, CANSparkMaxLowLevel.MotorType.kBrushless); frontLeftCanCoder = new WPI_CANCoder(0); - double driveVelConvFactor = (2 * 0) / (0 * 60); - double drivePosConvFactor = (2 * 0) / (0); - double steerPosConvFactor = (2 * Math.PI / 0); + double driveVelConvFactor = 1; + double drivePosConvFactor = 1; + double steerPosConvFactor = 1; EncodedSwerveSparkMax encodedSwerveSparkMax = new EncodedSwerveSparkMax(m_frontLeftDrive, m_frontLeftTurn, frontLeftCanCoder, driveVelConvFactor, drivePosConvFactor, steerPosConvFactor); TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(18 * 4, 2 * Math.PI * 10); m_frontLeft = new SparkMaxSwerveModule(encodedSwerveSparkMax, PID.of(0,0,0),PID.of(0,0,0), Gain.of(0,0),Gain.of(0,0),constraints); From 6ad237f957076c7dc4b5e065ab17deacfb9b927f Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 8 Nov 2023 20:11:42 -0500 Subject: [PATCH 13/28] added spark max specific motor setup --- .../common/swervev2/components/EncodedSwerveSparkMax.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java b/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java index efe1220..23c20a5 100644 --- a/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java @@ -6,5 +6,10 @@ public class EncodedSwerveSparkMax extends GenericEncodedSwerve { public EncodedSwerveSparkMax(CANSparkMax driveMotor, CANSparkMax steerMotor, WPI_CANCoder absEncoder, double driveVelFactor, double drivePosFactor, double steerPosFactor) { super(driveMotor, steerMotor, absEncoder, driveMotor.getEncoder(), steerMotor.getEncoder(), driveVelFactor, drivePosFactor, steerPosFactor); + driveMotor.restoreFactoryDefaults(); + steerMotor.restoreFactoryDefaults(); + + driveMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + steerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); } } From 469972d8dc8bf64174189a196bf0ec286acdf3aa Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 19 Nov 2023 23:35:17 -0500 Subject: [PATCH 14/28] added april tag stuff (not done or tested) --- .../frc4048/common/apriltags/AprilTagMap.java | 45 +++++ .../common/apriltags/AprilTagPoseFilter.java | 93 ++++++++++ .../common/apriltags/AprilTagPosition.java | 141 +++++++++++++++ .../apriltags/PhotonCameraSubsystem.java | 170 ++++++++++++++++++ .../common/apriltags/RobotPosition.java | 10 ++ 5 files changed, 459 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagMap.java create mode 100644 src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPoseFilter.java create mode 100644 src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPosition.java create mode 100644 src/main/java/org/usfirst/frc4048/common/apriltags/PhotonCameraSubsystem.java create mode 100644 src/main/java/org/usfirst/frc4048/common/apriltags/RobotPosition.java diff --git a/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagMap.java b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagMap.java new file mode 100644 index 0000000..91b3825 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagMap.java @@ -0,0 +1,45 @@ +package org.usfirst.frc4048.common.apriltags; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +import java.util.ArrayList; +import java.util.List; + +public class AprilTagMap { + + public static AprilTagFieldLayout getAprilTagLayout(Alliance alliance) { + List apriltags = new ArrayList<>(); + if (alliance.equals(Alliance.Red)) { + + apriltags.add(0, new AprilTag(1, new Pose3d(1.02743, 6.938264, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(1, new AprilTag(2, new Pose3d(1.02743, 5.261864, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(2, new AprilTag(3, new Pose3d(1.02743, 3.585464, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(3, new AprilTag(4, new Pose3d(0.36195, 1.260094, 0.695452, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(4, new AprilTag(5, new Pose3d(16.178784, 1.260094, 0.695452, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(5, new AprilTag(6, new Pose3d(15.513558, 3.585464, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(6, new AprilTag(7, new Pose3d(15.513558, 5.261864, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(7, new AprilTag(8, new Pose3d(15.513558, 6.938264, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + + AprilTagFieldLayout aprilTagFieldLayout = new AprilTagFieldLayout(apriltags, 16.54175, 8.0137); + return aprilTagFieldLayout; + } else { + apriltags.add(0, new AprilTag(1, new Pose3d(15.513558, 1.071626, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(1, new AprilTag(2, new Pose3d(15.513558, 2.748026, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(2, new AprilTag(3, new Pose3d(15.513558, 4.424426, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(3, new AprilTag(4, new Pose3d(16.178784, 6.749796, 0.695452, new Rotation3d(new Quaternion(0, 0, 0, 1.0))))); + apriltags.add(4, new AprilTag(5, new Pose3d(0.36195, 6.749796, 0.695452, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(5, new AprilTag(6, new Pose3d(1.02743, 4.424426, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(6, new AprilTag(7, new Pose3d(1.02743, 2.748026, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + apriltags.add(7, new AprilTag(8, new Pose3d(1.02743, 1.071626, 0.462788, new Rotation3d(new Quaternion(0, 0, 0, 0))))); + + AprilTagFieldLayout aprilTagFieldLayout = new AprilTagFieldLayout(apriltags, 16.54175, 8.0137); + return aprilTagFieldLayout; + } + + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPoseFilter.java b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPoseFilter.java new file mode 100644 index 0000000..8d91180 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPoseFilter.java @@ -0,0 +1,93 @@ +package org.usfirst.frc4048.common.apriltags; + +import edu.wpi.first.util.CircularBuffer; + +public class AprilTagPoseFilter { + + private int counter = 0; + private double average = 1; + private int maxInputs = 1; + private CircularBuffer values; + private double tolerance; + + /** + * Creates the filter + * + * @param maxInputs The number of inputs taken by before the filter starts + * filtering. + * @param tolerance The size of the range that the value can deviate from. ex. + * range of 5 will allow new inputs to be 5 less than and 5 + * greater than the average + */ + public AprilTagPoseFilter(int maxInputs, double tolerance) { + this.maxInputs = maxInputs; + this.tolerance = tolerance; + values = new CircularBuffer(maxInputs); + } + + /** + * Checks if the inputed value should be filtered or not using the tolerence given and the average of the last few values + * + * @param averageLast average of the past few values + * @param input new value to check if valid or not + * @param tolerance The size of the range that the value can deviate from. ex. + * range of 5 will allow new inputs to be 5 less than and 5 + * greater than the average + * + * @return if the value is valid or not + */ + public boolean isValid(double averageLast, double input, double tolerance) { + return (input <= (averageLast + tolerance) && input >= (averageLast - tolerance)); + } + + /** + * Calculates the new value from the previous values and/or adds a new value to + * compare future values with. + * + * @param newInput input to be calculated by and/or added to the filter + * + * @return new calculated values + */ + public double calculate(double newInput) { + if (!isValid(average, newInput, tolerance) && counter >= maxInputs) { + newInput = values.getLast(); + } else { + values.addLast(newInput); + } + double intermediateAverage = 0; + for (int i = values.size(); i > 0; i--) { + intermediateAverage = intermediateAverage + values.get(i); + } + intermediateAverage = intermediateAverage / values.size(); + average = intermediateAverage; + counter++; + return newInput; + } + + /** + * Resets the filter so that it will ignore all previous values given and will + * gain new filter + */ + public void resetFilter() { + counter = 0; + values.clear(); + } + + /** + * @return The current average of all previous valid values + */ + public double getAverage() { + return average; + } + + /** + * @return The last values used when doing calculations in the order {oldest value, 2nd oldest value, ... 2nd newest value, newest value} + */ + public double[] getValuesInFilter() { + double[] output = new double[values.size() - 1]; + for (int i = 0; i >= (values.size() - 1); i++) { + output[i] = values.get(i); + } + return output; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPosition.java b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPosition.java new file mode 100644 index 0000000..4734f18 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/apriltags/AprilTagPosition.java @@ -0,0 +1,141 @@ +package org.usfirst.frc4048.common.apriltags; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** + * @see org.usfirst.frc4048.common.apriltags.PhotonCameraSubsystem + */ +@Deprecated +public class AprilTagPosition extends SubsystemBase { + + NetworkTableEntry tagIdLeftCam; + NetworkTableEntry yRotationLeftCam; + NetworkTableEntry horizontalOffsetLeftCam; + NetworkTableEntry realDistanceLeftCam; + NetworkTableEntry fieldXCoordinateLeft; + NetworkTableEntry fieldYCoordinateLeft; + NetworkTableEntry tagIdRightCam; + NetworkTableEntry yRotationRightCam; + NetworkTableEntry horizontalOffsetRightCam; + NetworkTableEntry realDistanceRightCam; + NetworkTableEntry fieldXCoordinateRight; + NetworkTableEntry fieldYCoordinateRight; + NetworkTableEntry fieldRotationLeft; + NetworkTableEntry fieldRotationRight; + + private final Field2d robotField = new Field2d(); + + NetworkTable tableLeft; + NetworkTable tableRight; + double distanceFromCamLeft; + double distanceFromCamRight; + + public AprilTagPosition() { + + tableLeft = NetworkTableInstance.getDefault().getTable("apriltag").getSubTable("left"); + this.tagIdLeftCam = tableLeft.getEntry("tagId"); + this.yRotationLeftCam = tableLeft.getEntry("angleY"); + this.horizontalOffsetLeftCam = tableLeft.getEntry("horizontalOffset"); + this.realDistanceLeftCam = tableLeft.getEntry("realDistance"); + this.fieldXCoordinateLeft = tableLeft.getEntry("trueXCoordinate"); + this.fieldYCoordinateLeft = tableLeft.getEntry("trueYCoordinate"); + this.fieldRotationLeft = tableLeft.getEntry("fieldRotation"); + + tableRight = NetworkTableInstance.getDefault().getTable("apriltag").getSubTable("right"); + this.tagIdRightCam = tableRight.getEntry("tagId"); + this.yRotationRightCam = tableRight.getEntry("angleY"); + this.horizontalOffsetRightCam = tableRight.getEntry("horizontalOffset"); + this.realDistanceRightCam = tableRight.getEntry("realDistance"); + this.fieldXCoordinateRight = tableRight.getEntry("trueXCoordinate"); + this.fieldYCoordinateRight = tableRight.getEntry("trueYCoordinate"); + this.fieldRotationRight = tableRight.getEntry("fieldRotation"); + + SmartDashboard.putData(robotField); + + } + + public boolean isLeftCameraDetectingTag() { + return tagIdLeftCam.getInteger(0L) != 0; + } + + public boolean isRightCameraDetectingTag() { + return tagIdRightCam.getInteger(0L) != 0; + } + + public Pose2d getTagRelativePose2d() { + RobotPosition position = getRobotPosition(); + if (position != null) { + return new Pose2d(position.depthOffset, position.horizontalOffset, new Rotation2d(position.rotation)); + } + return null; + } + + public Pose2d getFieldRelativePose2d() { + RobotPosition position = getRobotPosition(); + if (position != null) { + return new Pose2d(position.fieldXPosition, position.fieldYPosition, new Rotation2d(position.fieldRotation)); + } + return null; + } + + private RobotPosition getLeftRobotPosition() { + RobotPosition positionLeft = new RobotPosition(); + positionLeft.horizontalOffset = horizontalOffsetLeftCam.getDouble(0); + positionLeft.depthOffset = realDistanceLeftCam.getDouble(0); + positionLeft.rotation = yRotationLeftCam.getDouble(0); + positionLeft.fieldXPosition = fieldXCoordinateLeft.getDouble(0); + positionLeft.fieldYPosition = fieldYCoordinateLeft.getDouble(0); + positionLeft.fieldRotation = fieldRotationLeft.getDouble(0); + return positionLeft; + } + + private RobotPosition getRightRobotPosition() { + RobotPosition positionRight = new RobotPosition(); + positionRight.horizontalOffset = horizontalOffsetRightCam.getDouble(0); + positionRight.depthOffset = realDistanceRightCam.getDouble(0); + positionRight.rotation = yRotationRightCam.getDouble(0); + positionRight.fieldXPosition = fieldXCoordinateRight.getDouble(0); + positionRight.fieldYPosition = fieldYCoordinateRight.getDouble(0); + positionRight.fieldRotation = fieldRotationRight.getDouble(0); + return positionRight; + } + + public RobotPosition getRobotPosition() { + if (isLeftCameraDetectingTag() && !isRightCameraDetectingTag()) { + return getLeftRobotPosition(); + } + + else if (isRightCameraDetectingTag() && !isLeftCameraDetectingTag()) { + return getRightRobotPosition(); + } else if (isLeftCameraDetectingTag() && isRightCameraDetectingTag()) { + distanceFromCamLeft = Math.sqrt((horizontalOffsetLeftCam.getDouble(0) * horizontalOffsetLeftCam.getDouble(0)) + + (realDistanceLeftCam.getDouble(0) * realDistanceLeftCam.getDouble(0))); + distanceFromCamRight = Math.sqrt((horizontalOffsetRightCam.getDouble(0) * horizontalOffsetRightCam.getDouble(0)) + + (realDistanceRightCam.getDouble(0) * realDistanceRightCam.getDouble(0))); + if (distanceFromCamLeft <= distanceFromCamRight) { + return getLeftRobotPosition(); + } else { + return getRightRobotPosition(); + } + } + return null; + } + + @Override + public void periodic() { + //Constants.APRILTAG_DEBUG + if (false) { + RobotPosition position = getRobotPosition(); + if (position != null) { + robotField.setRobotPose(getFieldRelativePose2d()); + } + } + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/apriltags/PhotonCameraSubsystem.java b/src/main/java/org/usfirst/frc4048/common/apriltags/PhotonCameraSubsystem.java new file mode 100644 index 0000000..208e49f --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/apriltags/PhotonCameraSubsystem.java @@ -0,0 +1,170 @@ +package org.usfirst.frc4048.common.apriltags; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import java.util.*; + +public class PhotonCameraSubsystem extends SubsystemBase { + + private static final String FMSINFO_TABLE = "FMSInfo"; + private static final String IS_RED_ALLIANCE = "IsRedAlliance"; + + private AprilTagPoseFilter x2DFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter y2DFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter angleFilter = new AprilTagPoseFilter(3, 0.15708); //Placeholder value in radians + private AprilTagPoseFilter angleRFilter = new AprilTagPoseFilter(3, 0.15708); //Placeholder value in radians + private AprilTagPoseFilter x3DFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter y3DFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter z3DFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter rX3DFilter = new AprilTagPoseFilter(3, 0.15708); //Placeholder value in radians + private AprilTagPoseFilter rY3DFilter = new AprilTagPoseFilter(3, 0.15708); //Placeholder value in radians + private AprilTagPoseFilter rZ3DFilter = new AprilTagPoseFilter(3, 0.15708); //Placeholder value in radians + private AprilTagPoseFilter positionXFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + private AprilTagPoseFilter positionYFilter = new AprilTagPoseFilter(3, 1); //Placeholder value in meters + + private boolean useFilters = true; + + private PhotonCamera camera; + private AprilTagFieldLayout layout; + private Pose2d robotFieldPose; + PhotonPoseEstimator estimator; + private Pose3d tagFieldPosition; + private int noTagDetectedCounter; + // private boolean isRedAlliance; + private Alliance currentAlliance; + private double timestamp; + private EstimatedRobotPose estimatedPose; + private int periodicCounter = 0; + + int targetId; + + private NetworkTableEntry cameraLatency; + private final List robot2dFilters = new ArrayList<>(); + private final List robot3dFilters = new ArrayList<>(); + private final List tag3dFilters = new ArrayList<>(); + + + // TODO Adjust constant based on actual camera to robot height + // TODO: Add constant to shift to center of robot (or wherever needed) + Transform3d camToRobot = new Transform3d( + new Translation3d(0.0, 0, -.47), + new Rotation3d(0, 0, 0)); + + public PhotonCameraSubsystem(String cameraName) { + camera = new PhotonCamera(cameraName); + camera.setDriverMode(false); + currentAlliance = DriverStation.getAlliance(); + + layout = AprilTagMap.getAprilTagLayout(currentAlliance); + estimator = new PhotonPoseEstimator(layout, PoseStrategy.AVERAGE_BEST_TARGETS, camera, camToRobot); + + robot2dFilters.addAll(List.of(x2DFilter, y2DFilter, angleFilter, angleRFilter)); + robot3dFilters.addAll(List.of(x3DFilter, y3DFilter, z3DFilter, rX3DFilter, rY3DFilter, rZ3DFilter)); + tag3dFilters.addAll(List.of(positionXFilter, positionYFilter)); + } + + private void updateAlliance() { + if (currentAlliance != DriverStation.getAlliance()) { + currentAlliance = DriverStation.getAlliance(); + + layout = AprilTagMap.getAprilTagLayout(currentAlliance); + estimator = new PhotonPoseEstimator(layout, PoseStrategy.AVERAGE_BEST_TARGETS, camera, camToRobot); + + } + } + + /** + * Return the camera latency from network tables, will return -1 if no value is available + */ + public double getCameraLatencyMs() { + return cameraLatency.getDouble(-1.0); + } + + private void calculateUsingEstimator() { + if (camera.isConnected()) { + Optional result = estimator.update(); + + if (result.isPresent()) { + estimatedPose = result.get(); + targetId = estimatedPose.targetsUsed.get(0).getFiducialId(); + tagFieldPosition = layout.getTagPose(targetId).get(); + robotFieldPose = estimatedPose.estimatedPose.toPose2d(); + noTagDetectedCounter = 0; + } else { + if (robotFieldPose != null) { + noTagDetectedCounter++; + if (noTagDetectedCounter >= 10) { + robotFieldPose = null; + noTagDetectedCounter = 0; + estimatedPose = null; + targetId = 0; + tagFieldPosition = null; + } + } + + } + } + } + + + public double getDetectionTimestamp() { + if (estimatedPose == null) { + return -1; + } else { + timestamp = estimatedPose.timestampSeconds; + return timestamp; + } + + } + + /** + * Gets the latest robotFieldPose, could be null + * @return Pose2d + */ + public Pose2d getRobot2dFieldPose() { + return robotFieldPose; + + } + + @Override + public void periodic() { + + if (periodicCounter % 6 == 0) { + periodicCounter = 1; + //continue periodic + } else { + periodicCounter++; + return; //break out + } + + updateAlliance(); + calculateUsingEstimator(); + + //Constants.APRILTAG_DEBUG + if (false) { + resetFilters(); + } + } + + private void resetFilters() { + Pose3d pose3dPosition = null; + if (estimatedPose != null) pose3dPosition = estimatedPose.estimatedPose; + if (robotFieldPose == null) robot2dFilters.forEach(AprilTagPoseFilter::resetFilter); + if (pose3dPosition == null) robot3dFilters.forEach(AprilTagPoseFilter::resetFilter); + if (tagFieldPosition == null) tag3dFilters.forEach(AprilTagPoseFilter::resetFilter); + } + + + public int getTargetId() { + return targetId; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/apriltags/RobotPosition.java b/src/main/java/org/usfirst/frc4048/common/apriltags/RobotPosition.java new file mode 100644 index 0000000..ce73951 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/apriltags/RobotPosition.java @@ -0,0 +1,10 @@ +package org.usfirst.frc4048.common.apriltags; + +public class RobotPosition { + public double horizontalOffset; + public double depthOffset; + public double rotation; + public double fieldXPosition; + public double fieldYPosition; + public double fieldRotation; +} From 134050aa9d53d6941ba13ff84062b13db42d4d9e Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 10 Dec 2023 23:15:10 -0500 Subject: [PATCH 15/28] made color related stuff use an enum --- .../frc4048/common/diag/DiagColorSensor.java | 19 +++-- .../frc4048/common/util/ColorSensor.java | 69 +++++++------------ .../frc4048/common/util/ColorValue.java | 33 +++++++++ 3 files changed, 66 insertions(+), 55 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4048/common/util/ColorValue.java diff --git a/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java b/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java index 50a279c..3dae9dc 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java @@ -11,9 +11,12 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import org.usfirst.frc4048.common.util.ColorSensor; +import org.usfirst.frc4048.common.util.ColorValue; +import java.util.Arrays; import java.util.HashMap; import java.util.Map; +import java.util.stream.Collectors; /** * Add your docs here. @@ -24,13 +27,13 @@ public class DiagColorSensor implements Diagnosable { private String name; private String title; private GenericEntry networkTableEntry; - private Map colorMap; + private Map colorMap; public DiagColorSensor(String name, String title, ColorSensor colorsensor) { this.name = name; this.title = title; this.colorsensor = colorsensor; - colorMap = new HashMap(); + colorMap = new HashMap<>(Arrays.stream(ColorValue.values()).map(colorValue -> new HashMap.SimpleEntry<>(colorValue, false)).collect(Collectors.toMap(Map.Entry::getKey, Map.Entry::getValue))); reset(); } @@ -41,11 +44,9 @@ public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int h @Override public void refresh() { - ColorSensor.ColorValue colorValue = colorsensor.getColor(); + ColorValue colorValue = colorsensor.getColor(); colorMap.put(colorValue, true); - boolean allColors = colorMap.get(ColorSensor.ColorValue.RED) && colorMap.get(ColorSensor.ColorValue.BLUE) - && colorMap.get(ColorSensor.ColorValue.GREEN) && colorMap.get(ColorSensor.ColorValue.YELLOW) - && colorMap.get(ColorSensor.ColorValue.UNKNOWN); + boolean allColors = colorMap.values().stream().allMatch(value -> value); if (networkTableEntry != null) { networkTableEntry.setBoolean(allColors); } @@ -53,10 +54,6 @@ public void refresh() { @Override public void reset() { - colorMap.put(ColorSensor.ColorValue.RED, false); - colorMap.put(ColorSensor.ColorValue.GREEN, false); - colorMap.put(ColorSensor.ColorValue.BLUE, false); - colorMap.put(ColorSensor.ColorValue.YELLOW, false); - colorMap.put(ColorSensor.ColorValue.UNKNOWN, false); + colorMap = new HashMap<>(Arrays.stream(ColorValue.values()).map(colorValue -> new HashMap.SimpleEntry<>(colorValue, false)).collect(Collectors.toMap(Map.Entry::getKey, Map.Entry::getValue))); } } diff --git a/src/main/java/org/usfirst/frc4048/common/util/ColorSensor.java b/src/main/java/org/usfirst/frc4048/common/util/ColorSensor.java index 69bfa95..74c685c 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/ColorSensor.java +++ b/src/main/java/org/usfirst/frc4048/common/util/ColorSensor.java @@ -13,52 +13,33 @@ import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.util.Color; +import java.util.AbstractMap; +import java.util.Arrays; +import java.util.HashMap; +import java.util.stream.Collectors; + /** * Add your docs here. */ public class ColorSensor { - public enum ColorValue {RED, YELLOW, GREEN, BLUE, UNKNOWN}; - - //Accurate measurments for up to 2.5 inches - private static final Color BLUE_TARGET = new Color(0.135, 0.461, 0.404); - private static final Color GREEN_TARGET = new Color(0.197, 0.561, 0.240); - private static final Color RED_TARGET = new Color(0.504, 0.353, 0.144); - private static final Color YELLOW_TARGET = new Color(0.361, 0.524, 0.113); - - private I2C.Port sensorPort; - private ColorSensorV3 colorSensor; - private ColorMatch m_colorMatcher; - - public ColorSensor(I2C.Port sensorPort){ - this.sensorPort = sensorPort; - colorSensor = new ColorSensorV3(sensorPort); - m_colorMatcher = new ColorMatch(); - - m_colorMatcher.addColorMatch(BLUE_TARGET); - m_colorMatcher.addColorMatch(GREEN_TARGET); - m_colorMatcher.addColorMatch(RED_TARGET); - m_colorMatcher.addColorMatch(YELLOW_TARGET); - } - - //Checks if the color is Red, Blue, Green, Yellow, or Unknown - public ColorValue getColor(){ - Color detectedColor = colorSensor.getColor(); - - ColorMatchResult match = m_colorMatcher.matchColor(detectedColor); - if (match == null){ - return ColorValue.UNKNOWN; - } - if (match.color == BLUE_TARGET) { - return ColorValue.BLUE; - } else if (match.color == RED_TARGET) { - return ColorValue.RED; - } else if (match.color == GREEN_TARGET) { - return ColorValue.GREEN; - } else if (match.color == YELLOW_TARGET) { - return ColorValue.YELLOW; - } else { - return ColorValue.UNKNOWN; - } - } -} \ No newline at end of file + private I2C.Port sensorPort; + private ColorSensorV3 colorSensor; + private ColorMatch m_colorMatcher; + + public ColorSensor(I2C.Port sensorPort) { + this.sensorPort = sensorPort; + colorSensor = new ColorSensorV3(sensorPort); + m_colorMatcher = new ColorMatch(); + Arrays.stream(ColorValue.values()) + .filter(colorValue -> colorValue.getColor()!=ColorValue.UNKNOWN.getColor()) + .forEach(colorValue -> m_colorMatcher.addColorMatch(colorValue.getColor())); + } + + //Checks if the color is Red, Blue, Green, Yellow, or Unknown + public ColorValue getColor() { + Color detectedColor = colorSensor.getColor(); + ColorMatchResult match = m_colorMatcher.matchColor(detectedColor); + return ColorValue.getColorValue(match.color); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/ColorValue.java b/src/main/java/org/usfirst/frc4048/common/util/ColorValue.java new file mode 100644 index 0000000..167014a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/ColorValue.java @@ -0,0 +1,33 @@ +package org.usfirst.frc4048.common.util; + +import edu.wpi.first.wpilibj.util.Color; + +import java.util.AbstractMap; +import java.util.Arrays; +import java.util.HashMap; +import java.util.stream.Collectors; + +public enum ColorValue { + RED(new Color(0.504, 0.353, 0.144)), + YELLOW(new Color(0.361, 0.524, 0.113)), + GREEN(new Color(0.197, 0.561, 0.240)), + BLUE(new Color(0.135, 0.461, 0.404)), + UNKNOWN(new Color(-1,-1,-1)); + private static final HashMap colorMap = + new HashMap<>(Arrays.stream(ColorValue.values()) + .map(colorValue -> new AbstractMap.SimpleEntry<>(colorValue.getColor(), colorValue)) + .collect(Collectors.toMap(AbstractMap.SimpleEntry::getKey, AbstractMap.SimpleEntry::getValue))); + private final Color color; + + ColorValue(Color color) { + this.color = color; + } + + public Color getColor() { + return color; + } + public static ColorValue getColorValue(Color color) { + if (color == null) color = new Color(-1,-1,-1); + return colorMap.get(color); + } +} From 1f18cefbfb5caa7da3a9ab1ace5b7873c89e78df Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 10 Dec 2023 23:18:52 -0500 Subject: [PATCH 16/28] left ugly if statement block --- .../usfirst/frc4048/common/util/LedPanel.java | 130 +++++++----------- 1 file changed, 52 insertions(+), 78 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java index 119c3d2..2584ae3 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java +++ b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java @@ -3,95 +3,69 @@ import edu.wpi.first.wpilibj.DigitalOutput; public class LedPanel { - public enum PicID { - ZERO(false,false,false), - ONE(true,false,false), - TWO(false,true,false), - THREE(true,true,false), - FOUR(false,false,true), - FIVE(true,false,true), - SIX(false,true,true), - SEVEN(true,true,true); - private final boolean op1; - private final boolean op2; - private final boolean op3; - PicID(boolean op1, boolean op2, boolean op3) { - this.op1 = op1; - this.op2 = op2; - this.op3 = op3; - } - } - private PicID ID; + private int ID; private final DigitalOutput output1; private final DigitalOutput output2; private final DigitalOutput output3; - public LedPanel(int digitalOut1, int digitalOut2, int digitalOut3) { - ID = PicID.ZERO; - output1 = new DigitalOutput(digitalOut1); - output2 = new DigitalOutput(digitalOut2); - output3 = new DigitalOutput(digitalOut3); + public LedPanel() { + ID = 0; + + output1 = new DigitalOutput(1); + output2 = new DigitalOutput(2); + output3 = new DigitalOutput(3); } - public PicID getID() { + public int getID() { return ID; } - public void setID(PicID picID) { - ID = picID; - setPic(picID); + public void setID(int iD) { + ID = iD; + setPic(ID); } - /* Output1 Output 2 Output 3 f(x) -True Values{ 1 2 4 x%2!=0 - 3 3 5 - 5 6 6 - 7 7 7 } - */ - private void setPic(PicID picID) { - output1.set(picID.op1); - output2.set(picID.op2); - output3.set(picID.op3); -// if(picID == 0) { -// output1.set(false); -// output2.set(false); -// output3.set(false); -// } -// else if(picID == 1) { -// output1.set(true); -// output2.set(false); -// output3.set(false); -// } -// else if(picID == 2) { -// output1.set(false); -// output2.set(true); -// output3.set(false); -// } -// else if(picID == 3) { -// output1.set(true); -// output2.set(true); -// output3.set(false); -// } -// else if(picID == 4) { -// output1.set(false); -// output2.set(false); -// output3.set(true); -// } -// else if(picID == 5) { -// output1.set(true); -// output2.set(false); -// output3.set(true); -// } -// else if(picID == 6) { -// output1.set(false); -// output2.set(true); -// output3.set(true); -// } -// else if(picID == 7) { -// output1.set(true); -// output2.set(true); -// output3.set(true); -// } + private void setPic(int picID) { + if(picID == 0) { + output1.set(false); + output2.set(false); + output3.set(false); + } + else if(picID == 1) { + output1.set(true); + output2.set(false); + output3.set(false); + } + else if(picID == 2) { + output1.set(false); + output2.set(true); + output3.set(false); + } + else if(picID == 3) { + output1.set(true); + output2.set(true); + output3.set(false); + } + else if(picID == 4) { + output1.set(false); + output2.set(false); + output3.set(true); + } + else if(picID == 5) { + output1.set(true); + output2.set(false); + output3.set(true); + } + else if(picID == 6) { + output1.set(false); + output2.set(true); + output3.set(true); + } + else if(picID == 7) { + output1.set(true); + output2.set(true); + output3.set(true); + } } } From 8eb5c526caec842b84f3894a3a8fbbcc2a65887f Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Mon, 11 Dec 2023 00:41:23 -0500 Subject: [PATCH 17/28] refactored auto chooser (not tested) --- .../frc4048/common/auto/AutoAction.java | 43 +++++++++++++++++ .../frc4048/common/auto/AutoChooser.java | 14 ++++++ .../frc4048/common/auto/AutoEvent.java | 20 ++++++++ .../common/auto/AutoEventComparator.java | 48 +++++++++++++++++++ .../common/auto/ExampleAutoChooser.java | 21 ++++++++ .../frc4048/common/auto/FieldLocation.java | 6 +++ .../common/auto/GenericAutoChooser.java | 48 +++++++++++++++++++ 7 files changed, 200 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java new file mode 100644 index 0000000..59ee75e --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.DriverStation; + +public enum AutoAction { + DoNothing("Do Nothing",0,0), + TwoPieceMoveLeft("Two Piece Move Left",20,150); + + private final int rightLocation; + private final int leftLocation; + private final String name; + private static final int RED_ALLIANCE_YPOS = 99; + private static final int BLUE_ALLIANCE_YPOS = 0; + + AutoAction(String name,int rightLocation, int leftLocation) { + this.name = name; + this.rightLocation = rightLocation; + this.leftLocation = leftLocation; + } + + public int getRightLocation() { + return rightLocation; + } + + public int getLeftLocation() { + return leftLocation; + } + + public String getName() { + return name; + } + + public int getYCord(FieldLocation location, DriverStation.Alliance alliance){ + int y = alliance.equals(DriverStation.Alliance.Red) ? RED_ALLIANCE_YPOS : BLUE_ALLIANCE_YPOS; + switch (location){ + case Left: + return y + getLeftLocation(); + case Right: + return y + getRightLocation(); + default: return y + 180;// idk where this came from + } + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java new file mode 100644 index 0000000..20c7ad0 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java @@ -0,0 +1,14 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import java.util.AbstractMap; +import java.util.HashMap; + +public interface AutoChooser { + void setOptions(); + AutoAction getDefaultOption(); + + Command getAutoCommand(); +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java new file mode 100644 index 0000000..4fe25d3 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java @@ -0,0 +1,20 @@ +package org.usfirst.frc4048.common.auto; + +public class AutoEvent { + private final AutoAction action; + private final FieldLocation location; + ; + + public AutoEvent(AutoAction action, FieldLocation location) { + this.action = action; + this.location = location; + } + + public AutoAction getAction() { + return action; + } + + public FieldLocation getLocation() { + return location; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java new file mode 100644 index 0000000..ebcda4a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4048.common.auto; + +import java.util.Arrays; +import java.util.Collections; +import java.util.List; +import java.util.stream.Collectors; + +public class AutoEventComparator { + private final List includedActions; + private final List includedLocations; + + private AutoEventComparator (List includedActions, List includedLocations) { + this.includedActions = includedActions; + this.includedLocations = includedLocations; + } + public static AutoEventComparator fromActions(List includedActions) { + List locations = Arrays.stream(FieldLocation.values()).collect(Collectors.toList()); + return new AutoEventComparator(includedActions,locations); + } + public static AutoEventComparator fromLocations(List includedLocations) { + List actions = Arrays.stream(AutoAction.values()).collect(Collectors.toList()); + return new AutoEventComparator(actions,includedLocations); + } + public static AutoEventComparator fromActionsAndLocations(List includedActions, List includedLocations) { + return new AutoEventComparator(includedActions,includedLocations); + } + public static AutoEventComparator fromExcludedAction(List excludedActions){ + List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); + return new AutoEventComparator(actions,Arrays.asList(FieldLocation.values())); + } + public static AutoEventComparator fromExcludedLocation(List excludedLocations){ + List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); + return new AutoEventComparator(Arrays.asList(AutoAction.values()),locations); + } + public static AutoEventComparator fromExcludedActionAndLocation(List excludedActions, List excludedLocations){ + List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); + List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); + return new AutoEventComparator(actions,locations); + } + + public boolean isAutoEventValid(AutoEvent event){ + return includedActions.contains(event.getAction()) && includedLocations.contains(event.getLocation()); + } + + + + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java new file mode 100644 index 0000000..05bed4c --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -0,0 +1,21 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import org.usfirst.frc4048.common.PlaceHolderCommand; + +import java.util.List; +import java.util.Map; + +public class ExampleAutoChooser extends GenericAutoChooser{ + @Override + public AutoAction getDefaultOption() { + return AutoAction.DoNothing; + } + + @Override + Map getCommandMap() { + return Map.of(AutoEventComparator.fromActions(List.of(AutoAction.DoNothing)), new PlaceHolderCommand(), + AutoEventComparator.fromActionsAndLocations(List.of(AutoAction.TwoPieceMoveLeft), List.of(FieldLocation.Middle)), new PlaceHolderCommand()); + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java b/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java new file mode 100644 index 0000000..7e4bab6 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java @@ -0,0 +1,6 @@ +package org.usfirst.frc4048.common.auto; +public enum FieldLocation { + Right, + Middle, + Left; +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java new file mode 100644 index 0000000..f851372 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import jdk.javadoc.doclet.Taglet; +import org.usfirst.frc4048.common.PlaceHolderCommand; + +import javax.swing.*; +import java.util.*; + +public abstract class GenericAutoChooser implements AutoChooser { + private SendableChooser actionChooser; + private SendableChooser locationChooser; + + public GenericAutoChooser() { + actionChooser = new SendableChooser<>(); + locationChooser = new SendableChooser<>(); + } + + public void init() { + setOptions(); + actionChooser.setDefaultOption(getDefaultOption().getName(), getDefaultOption()); + } + + @Override + public void setOptions() { + Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); + Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); + } + + public AutoAction getAction() { + return actionChooser.getSelected() == null ? AutoAction.DoNothing : actionChooser.getSelected(); + } + + public FieldLocation getLocation() { + return locationChooser.getSelected() == null ? FieldLocation.Middle : locationChooser.getSelected(); + } + + @Override + public Command getAutoCommand() { + Optional first = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).findFirst(); + if (first.isPresent()) return getCommandMap().get(first.get()); + else return new PlaceHolderCommand(); //default command + } + + abstract Map getCommandMap(); + +} From e71e8c45b3728cd7c989e04bf3615a423c60c21c Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Mon, 11 Dec 2023 00:41:23 -0500 Subject: [PATCH 18/28] refactored auto chooser (not tested) --- .../frc4048/common/auto/AutoAction.java | 43 ++++++++++++++ .../frc4048/common/auto/AutoChooser.java | 14 +++++ .../frc4048/common/auto/AutoEvent.java | 20 +++++++ .../common/auto/AutoEventComparer.java | 56 +++++++++++++++++++ .../common/auto/ExampleAutoChooser.java | 19 +++++++ .../frc4048/common/auto/FieldLocation.java | 6 ++ .../common/auto/GenericAutoChooser.java | 45 +++++++++++++++ .../common/auto/PlaceHolderCommand.java | 11 ++++ 8 files changed, 214 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java new file mode 100644 index 0000000..59ee75e --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.DriverStation; + +public enum AutoAction { + DoNothing("Do Nothing",0,0), + TwoPieceMoveLeft("Two Piece Move Left",20,150); + + private final int rightLocation; + private final int leftLocation; + private final String name; + private static final int RED_ALLIANCE_YPOS = 99; + private static final int BLUE_ALLIANCE_YPOS = 0; + + AutoAction(String name,int rightLocation, int leftLocation) { + this.name = name; + this.rightLocation = rightLocation; + this.leftLocation = leftLocation; + } + + public int getRightLocation() { + return rightLocation; + } + + public int getLeftLocation() { + return leftLocation; + } + + public String getName() { + return name; + } + + public int getYCord(FieldLocation location, DriverStation.Alliance alliance){ + int y = alliance.equals(DriverStation.Alliance.Red) ? RED_ALLIANCE_YPOS : BLUE_ALLIANCE_YPOS; + switch (location){ + case Left: + return y + getLeftLocation(); + case Right: + return y + getRightLocation(); + default: return y + 180;// idk where this came from + } + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java new file mode 100644 index 0000000..20c7ad0 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java @@ -0,0 +1,14 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import java.util.AbstractMap; +import java.util.HashMap; + +public interface AutoChooser { + void setOptions(); + AutoAction getDefaultOption(); + + Command getAutoCommand(); +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java new file mode 100644 index 0000000..4fe25d3 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java @@ -0,0 +1,20 @@ +package org.usfirst.frc4048.common.auto; + +public class AutoEvent { + private final AutoAction action; + private final FieldLocation location; + ; + + public AutoEvent(AutoAction action, FieldLocation location) { + this.action = action; + this.location = location; + } + + public AutoAction getAction() { + return action; + } + + public FieldLocation getLocation() { + return location; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java new file mode 100644 index 0000000..20eda5e --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java @@ -0,0 +1,56 @@ +package org.usfirst.frc4048.common.auto; + +import java.util.Arrays; +import java.util.List; +import java.util.stream.Collectors; + +public class AutoEventComparer { + private final List includedActions; + private final List includedLocations; + + private AutoEventComparer(List includedActions, List includedLocations) { + this.includedActions = includedActions; + this.includedLocations = includedLocations; + } + public static AutoEventComparer fromActions(List includedActions) { + List locations = Arrays.stream(FieldLocation.values()).collect(Collectors.toList()); + return new AutoEventComparer(includedActions,locations); + } + public static AutoEventComparer fromLocations(List includedLocations) { + List actions = Arrays.stream(AutoAction.values()).collect(Collectors.toList()); + return new AutoEventComparer(actions,includedLocations); + } + public static AutoEventComparer fromActionsAndLocations(List includedActions, List includedLocations) { + return new AutoEventComparer(includedActions,includedLocations); + } + public static AutoEventComparer fromExcludedAction(List excludedActions){ + List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); + return new AutoEventComparer(actions,Arrays.asList(FieldLocation.values())); + } + public static AutoEventComparer fromExcludedLocation(List excludedLocations){ + List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); + return new AutoEventComparer(Arrays.asList(AutoAction.values()),locations); + } + public static AutoEventComparer fromExcludedActionAndLocation(List excludedActions, List excludedLocations){ + List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); + List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); + return new AutoEventComparer(actions,locations); + } + public static AutoEventComparer fromAction(AutoAction action){ + return new AutoEventComparer(List.of(action),Arrays.asList(FieldLocation.values())); + } + public static AutoEventComparer fromIncludedLocation(FieldLocation location){ + return new AutoEventComparer(Arrays.asList(AutoAction.values()),List.of(location)); + } + public static AutoEventComparer fromActionAndLocation(AutoAction action, FieldLocation location){ + return new AutoEventComparer(List.of(action),List.of(location)); + } + + public boolean isAutoEventValid(AutoEvent event){ + return includedActions.contains(event.getAction()) && includedLocations.contains(event.getLocation()); + } + + + + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java new file mode 100644 index 0000000..a1806a3 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -0,0 +1,19 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.Map; + +public class ExampleAutoChooser extends GenericAutoChooser{ + @Override + public AutoAction getDefaultOption() { + return AutoAction.DoNothing; + } + + @Override + Map getCommandMap() { + return Map.of(AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), + AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand()); + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java b/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java new file mode 100644 index 0000000..7e4bab6 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java @@ -0,0 +1,6 @@ +package org.usfirst.frc4048.common.auto; +public enum FieldLocation { + Right, + Middle, + Left; +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java new file mode 100644 index 0000000..dc8d734 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java @@ -0,0 +1,45 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.*; + +public abstract class GenericAutoChooser implements AutoChooser { + private SendableChooser actionChooser; + private SendableChooser locationChooser; + + public GenericAutoChooser() { + actionChooser = new SendableChooser<>(); + locationChooser = new SendableChooser<>(); + } + + public void init() { + setOptions(); + actionChooser.setDefaultOption(getDefaultOption().getName(), getDefaultOption()); + } + + @Override + public void setOptions() { + Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); + Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); + } + + public AutoAction getAction() { + return actionChooser.getSelected() == null ? AutoAction.DoNothing : actionChooser.getSelected(); + } + + public FieldLocation getLocation() { + return locationChooser.getSelected() == null ? FieldLocation.Middle : locationChooser.getSelected(); + } + + @Override + public Command getAutoCommand() { + Optional first = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).findFirst(); + if (first.isPresent()) return getCommandMap().get(first.get()); + else return new PlaceHolderCommand(); //default command + } + + abstract Map getCommandMap(); + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java b/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java new file mode 100644 index 0000000..b006b08 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java @@ -0,0 +1,11 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.util.function.BooleanConsumer; +import edu.wpi.first.wpilibj2.command.*; + +import java.util.Set; +import java.util.function.BooleanSupplier; + +public class PlaceHolderCommand extends CommandBase{ + +} From 28baaf8c76149dccc3c294a91694e042bd8a704f Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Mon, 11 Dec 2023 00:54:10 -0500 Subject: [PATCH 19/28] fixed missed functionality and removed unneeded interface --- .../frc4048/common/auto/AutoChooser.java | 14 ----------- .../common/auto/ExampleAutoChooser.java | 22 +++++++++++++---- .../common/auto/GenericAutoChooser.java | 24 +++++++++---------- 3 files changed, 28 insertions(+), 32 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java deleted file mode 100644 index 20c7ad0..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.usfirst.frc4048.common.auto; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; - -import java.util.AbstractMap; -import java.util.HashMap; - -public interface AutoChooser { - void setOptions(); - AutoAction getDefaultOption(); - - Command getAutoCommand(); -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java index a1806a3..67a64c6 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -1,19 +1,31 @@ package org.usfirst.frc4048.common.auto; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import java.util.Map; public class ExampleAutoChooser extends GenericAutoChooser{ - @Override - public AutoAction getDefaultOption() { - return AutoAction.DoNothing; - } @Override Map getCommandMap() { return Map.of(AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand()); } - + + @Override + CommandBase getDefaultCommand() { + return new PlaceHolderCommand(); + } + + @Override + AutoAction getDefaultActionOption() { + return AutoAction.DoNothing; + } + + @Override + FieldLocation getDefaultLocationOption() { + return FieldLocation.Middle; + } + } diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java index dc8d734..c85aa38 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java @@ -2,28 +2,24 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import javax.swing.*; import java.util.*; -public abstract class GenericAutoChooser implements AutoChooser { +public abstract class GenericAutoChooser { private SendableChooser actionChooser; private SendableChooser locationChooser; public GenericAutoChooser() { actionChooser = new SendableChooser<>(); locationChooser = new SendableChooser<>(); - } - - public void init() { - setOptions(); - actionChooser.setDefaultOption(getDefaultOption().getName(), getDefaultOption()); - } - - @Override - public void setOptions() { Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); + actionChooser.setDefaultOption(getDefaultActionOption().getName(), getDefaultActionOption()); + locationChooser.setDefaultOption(getDefaultLocationOption().name(), getDefaultLocationOption()); } + public AutoAction getAction() { return actionChooser.getSelected() == null ? AutoAction.DoNothing : actionChooser.getSelected(); @@ -32,14 +28,16 @@ public AutoAction getAction() { public FieldLocation getLocation() { return locationChooser.getSelected() == null ? FieldLocation.Middle : locationChooser.getSelected(); } - - @Override + public Command getAutoCommand() { Optional first = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).findFirst(); if (first.isPresent()) return getCommandMap().get(first.get()); - else return new PlaceHolderCommand(); //default command + else return getDefaultCommand(); } abstract Map getCommandMap(); + abstract CommandBase getDefaultCommand(); + abstract AutoAction getDefaultActionOption(); + abstract FieldLocation getDefaultLocationOption(); } From 4c1a8c2a82e44713cf140d49b9dfa2095fc637c6 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Mon, 11 Dec 2023 01:00:53 -0500 Subject: [PATCH 20/28] made methods protected --- .../usfirst/frc4048/common/auto/ExampleAutoChooser.java | 8 ++++---- .../usfirst/frc4048/common/auto/GenericAutoChooser.java | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java index 67a64c6..8d44aa5 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -8,23 +8,23 @@ public class ExampleAutoChooser extends GenericAutoChooser{ @Override - Map getCommandMap() { + protected Map getCommandMap() { return Map.of(AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand()); } @Override - CommandBase getDefaultCommand() { + protected CommandBase getDefaultCommand() { return new PlaceHolderCommand(); } @Override - AutoAction getDefaultActionOption() { + protected AutoAction getDefaultActionOption() { return AutoAction.DoNothing; } @Override - FieldLocation getDefaultLocationOption() { + protected FieldLocation getDefaultLocationOption() { return FieldLocation.Middle; } diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java index c85aa38..5135d3b 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java @@ -35,9 +35,9 @@ public Command getAutoCommand() { else return getDefaultCommand(); } - abstract Map getCommandMap(); - abstract CommandBase getDefaultCommand(); - abstract AutoAction getDefaultActionOption(); - abstract FieldLocation getDefaultLocationOption(); + protected abstract Map getCommandMap(); + protected abstract CommandBase getDefaultCommand(); + protected abstract AutoAction getDefaultActionOption(); + protected abstract FieldLocation getDefaultLocationOption(); } From 849252fbc3d7da16a44d50055397f866e9a050a0 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 13 Dec 2023 22:33:58 -0500 Subject: [PATCH 21/28] got rid of a file that should have been removed in a refactor. Added priority to command selection. made AutoEventComparer properties accessible through a getter --- .../common/auto/AutoEventComparator.java | 48 ------------------- .../common/auto/AutoEventComparer.java | 12 +++-- .../common/auto/ExampleAutoChooser.java | 6 ++- .../common/auto/GenericAutoChooser.java | 24 +++++++--- 4 files changed, 30 insertions(+), 60 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java deleted file mode 100644 index ebcda4a..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparator.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc4048.common.auto; - -import java.util.Arrays; -import java.util.Collections; -import java.util.List; -import java.util.stream.Collectors; - -public class AutoEventComparator { - private final List includedActions; - private final List includedLocations; - - private AutoEventComparator (List includedActions, List includedLocations) { - this.includedActions = includedActions; - this.includedLocations = includedLocations; - } - public static AutoEventComparator fromActions(List includedActions) { - List locations = Arrays.stream(FieldLocation.values()).collect(Collectors.toList()); - return new AutoEventComparator(includedActions,locations); - } - public static AutoEventComparator fromLocations(List includedLocations) { - List actions = Arrays.stream(AutoAction.values()).collect(Collectors.toList()); - return new AutoEventComparator(actions,includedLocations); - } - public static AutoEventComparator fromActionsAndLocations(List includedActions, List includedLocations) { - return new AutoEventComparator(includedActions,includedLocations); - } - public static AutoEventComparator fromExcludedAction(List excludedActions){ - List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); - return new AutoEventComparator(actions,Arrays.asList(FieldLocation.values())); - } - public static AutoEventComparator fromExcludedLocation(List excludedLocations){ - List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); - return new AutoEventComparator(Arrays.asList(AutoAction.values()),locations); - } - public static AutoEventComparator fromExcludedActionAndLocation(List excludedActions, List excludedLocations){ - List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); - List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); - return new AutoEventComparator(actions,locations); - } - - public boolean isAutoEventValid(AutoEvent event){ - return includedActions.contains(event.getAction()) && includedLocations.contains(event.getLocation()); - } - - - - -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java index 20eda5e..3e2845b 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java @@ -49,8 +49,12 @@ public static AutoEventComparer fromActionAndLocation(AutoAction action, FieldLo public boolean isAutoEventValid(AutoEvent event){ return includedActions.contains(event.getAction()) && includedLocations.contains(event.getLocation()); } - - - - + + public List getIncludedActions() { + return List.copyOf(includedActions); + } + + public List getIncludedLocations() { + return List.copyOf(includedLocations); + } } diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java index 8d44aa5..91b56a6 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -9,8 +9,10 @@ public class ExampleAutoChooser extends GenericAutoChooser{ @Override protected Map getCommandMap() { - return Map.of(AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), - AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand()); + return Map.of( + AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), + AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand() + ); } @Override diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java index 5135d3b..5236d5b 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java @@ -6,10 +6,11 @@ import javax.swing.*; import java.util.*; +import java.util.stream.Collectors; public abstract class GenericAutoChooser { - private SendableChooser actionChooser; - private SendableChooser locationChooser; + private final SendableChooser actionChooser; + private final SendableChooser locationChooser; public GenericAutoChooser() { actionChooser = new SendableChooser<>(); @@ -28,11 +29,22 @@ public AutoAction getAction() { public FieldLocation getLocation() { return locationChooser.getSelected() == null ? FieldLocation.Middle : locationChooser.getSelected(); } - + + /** + * This method by default will pick the more specific command over the more generic one. + * For example if a command had the following criteria: action=TwoPieceMoveLeft, location=any
+ * And another command had the criteria: action=TwoPieceMoveLeft, location=left
+ * Then the second command would have a higher priority because it is more specific.
+ * If two commands the in specificity a random one is chosen + * @return the corresponding the command of the desired location and action + */ public Command getAutoCommand() { - Optional first = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).findFirst(); - if (first.isPresent()) return getCommandMap().get(first.get()); - else return getDefaultCommand(); + List commands = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).collect(Collectors.toList()); + if (commands.isEmpty())return getDefaultCommand(); + //most specific + Optional specificCommand = commands.stream().filter(c -> c.getIncludedActions().size() != AutoAction.values().length && c.getIncludedLocations().size() != FieldLocation.values().length).findFirst(); + if (specificCommand.isPresent()) return getCommandMap().get(specificCommand.get()); + return getCommandMap().get(commands.get(0)); } protected abstract Map getCommandMap(); From 4a9008f51da1be503742108e22518eb3a590e5c4 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 13 Dec 2023 22:56:48 -0500 Subject: [PATCH 22/28] abstracted code --- .../frc4048/common/auto/AutoChooser.java | 16 ++++++ .../common/auto/AutoEventProvider.java | 9 +++ .../common/auto/ExampleAutoChooser.java | 15 ++--- .../common/auto/GenericAutoChooser.java | 55 ------------------- .../common/auto/Nt4AutoEventProvider.java | 42 ++++++++++++++ .../common/auto/PrioritizedAutoChooser.java | 34 ++++++++++++ 6 files changed, 106 insertions(+), 65 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java create mode 100644 src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java new file mode 100644 index 0000000..8615942 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java @@ -0,0 +1,16 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import java.util.*; +import java.util.stream.Collectors; + +public interface AutoChooser { + Command getAutoCommand(); + Map getCommandMap(); + CommandBase getDefaultCommand(); + AutoEventProvider getAutoEventProvider(); + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java new file mode 100644 index 0000000..e7d1d0b --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java @@ -0,0 +1,9 @@ +package org.usfirst.frc4048.common.auto; + +public interface AutoEventProvider { + AutoAction getSelectedAction(); + FieldLocation getSelectedLocation(); + AutoAction getDefaultActionOption(); + FieldLocation getDefaultLocationOption(); + +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java index 91b56a6..f3a83c7 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java @@ -5,10 +5,10 @@ import java.util.Map; -public class ExampleAutoChooser extends GenericAutoChooser{ +public class ExampleAutoChooser extends PrioritizedAutoChooser { @Override - protected Map getCommandMap() { + public Map getCommandMap() { return Map.of( AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand() @@ -16,18 +16,13 @@ protected Map getCommandMap() { } @Override - protected CommandBase getDefaultCommand() { + public CommandBase getDefaultCommand() { return new PlaceHolderCommand(); } @Override - protected AutoAction getDefaultActionOption() { - return AutoAction.DoNothing; - } - - @Override - protected FieldLocation getDefaultLocationOption() { - return FieldLocation.Middle; + public AutoEventProvider getAutoEventProvider() { + return null; } } diff --git a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java deleted file mode 100644 index 5236d5b..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/GenericAutoChooser.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.usfirst.frc4048.common.auto; - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; - -import javax.swing.*; -import java.util.*; -import java.util.stream.Collectors; - -public abstract class GenericAutoChooser { - private final SendableChooser actionChooser; - private final SendableChooser locationChooser; - - public GenericAutoChooser() { - actionChooser = new SendableChooser<>(); - locationChooser = new SendableChooser<>(); - Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); - Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); - actionChooser.setDefaultOption(getDefaultActionOption().getName(), getDefaultActionOption()); - locationChooser.setDefaultOption(getDefaultLocationOption().name(), getDefaultLocationOption()); - } - - - public AutoAction getAction() { - return actionChooser.getSelected() == null ? AutoAction.DoNothing : actionChooser.getSelected(); - } - - public FieldLocation getLocation() { - return locationChooser.getSelected() == null ? FieldLocation.Middle : locationChooser.getSelected(); - } - - /** - * This method by default will pick the more specific command over the more generic one. - * For example if a command had the following criteria: action=TwoPieceMoveLeft, location=any
- * And another command had the criteria: action=TwoPieceMoveLeft, location=left
- * Then the second command would have a higher priority because it is more specific.
- * If two commands the in specificity a random one is chosen - * @return the corresponding the command of the desired location and action - */ - public Command getAutoCommand() { - List commands = getCommandMap().keySet().stream().filter(event -> event.isAutoEventValid(new AutoEvent(getAction(), getLocation()))).collect(Collectors.toList()); - if (commands.isEmpty())return getDefaultCommand(); - //most specific - Optional specificCommand = commands.stream().filter(c -> c.getIncludedActions().size() != AutoAction.values().length && c.getIncludedLocations().size() != FieldLocation.values().length).findFirst(); - if (specificCommand.isPresent()) return getCommandMap().get(specificCommand.get()); - return getCommandMap().get(commands.get(0)); - } - - protected abstract Map getCommandMap(); - protected abstract CommandBase getDefaultCommand(); - protected abstract AutoAction getDefaultActionOption(); - protected abstract FieldLocation getDefaultLocationOption(); - -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java new file mode 100644 index 0000000..5a5c7b7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; + +import java.util.Arrays; + +public class Nt4AutoEventProvider implements AutoEventProvider { + + private final SendableChooser actionChooser; + private final SendableChooser locationChooser; + + private final AutoAction defaultAutoAction; + private final FieldLocation defaultFieldLocation; + + public Nt4AutoEventProvider(AutoAction defaultAutoAction, FieldLocation defaultFieldLocation) { + this.defaultAutoAction = defaultAutoAction; + this.defaultFieldLocation = defaultFieldLocation; + this.actionChooser = new SendableChooser<>(); + this.locationChooser = new SendableChooser<>(); + Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); + Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); + actionChooser.setDefaultOption(getDefaultActionOption().getName(), getDefaultActionOption()); + locationChooser.setDefaultOption(getDefaultLocationOption().name(), getDefaultLocationOption()); + } + public AutoAction getSelectedAction() { + return actionChooser.getSelected() == null ? getDefaultActionOption() : actionChooser.getSelected(); + } + + public FieldLocation getSelectedLocation() { + return locationChooser.getSelected() == null ? getDefaultLocationOption() : locationChooser.getSelected(); + } + + @Override + public AutoAction getDefaultActionOption() { + return defaultAutoAction; + } + + @Override + public FieldLocation getDefaultLocationOption() { + return defaultFieldLocation; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java new file mode 100644 index 0000000..f9102a7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4048.common.auto; + +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.List; +import java.util.Optional; +import java.util.stream.Collectors; + +public abstract class PrioritizedAutoChooser implements AutoChooser { + /** + * This method by default will pick the more specific command over the more generic one. + * For example if a command had the following criteria: action=TwoPieceMoveLeft, location=any
+ * And another command had the criteria: action=TwoPieceMoveLeft, location=left
+ * Then the second command would have a higher priority because it is more specific.
+ * If two commands the in specificity a random one is chosen + * @return the corresponding the command of the desired location and action + */ + public Command getAutoCommand() { + List commands = getCommandMap() + .keySet().stream() + .filter(event -> event.isAutoEventValid(new AutoEvent(getAutoEventProvider().getSelectedAction(),getAutoEventProvider().getSelectedLocation()))) + .collect(Collectors.toList() + ); + if (commands.isEmpty()) return getDefaultCommand(); + //most specific + Optional specificCommand = commands.stream() + .filter(c -> c.getIncludedActions().size() != AutoAction.values().length && + c.getIncludedLocations().size() != FieldLocation.values().length) + .findFirst(); + if (specificCommand.isPresent()) return getCommandMap().get(specificCommand.get()); + return getCommandMap().get(commands.get(0)); + } + +} From f6b137093c113fb8a0b1ac4b747bcad1d59589ae Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 13 Dec 2023 22:58:53 -0500 Subject: [PATCH 23/28] moved autochooser code into packages --- .../frc4048/common/auto/{ => chooser}/AutoChooser.java | 6 +++--- .../common/auto/{ => chooser}/ExampleAutoChooser.java | 7 ++++++- .../common/auto/{ => chooser}/PrioritizedAutoChooser.java | 6 +++++- .../usfirst/frc4048/common/auto/{ => event}/AutoEvent.java | 5 ++++- .../frc4048/common/auto/{ => event}/AutoEventComparer.java | 5 ++++- .../frc4048/common/auto/{ => event}/AutoEventProvider.java | 5 ++++- .../common/auto/{ => event}/Nt4AutoEventProvider.java | 4 +++- 7 files changed, 29 insertions(+), 9 deletions(-) rename src/main/java/org/usfirst/frc4048/common/auto/{ => chooser}/AutoChooser.java (64%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => chooser}/ExampleAutoChooser.java (68%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => chooser}/PrioritizedAutoChooser.java (85%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => event}/AutoEvent.java (72%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => event}/AutoEventComparer.java (95%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => event}/AutoEventProvider.java (57%) rename src/main/java/org/usfirst/frc4048/common/auto/{ => event}/Nt4AutoEventProvider.java (91%) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java similarity index 64% rename from src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java rename to src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java index 8615942..dec0cd8 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java @@ -1,11 +1,11 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.chooser; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; +import org.usfirst.frc4048.common.auto.event.AutoEventComparer; +import org.usfirst.frc4048.common.auto.event.AutoEventProvider; import java.util.*; -import java.util.stream.Collectors; public interface AutoChooser { Command getAutoCommand(); diff --git a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java similarity index 68% rename from src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java rename to src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java index f3a83c7..64f0f50 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java @@ -1,7 +1,12 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.chooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; +import org.usfirst.frc4048.common.auto.PlaceHolderCommand; +import org.usfirst.frc4048.common.auto.event.AutoEventComparer; +import org.usfirst.frc4048.common.auto.event.AutoEventProvider; import java.util.Map; diff --git a/src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java similarity index 85% rename from src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java rename to src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java index f9102a7..a04b46d 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/PrioritizedAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java @@ -1,6 +1,10 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.chooser; import edu.wpi.first.wpilibj2.command.Command; +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; +import org.usfirst.frc4048.common.auto.event.AutoEvent; +import org.usfirst.frc4048.common.auto.event.AutoEventComparer; import java.util.List; import java.util.Optional; diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java similarity index 72% rename from src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java rename to src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java index 4fe25d3..df708b8 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoEvent.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java @@ -1,4 +1,7 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.event; + +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; public class AutoEvent { private final AutoAction action; diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java similarity index 95% rename from src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java rename to src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java index 3e2845b..b06b3aa 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventComparer.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java @@ -1,4 +1,7 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.event; + +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; import java.util.Arrays; import java.util.List; diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java similarity index 57% rename from src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java rename to src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java index e7d1d0b..f2f2194 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java @@ -1,4 +1,7 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.event; + +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; public interface AutoEventProvider { AutoAction getSelectedAction(); diff --git a/src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java similarity index 91% rename from src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java rename to src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java index 5a5c7b7..cbc1599 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/Nt4AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java @@ -1,6 +1,8 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.auto.event; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import org.usfirst.frc4048.common.auto.AutoAction; +import org.usfirst.frc4048.common.auto.FieldLocation; import java.util.Arrays; From 54a0ca89603c7cf1930651fe247ec53395d57f11 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 13 Dec 2023 23:02:33 -0500 Subject: [PATCH 24/28] added unimportant comment --- .../org/usfirst/frc4048/common/auto/PlaceHolderCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java b/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java index b006b08..e69c0ad 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java @@ -5,7 +5,7 @@ import java.util.Set; import java.util.function.BooleanSupplier; - +//Why is CommandBase abstract if it has not abstract methods???? public class PlaceHolderCommand extends CommandBase{ } From 740d5961cfd228cf945ba73d3dc4380d6e26fc81 Mon Sep 17 00:00:00 2001 From: Noah Date: Fri, 15 Dec 2023 15:08:36 -0500 Subject: [PATCH 25/28] replace null return in ExampleAutoChooser with an Nt4AutoEventProvider --- .../frc4048/common/auto/chooser/ExampleAutoChooser.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java index 64f0f50..9728116 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java @@ -7,6 +7,7 @@ import org.usfirst.frc4048.common.auto.PlaceHolderCommand; import org.usfirst.frc4048.common.auto.event.AutoEventComparer; import org.usfirst.frc4048.common.auto.event.AutoEventProvider; +import org.usfirst.frc4048.common.auto.event.Nt4AutoEventProvider; import java.util.Map; @@ -27,7 +28,7 @@ public CommandBase getDefaultCommand() { @Override public AutoEventProvider getAutoEventProvider() { - return null; + return new Nt4AutoEventProvider(AutoAction.DoNothing,FieldLocation.Middle); } } From 47fc8a8c939b385116d0de30a8c5ad113ded9ce6 Mon Sep 17 00:00:00 2001 From: Noah Date: Fri, 15 Dec 2023 15:34:33 -0500 Subject: [PATCH 26/28] add AutoAction values and added middle location as an options --- .../frc4048/common/auto/AutoAction.java | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java index 59ee75e..9c395cc 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java +++ b/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java @@ -2,20 +2,27 @@ import edu.wpi.first.wpilibj.DriverStation; +import java.io.InvalidObjectException; +import java.security.InvalidParameterException; + public enum AutoAction { - DoNothing("Do Nothing",0,0), - TwoPieceMoveLeft("Two Piece Move Left",20,150); + DoNothing("Do Nothing",0,0,0), + CrossLine("Cross The Line",30,108,186), + TwoPieceMoveLeft("Two Piece Move Left",20,108,150), + OnePieceMoveLeft("One Piece Move Left",20,108,150); private final int rightLocation; private final int leftLocation; + private final int middleLocation; private final String name; private static final int RED_ALLIANCE_YPOS = 99; private static final int BLUE_ALLIANCE_YPOS = 0; - AutoAction(String name,int rightLocation, int leftLocation) { + AutoAction(String name,int rightLocation,int middleLocation, int leftLocation) { this.name = name; this.rightLocation = rightLocation; this.leftLocation = leftLocation; + this.middleLocation = middleLocation; } public int getRightLocation() { @@ -26,18 +33,24 @@ public int getLeftLocation() { return leftLocation; } + public int getMiddleLocation() { + return middleLocation; + } + public String getName() { return name; } - public int getYCord(FieldLocation location, DriverStation.Alliance alliance){ + public int getNormalizedYCord(FieldLocation location, DriverStation.Alliance alliance){ int y = alliance.equals(DriverStation.Alliance.Red) ? RED_ALLIANCE_YPOS : BLUE_ALLIANCE_YPOS; switch (location){ case Left: return y + getLeftLocation(); case Right: return y + getRightLocation(); - default: return y + 180;// idk where this came from + case Middle: + if (getMiddleLocation() !=-1) return y + getMiddleLocation(); + default: throw new InvalidParameterException(String.format("FieldLocation must be of types {%s,%s,%s} and location must not be -1",FieldLocation.Left,FieldLocation.Right,FieldLocation.Middle)); } } } From 19a9c2c7c84de4526ff966cc30471c1ebdc7bfad Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 14 Jan 2024 14:44:48 -0500 Subject: [PATCH 27/28] pulled from branch and renamed package --- .../common/auto/chooser/AutoChooser.java | 16 ----- .../auto/chooser/ExampleAutoChooser.java | 34 ---------- .../auto/chooser/PrioritizedAutoChooser.java | 38 ----------- .../frc4048/common/auto/event/AutoEvent.java | 23 ------- .../common/auto/event/AutoEventComparer.java | 63 ------------------- .../{auto => autochooser}/AutoAction.java | 3 +- .../{auto => autochooser}/FieldLocation.java | 2 +- .../PlaceHolderCommand.java | 5 +- .../autochooser/chooser/AutoChooser.java | 10 +++ .../chooser/ExampleAutoChooser.java | 18 ++++++ .../autochooser/chooser/Nt4AutoChooser.java | 15 +++++ .../common/autochooser/event/AutoEvent.java | 39 ++++++++++++ .../event/AutoEventProvider.java | 7 ++- .../event/Nt4AutoEventProvider.java | 12 +++- 14 files changed, 98 insertions(+), 187 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java delete mode 100644 src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java rename src/main/java/org/usfirst/frc4048/common/{auto => autochooser}/AutoAction.java (95%) rename src/main/java/org/usfirst/frc4048/common/{auto => autochooser}/FieldLocation.java (57%) rename src/main/java/org/usfirst/frc4048/common/{auto => autochooser}/PlaceHolderCommand.java (50%) create mode 100644 src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java create mode 100644 src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java rename src/main/java/org/usfirst/frc4048/common/{auto => autochooser}/event/AutoEventProvider.java (57%) rename src/main/java/org/usfirst/frc4048/common/{auto => autochooser}/event/Nt4AutoEventProvider.java (78%) diff --git a/src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java deleted file mode 100644 index dec0cd8..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/chooser/AutoChooser.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.usfirst.frc4048.common.auto.chooser; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import org.usfirst.frc4048.common.auto.event.AutoEventComparer; -import org.usfirst.frc4048.common.auto.event.AutoEventProvider; - -import java.util.*; - -public interface AutoChooser { - Command getAutoCommand(); - Map getCommandMap(); - CommandBase getDefaultCommand(); - AutoEventProvider getAutoEventProvider(); - -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java deleted file mode 100644 index 9728116..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/chooser/ExampleAutoChooser.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc4048.common.auto.chooser; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; -import org.usfirst.frc4048.common.auto.PlaceHolderCommand; -import org.usfirst.frc4048.common.auto.event.AutoEventComparer; -import org.usfirst.frc4048.common.auto.event.AutoEventProvider; -import org.usfirst.frc4048.common.auto.event.Nt4AutoEventProvider; - -import java.util.Map; - -public class ExampleAutoChooser extends PrioritizedAutoChooser { - - @Override - public Map getCommandMap() { - return Map.of( - AutoEventComparer.fromAction(AutoAction.DoNothing), new PlaceHolderCommand(), - AutoEventComparer.fromActionAndLocation(AutoAction.TwoPieceMoveLeft, FieldLocation.Middle), new PlaceHolderCommand() - ); - } - - @Override - public CommandBase getDefaultCommand() { - return new PlaceHolderCommand(); - } - - @Override - public AutoEventProvider getAutoEventProvider() { - return new Nt4AutoEventProvider(AutoAction.DoNothing,FieldLocation.Middle); - } - -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java deleted file mode 100644 index a04b46d..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/chooser/PrioritizedAutoChooser.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4048.common.auto.chooser; - -import edu.wpi.first.wpilibj2.command.Command; -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; -import org.usfirst.frc4048.common.auto.event.AutoEvent; -import org.usfirst.frc4048.common.auto.event.AutoEventComparer; - -import java.util.List; -import java.util.Optional; -import java.util.stream.Collectors; - -public abstract class PrioritizedAutoChooser implements AutoChooser { - /** - * This method by default will pick the more specific command over the more generic one. - * For example if a command had the following criteria: action=TwoPieceMoveLeft, location=any
- * And another command had the criteria: action=TwoPieceMoveLeft, location=left
- * Then the second command would have a higher priority because it is more specific.
- * If two commands the in specificity a random one is chosen - * @return the corresponding the command of the desired location and action - */ - public Command getAutoCommand() { - List commands = getCommandMap() - .keySet().stream() - .filter(event -> event.isAutoEventValid(new AutoEvent(getAutoEventProvider().getSelectedAction(),getAutoEventProvider().getSelectedLocation()))) - .collect(Collectors.toList() - ); - if (commands.isEmpty()) return getDefaultCommand(); - //most specific - Optional specificCommand = commands.stream() - .filter(c -> c.getIncludedActions().size() != AutoAction.values().length && - c.getIncludedLocations().size() != FieldLocation.values().length) - .findFirst(); - if (specificCommand.isPresent()) return getCommandMap().get(specificCommand.get()); - return getCommandMap().get(commands.get(0)); - } - -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java deleted file mode 100644 index df708b8..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEvent.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.usfirst.frc4048.common.auto.event; - -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; - -public class AutoEvent { - private final AutoAction action; - private final FieldLocation location; - ; - - public AutoEvent(AutoAction action, FieldLocation location) { - this.action = action; - this.location = location; - } - - public AutoAction getAction() { - return action; - } - - public FieldLocation getLocation() { - return location; - } -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java b/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java deleted file mode 100644 index b06b3aa..0000000 --- a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventComparer.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.usfirst.frc4048.common.auto.event; - -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; - -import java.util.Arrays; -import java.util.List; -import java.util.stream.Collectors; - -public class AutoEventComparer { - private final List includedActions; - private final List includedLocations; - - private AutoEventComparer(List includedActions, List includedLocations) { - this.includedActions = includedActions; - this.includedLocations = includedLocations; - } - public static AutoEventComparer fromActions(List includedActions) { - List locations = Arrays.stream(FieldLocation.values()).collect(Collectors.toList()); - return new AutoEventComparer(includedActions,locations); - } - public static AutoEventComparer fromLocations(List includedLocations) { - List actions = Arrays.stream(AutoAction.values()).collect(Collectors.toList()); - return new AutoEventComparer(actions,includedLocations); - } - public static AutoEventComparer fromActionsAndLocations(List includedActions, List includedLocations) { - return new AutoEventComparer(includedActions,includedLocations); - } - public static AutoEventComparer fromExcludedAction(List excludedActions){ - List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); - return new AutoEventComparer(actions,Arrays.asList(FieldLocation.values())); - } - public static AutoEventComparer fromExcludedLocation(List excludedLocations){ - List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); - return new AutoEventComparer(Arrays.asList(AutoAction.values()),locations); - } - public static AutoEventComparer fromExcludedActionAndLocation(List excludedActions, List excludedLocations){ - List actions = Arrays.stream(AutoAction.values()).filter(action -> !excludedActions.contains(action)).collect(Collectors.toList()); - List locations = Arrays.stream(FieldLocation.values()).filter(location -> !excludedLocations.contains(location)).collect(Collectors.toList()); - return new AutoEventComparer(actions,locations); - } - public static AutoEventComparer fromAction(AutoAction action){ - return new AutoEventComparer(List.of(action),Arrays.asList(FieldLocation.values())); - } - public static AutoEventComparer fromIncludedLocation(FieldLocation location){ - return new AutoEventComparer(Arrays.asList(AutoAction.values()),List.of(location)); - } - public static AutoEventComparer fromActionAndLocation(AutoAction action, FieldLocation location){ - return new AutoEventComparer(List.of(action),List.of(location)); - } - - public boolean isAutoEventValid(AutoEvent event){ - return includedActions.contains(event.getAction()) && includedLocations.contains(event.getLocation()); - } - - public List getIncludedActions() { - return List.copyOf(includedActions); - } - - public List getIncludedLocations() { - return List.copyOf(includedLocations); - } -} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java b/src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java similarity index 95% rename from src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java rename to src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java index 9c395cc..7a9c272 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/AutoAction.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java @@ -1,8 +1,7 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.autochooser; import edu.wpi.first.wpilibj.DriverStation; -import java.io.InvalidObjectException; import java.security.InvalidParameterException; public enum AutoAction { diff --git a/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java b/src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java similarity index 57% rename from src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java rename to src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java index 7e4bab6..e22c038 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/FieldLocation.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java @@ -1,4 +1,4 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.autochooser; public enum FieldLocation { Right, Middle, diff --git a/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java b/src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java similarity index 50% rename from src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java rename to src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java index e69c0ad..f9850c9 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/PlaceHolderCommand.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java @@ -1,10 +1,7 @@ -package org.usfirst.frc4048.common.auto; +package org.usfirst.frc4048.common.autochooser; -import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj2.command.*; -import java.util.Set; -import java.util.function.BooleanSupplier; //Why is CommandBase abstract if it has not abstract methods???? public class PlaceHolderCommand extends CommandBase{ diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java new file mode 100644 index 0000000..4b17c6a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java @@ -0,0 +1,10 @@ +package org.usfirst.frc4048.common.autochooser.chooser; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.auto.event.AutoEventProvider; + +public interface AutoChooser { + Command getAutoCommand(); + AutoEventProvider getAutoEventProvider(); + +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java new file mode 100644 index 0000000..b286d88 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java @@ -0,0 +1,18 @@ +package org.usfirst.frc4048.common.autochooser.chooser; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.auto.*; +import frc.robot.auto.event.AutoEvent; + +import java.util.Map; + +public class ExampleAutoChooser extends Nt4AutoChooser { + private final Map commandMap = Map.of( + new AutoEvent(AutoAction.TwoPieceMoveLeft,FieldLocation.Right), new PlaceHolderCommand(), + new AutoEvent(AutoAction.TwoPieceMoveLeft,FieldLocation.Left), new PlaceHolderCommand() + ); + @Override + public Command getAutoCommand() { + return commandMap.get(new AutoEvent(getAutoEventProvider().getSelectedAction(),getAutoEventProvider().getSelectedLocation())); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java new file mode 100644 index 0000000..67c73aa --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java @@ -0,0 +1,15 @@ +package org.usfirst.frc4048.common.autochooser.chooser; + +import frc.robot.auto.AutoAction; +import frc.robot.auto.FieldLocation; +import frc.robot.auto.event.AutoEventProvider; +import frc.robot.auto.event.Nt4AutoEventProvider; + +public abstract class Nt4AutoChooser implements AutoChooser{ + private final Nt4AutoEventProvider provider = new Nt4AutoEventProvider(AutoAction.DoNothing, FieldLocation.Middle); + @Override + public AutoEventProvider getAutoEventProvider() { + return provider; + } + +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java new file mode 100644 index 0000000..97e131a --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java @@ -0,0 +1,39 @@ +package frc.robot.auto.event; + + +import frc.robot.auto.AutoAction; +import frc.robot.auto.FieldLocation; + +import java.util.Objects; + +public class AutoEvent { + private final AutoAction action; + private final FieldLocation location; + ; + + public AutoEvent(AutoAction action, FieldLocation location) { + this.action = action; + this.location = location; + } + + public AutoAction getAction() { + return action; + } + + public FieldLocation getLocation() { + return location; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + AutoEvent autoEvent = (AutoEvent) o; + return action == autoEvent.action && location == autoEvent.location; + } + + @Override + public int hashCode() { + return Objects.hash(action, location); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java similarity index 57% rename from src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java rename to src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java index f2f2194..d95ba4a 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/event/AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java @@ -1,7 +1,8 @@ -package org.usfirst.frc4048.common.auto.event; +package frc.robot.auto.event; -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; + +import frc.robot.auto.AutoAction; +import frc.robot.auto.FieldLocation; public interface AutoEventProvider { AutoAction getSelectedAction(); diff --git a/src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java similarity index 78% rename from src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java rename to src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java index cbc1599..d2bf04b 100644 --- a/src/main/java/org/usfirst/frc4048/common/auto/event/Nt4AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java @@ -1,8 +1,10 @@ -package org.usfirst.frc4048.common.auto.event; +package frc.robot.auto.event; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import org.usfirst.frc4048.common.auto.AutoAction; -import org.usfirst.frc4048.common.auto.FieldLocation; +import frc.robot.auto.AutoAction; +import frc.robot.auto.FieldLocation; import java.util.Arrays; @@ -23,6 +25,10 @@ public Nt4AutoEventProvider(AutoAction defaultAutoAction, FieldLocation defaultF Arrays.stream(FieldLocation.values()).forEach(l -> locationChooser.addOption(l.name(), l)); actionChooser.setDefaultOption(getDefaultActionOption().getName(), getDefaultActionOption()); locationChooser.setDefaultOption(getDefaultLocationOption().name(), getDefaultLocationOption()); + ShuffleboardTab autoTab = Shuffleboard.getTab("Auto"); + autoTab.add("Auto Action",actionChooser).withPosition(0,0).withSize(4,1); + autoTab.add("Location Chooser",locationChooser).withPosition(0,1).withSize(4,1);; + } public AutoAction getSelectedAction() { return actionChooser.getSelected() == null ? getDefaultActionOption() : actionChooser.getSelected(); From 83d21273f32aefafe774fe7cd0abf689be1bf88c Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 14 Jan 2024 14:56:25 -0500 Subject: [PATCH 28/28] refactored package and got rid of unneeded interface and abstraction --- .../autochooser/chooser/AutoChooser.java | 15 +++++++++++---- .../chooser/ExampleAutoChooser.java | 18 +++++++++++++----- .../autochooser/chooser/Nt4AutoChooser.java | 15 --------------- .../common/autochooser/event/AutoEvent.java | 7 ++++--- .../autochooser/event/AutoEventProvider.java | 6 +++--- .../event/Nt4AutoEventProvider.java | 6 +++--- 6 files changed, 34 insertions(+), 33 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java index 4b17c6a..80bdd3b 100644 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java @@ -1,10 +1,17 @@ package org.usfirst.frc4048.common.autochooser.chooser; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.auto.event.AutoEventProvider; +import org.usfirst.frc4048.common.autochooser.event.AutoEventProvider; -public interface AutoChooser { - Command getAutoCommand(); - AutoEventProvider getAutoEventProvider(); +public abstract class AutoChooser { + abstract Command getAutoCommand(); + private final AutoEventProvider provider; + public AutoChooser(AutoEventProvider provider) { + this.provider = provider; + } + + public AutoEventProvider getProvider() { + return provider; + } } diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java index b286d88..dcfc9bd 100644 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java @@ -1,18 +1,26 @@ package org.usfirst.frc4048.common.autochooser.chooser; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.auto.*; -import frc.robot.auto.event.AutoEvent; +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; +import org.usfirst.frc4048.common.autochooser.PlaceHolderCommand; +import org.usfirst.frc4048.common.autochooser.event.AutoEvent; +import org.usfirst.frc4048.common.autochooser.event.Nt4AutoEventProvider; import java.util.Map; -public class ExampleAutoChooser extends Nt4AutoChooser { +public class ExampleAutoChooser extends AutoChooser { private final Map commandMap = Map.of( - new AutoEvent(AutoAction.TwoPieceMoveLeft,FieldLocation.Right), new PlaceHolderCommand(), + new AutoEvent(AutoAction.TwoPieceMoveLeft, FieldLocation.Right), new PlaceHolderCommand(), new AutoEvent(AutoAction.TwoPieceMoveLeft,FieldLocation.Left), new PlaceHolderCommand() ); + + public ExampleAutoChooser() { + super(new Nt4AutoEventProvider(AutoAction.DoNothing, FieldLocation.Middle)); + } + @Override public Command getAutoCommand() { - return commandMap.get(new AutoEvent(getAutoEventProvider().getSelectedAction(),getAutoEventProvider().getSelectedLocation())); + return commandMap.get(new AutoEvent(getProvider().getSelectedAction(),getProvider().getSelectedLocation())); } } diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java deleted file mode 100644 index 67c73aa..0000000 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/Nt4AutoChooser.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.usfirst.frc4048.common.autochooser.chooser; - -import frc.robot.auto.AutoAction; -import frc.robot.auto.FieldLocation; -import frc.robot.auto.event.AutoEventProvider; -import frc.robot.auto.event.Nt4AutoEventProvider; - -public abstract class Nt4AutoChooser implements AutoChooser{ - private final Nt4AutoEventProvider provider = new Nt4AutoEventProvider(AutoAction.DoNothing, FieldLocation.Middle); - @Override - public AutoEventProvider getAutoEventProvider() { - return provider; - } - -} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java index 97e131a..b52688b 100644 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java @@ -1,8 +1,9 @@ -package frc.robot.auto.event; +package org.usfirst.frc4048.common.autochooser.event; -import frc.robot.auto.AutoAction; -import frc.robot.auto.FieldLocation; + +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; import java.util.Objects; diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java index d95ba4a..219a280 100644 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java @@ -1,8 +1,8 @@ -package frc.robot.auto.event; +package org.usfirst.frc4048.common.autochooser.event; -import frc.robot.auto.AutoAction; -import frc.robot.auto.FieldLocation; +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; public interface AutoEventProvider { AutoAction getSelectedAction(); diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java index d2bf04b..f671153 100644 --- a/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java @@ -1,10 +1,10 @@ -package frc.robot.auto.event; +package org.usfirst.frc4048.common.autochooser.event; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc.robot.auto.AutoAction; -import frc.robot.auto.FieldLocation; +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; import java.util.Arrays;