diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..15546ce --- /dev/null +++ b/build.gradle @@ -0,0 +1,103 @@ +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' + // https://mvnrepository.com/artifact/org.mockito/mockito-core + testImplementation 'org.mockito:mockito-core:2.1.0' + +} + +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/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; +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java b/src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java new file mode 100644 index 0000000..7a9c272 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/AutoAction.java @@ -0,0 +1,55 @@ +package org.usfirst.frc4048.common.autochooser; + +import edu.wpi.first.wpilibj.DriverStation; + +import java.security.InvalidParameterException; + +public enum AutoAction { + 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 middleLocation, int leftLocation) { + this.name = name; + this.rightLocation = rightLocation; + this.leftLocation = leftLocation; + this.middleLocation = middleLocation; + } + + public int getRightLocation() { + return rightLocation; + } + + public int getLeftLocation() { + return leftLocation; + } + + public int getMiddleLocation() { + return middleLocation; + } + + public String getName() { + return name; + } + + 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(); + 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)); + } + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java b/src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java new file mode 100644 index 0000000..e22c038 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/FieldLocation.java @@ -0,0 +1,6 @@ +package org.usfirst.frc4048.common.autochooser; +public enum FieldLocation { + Right, + Middle, + Left; +} diff --git a/src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java b/src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java new file mode 100644 index 0000000..f9850c9 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/PlaceHolderCommand.java @@ -0,0 +1,8 @@ +package org.usfirst.frc4048.common.autochooser; + +import edu.wpi.first.wpilibj2.command.*; + +//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..80bdd3b --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/AutoChooser.java @@ -0,0 +1,17 @@ +package org.usfirst.frc4048.common.autochooser.chooser; + +import edu.wpi.first.wpilibj2.command.Command; +import org.usfirst.frc4048.common.autochooser.event.AutoEventProvider; + +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 new file mode 100644 index 0000000..dcfc9bd --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/chooser/ExampleAutoChooser.java @@ -0,0 +1,26 @@ +package org.usfirst.frc4048.common.autochooser.chooser; + +import edu.wpi.first.wpilibj2.command.Command; +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 AutoChooser { + private final Map commandMap = Map.of( + 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(getProvider().getSelectedAction(),getProvider().getSelectedLocation())); + } +} 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..b52688b --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEvent.java @@ -0,0 +1,40 @@ +package org.usfirst.frc4048.common.autochooser.event; + + + +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.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/autochooser/event/AutoEventProvider.java b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java new file mode 100644 index 0000000..219a280 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/AutoEventProvider.java @@ -0,0 +1,13 @@ +package org.usfirst.frc4048.common.autochooser.event; + + +import org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; + +public interface AutoEventProvider { + AutoAction getSelectedAction(); + FieldLocation getSelectedLocation(); + AutoAction getDefaultActionOption(); + FieldLocation getDefaultLocationOption(); + +} 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 new file mode 100644 index 0000000..f671153 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/autochooser/event/Nt4AutoEventProvider.java @@ -0,0 +1,50 @@ +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 org.usfirst.frc4048.common.autochooser.AutoAction; +import org.usfirst.frc4048.common.autochooser.FieldLocation; + +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()); + 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(); + } + + 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/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..3dae9dc 100644 --- a/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java +++ b/src/main/java/org/usfirst/frc4048/common/diag/DiagColorSensor.java @@ -7,12 +7,16 @@ 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; +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. @@ -21,28 +25,28 @@ public class DiagColorSensor implements Diagnosable { private ColorSensor colorsensor; private String name; - private NetworkTableEntry networkTableEntry; - private Map colorMap; + 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(); + colorMap = new HashMap<>(Arrays.stream(ColorValue.values()).map(colorValue -> new HashMap.SimpleEntry<>(colorValue, false)).collect(Collectors.toMap(Map.Entry::getKey, Map.Entry::getValue))); 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 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); } @@ -50,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/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/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/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/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/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/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/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/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/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/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(); + } +} 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/limelight/LimeLightVision.java b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java index c4fcb72..8ceafd2 100644 --- a/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java +++ b/src/main/java/org/usfirst/frc4048/common/limelight/LimeLightVision.java @@ -9,8 +9,8 @@ public class LimeLightVision { public static final int LED_BLINK = 2; public static final int LED_OFF = 1; - private double cameraHeight; - private double targetHeight; + /** The relative height of the target to the camera, for trig calculations */ + private double relativeHeight; private double cameraAngle; NetworkTable table; @@ -29,8 +29,7 @@ public class LimeLightVision { * @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.relativeHeight = targetHeight - cameraHeight; this.cameraAngle = cameraAngle; table = NetworkTableInstance.getDefault().getTable("limelight"); tv = table.getEntry("tv"); @@ -43,9 +42,8 @@ public LimeLightVision(double cameraHeight, double targetHeight, double cameraAn } // 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; + public LimeLightVision(boolean DO_NOT_USE_FOR_TESTING_ONLY, double cameraHeight, double targetHeight, double cameraAngle) { + this.relativeHeight = targetHeight - cameraHeight; this.cameraAngle = cameraAngle; } @@ -66,7 +64,16 @@ public CameraDistance getTargetDistance() { double x = tx.getDouble(0.0); double y = ty.getDouble(0.0); - return calcCameraDistance(x, y, targetHeight, cameraHeight, cameraAngle); + 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); } /** @@ -96,6 +103,10 @@ public void setLedOff() { 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. @@ -117,11 +128,21 @@ public double getStream() { /** * 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)); + 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; + } + } 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 0bc9483..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,15 +1,13 @@ -/*----------------------------------------------------------------------------*/ -/* 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; import java.util.HashMap; import java.util.Map; @@ -20,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()); */ @@ -34,16 +32,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 +119,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..af9e8cf 100644 --- a/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java +++ b/src/main/java/org/usfirst/frc4048/common/smartshuffleboard/SmartShuffleboardTab.java @@ -7,11 +7,11 @@ package org.usfirst.frc4048.common.smartshuffleboard; -import edu.wpi.first.networktables.NetworkTableEntry; +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 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,19 +36,19 @@ 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) { - NetworkTableEntry ntEntry= widget.getEntry(); + GenericEntry ntEntry= widget.getEntry(); ntEntry.setValue(value); } else @@ -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 { @@ -71,7 +72,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 @@ -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/swervev2/SwerveDrivetrain.java b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java new file mode 100644 index 0000000..f03af71 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/SwerveDrivetrain.java @@ -0,0 +1,81 @@ +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; + +//TODO change to test +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 = 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); + 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..23c20a5 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/EncodedSwerveSparkMax.java @@ -0,0 +1,15 @@ +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); + driveMotor.restoreFactoryDefaults(); + steerMotor.restoreFactoryDefaults(); + + driveMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + steerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + } +} 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..3628b26 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/GenericEncodedSwerve.java @@ -0,0 +1,84 @@ +package org.usfirst.frc4048.common.swervev2.components; + +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 { + 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; + } + } + public SwerveModulePosition getPosition(){ + return new SwerveModulePosition(getDriveEncPosition(),new Rotation2d(getSteerEncPosition())); + } +} 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..964f314 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/swervev2/components/SwerveMotorEncoder.java @@ -0,0 +1,17 @@ +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); + +} 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/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); + } +} 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/LedPanel.java b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java new file mode 100644 index 0000000..2584ae3 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/LedPanel.java @@ -0,0 +1,71 @@ +package org.usfirst.frc4048.common.util; + +import edu.wpi.first.wpilibj.DigitalOutput; + +public class LedPanel { + private int ID; + + private final DigitalOutput output1; + private final DigitalOutput output2; + private final DigitalOutput output3; + + public LedPanel() { + ID = 0; + + output1 = new DigitalOutput(1); + output2 = new DigitalOutput(2); + output3 = new DigitalOutput(3); + } + + public int getID() { + return ID; + } + + public void setID(int iD) { + ID = iD; + setPic(ID); + } + + 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); + } + } +} 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..4122197 100644 --- a/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java +++ b/src/main/java/org/usfirst/frc4048/common/util/MotorUtils.java @@ -10,39 +10,46 @@ * 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; + } } 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); + } +} 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()); 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); } 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