From 882a74e54e8cd46badf0bd33e43ea4f8fd8e918d Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Mon, 19 Aug 2024 18:50:24 -0400 Subject: [PATCH] add basic file structure --- geometry/build.gradle | 41 +++++ lib/build.gradle | 107 ----------- .../coppercore/constants/FieldConstants.java | 67 ------- .../java/coppercore/utils/AllianceUtil.java | 124 ------------- .../main/java/coppercore/utils/Deadband.java | 41 ----- .../java/coppercore/utils/FieldFinder.java | 169 ------------------ .../main/java/coppercore/utils/GeomUtil.java | 144 --------------- .../coppercore/utils/InterpolateDouble.java | 89 --------- .../java/coppercore/utils/SystemsTest.java | 63 ------- .../main/java/coppercore/utils/Tunable.java | 25 --- .../coppercore/utils/feedforward/TuneG.java | 49 ----- .../coppercore/utils/feedforward/TuneS.java | 47 ----- .../coppercore/utils/feedforward/TuneV.java | 73 -------- lib/vendordeps/WPILibNewCommands.json | 38 ---- settings.gradle | 37 +--- wpi_interface/build.gradle | 41 +++++ 16 files changed, 88 insertions(+), 1067 deletions(-) create mode 100644 geometry/build.gradle delete mode 100644 lib/build.gradle delete mode 100644 lib/src/main/java/coppercore/constants/FieldConstants.java delete mode 100644 lib/src/main/java/coppercore/utils/AllianceUtil.java delete mode 100644 lib/src/main/java/coppercore/utils/Deadband.java delete mode 100644 lib/src/main/java/coppercore/utils/FieldFinder.java delete mode 100644 lib/src/main/java/coppercore/utils/GeomUtil.java delete mode 100644 lib/src/main/java/coppercore/utils/InterpolateDouble.java delete mode 100644 lib/src/main/java/coppercore/utils/SystemsTest.java delete mode 100644 lib/src/main/java/coppercore/utils/Tunable.java delete mode 100644 lib/src/main/java/coppercore/utils/feedforward/TuneG.java delete mode 100644 lib/src/main/java/coppercore/utils/feedforward/TuneS.java delete mode 100644 lib/src/main/java/coppercore/utils/feedforward/TuneV.java delete mode 100644 lib/vendordeps/WPILibNewCommands.json create mode 100644 wpi_interface/build.gradle diff --git a/geometry/build.gradle b/geometry/build.gradle new file mode 100644 index 0000000..fe89829 --- /dev/null +++ b/geometry/build.gradle @@ -0,0 +1,41 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * This generated file contains a sample Java library project to get you started. + * For more details on building Java & JVM projects, please refer to https://docs.gradle.org/8.7/userguide/building_java_projects.html in the Gradle documentation. + */ + +plugins { + // Apply the java-library plugin for API and implementation separation. + id 'java-library' +} + +repositories { + // Use Maven Central for resolving dependencies. + mavenCentral() +} + +dependencies { + // Use JUnit Jupiter for testing. + testImplementation libs.junit.jupiter + + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + // This dependency is exported to consumers, that is to say found on their compile classpath. + api libs.commons.math3 + + // This dependency is used internally, and not exposed to consumers on their own compile classpath. + implementation libs.guava +} + +// Apply a specific Java toolchain to ease working on different environments. +java { + toolchain { + languageVersion = JavaLanguageVersion.of(17) + } +} + +tasks.named('test') { + // Use JUnit Platform for unit tests. + useJUnitPlatform() +} diff --git a/lib/build.gradle b/lib/build.gradle deleted file mode 100644 index 45d1178..0000000 --- a/lib/build.gradle +++ /dev/null @@ -1,107 +0,0 @@ -import java.text.SimpleDateFormat - -plugins { - // Apply the java-library plugin for API and implementation separation. - id 'java-library' - id "edu.wpi.first.GradleRIO" version "2024.3.2" - id "com.peterabeles.gversion" version "1.10" - id "com.diffplug.spotless" version "6.24.0" - id 'org.ajoberstar.grgit' version "5.2.1" -} - -repositories { - // Use Maven Central for resolving dependencies. - mavenCentral() - mavenLocal() -} - -def MAIN_CLASS = "coppercore" - -deploy { - targets { - roborio(getTargetTypeClass("RoboRIO")) { - team = project.frc.getTeamOrDefault(401) - debug = project.frc.getDebugOrDefault(false) - - artifacts { - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - - } - - frcStaticFileDeploy(getArtifactTypeClass("FileTreeArtifact")) { - files = project.fileTree('lib/src/main/deploy') - directory = "/home/lvuser/deploy" - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -def includeDesktopSupport = true - -dependencies { - // Use JUnit Jupiter for testing. - testImplementation libs.junit.jupiter - - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - - // This dependency is exported to consumers, that is to say found on their compile classpath. - api libs.commons.math3 - - // This dependency is used internally, and not exposed to consumers on their own compile classpath. - implementation libs.guava - - // WPILIB setup - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - 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() -} - -// Apply a specific Java toolchain to ease working on different environments. -java { - toolchain { - languageVersion = JavaLanguageVersion.of(17) - } - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 -} - -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -tasks.named('test') { - // Use JUnit Platform for unit tests. - useJUnitPlatform() -} - -// adds dependency files to large jar -jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE -} - -// deploy jar + tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) - -tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' -} diff --git a/lib/src/main/java/coppercore/constants/FieldConstants.java b/lib/src/main/java/coppercore/constants/FieldConstants.java deleted file mode 100644 index fac2a6f..0000000 --- a/lib/src/main/java/coppercore/constants/FieldConstants.java +++ /dev/null @@ -1,67 +0,0 @@ -package constants; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public final class FieldConstants { - public static final double lengthM = 16.451; - public static final double widthM = 8.211; - - public static final double midfieldLowThresholdM = 5.87; - public static final double midfieldHighThresholdM = 10.72; - - public static final Rotation2d ampHeading = new Rotation2d(-Math.PI / 2); - - public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); - public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); - public static final Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0); - public static final Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - - public static final Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI); - public static final Rotation2d redDownHeading = Rotation2d.fromRadians(0.0); - public static final Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - public static final Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0); - - public static final Rotation2d redSourceHeading = - new Rotation2d(Math.PI * 4 / 3); // 60 degrees - public static final Rotation2d blueSourceHeading = - new Rotation2d(Math.PI * 5 / 3); // 120 degrees - - public static final Translation2d fieldToRedSpeaker = - new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)); - - public static final Translation2d fieldToBlueSpeaker = - new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); - - public static final Pose2d robotAgainstBlueSpeaker = - new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); - - public static final Pose2d robotAgainstRedSpeaker = - new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0)); - - public static final Pose2d robotAgainstBlueSpeakerRight = - new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120)); - - public static final Pose2d robotAgainstRedSpeakerRight = - new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60)); - - public static final Pose2d robotAgainstBlueSpeakerLeft = - new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120)); - - public static final Pose2d robotAgainstRedSpeakerLeft = - new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60)); - - public static final Pose2d robotAgainstBluePodium = - new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180)); - - public static final Pose2d robotAgainstRedPodium = - new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0)); - - public static final Pose2d robotAgainstBlueAmpZone = - new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90)); - - public static final Pose2d robotAgainstRedAmpZone = - new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); -} diff --git a/lib/src/main/java/coppercore/utils/AllianceUtil.java b/lib/src/main/java/coppercore/utils/AllianceUtil.java deleted file mode 100644 index f9cdfc3..0000000 --- a/lib/src/main/java/coppercore/utils/AllianceUtil.java +++ /dev/null @@ -1,124 +0,0 @@ -package coppercore.utils; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import constants.FieldConstants; - -public class AllianceUtil { - - public static Translation2d getFieldToSpeaker() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.fieldToBlueSpeaker; - case Red: - return FieldConstants.fieldToRedSpeaker; - } - } - return FieldConstants.fieldToRedSpeaker; - } - - public static Rotation2d getAmpHeading() { - return FieldConstants.ampHeading; - } - - public static Pose2d getPoseAgainstSpeaker() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.robotAgainstBlueSpeaker; - case Red: - return FieldConstants.robotAgainstRedSpeaker; - } - } - return FieldConstants.robotAgainstRedSpeaker; - } - - public static Pose2d getPoseAgainstSpeakerLeft() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.robotAgainstBlueSpeakerLeft; - case Red: - return FieldConstants.robotAgainstRedSpeakerLeft; - } - } - return FieldConstants.robotAgainstRedSpeakerLeft; - } - - public static Pose2d getPoseAgainstSpeakerRight() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.robotAgainstBlueSpeakerRight; - case Red: - return FieldConstants.robotAgainstRedSpeakerRight; - } - } - return FieldConstants.robotAgainstRedSpeakerRight; - } - - public static Pose2d getPoseAgainstPodium() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.robotAgainstBluePodium; - case Red: - return FieldConstants.robotAgainstRedPodium; - } - } - return FieldConstants.robotAgainstRedPodium; - } - - public static Pose2d getPoseAgainstAmpZone() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.robotAgainstRedAmpZone; - case Red: - return FieldConstants.robotAgainstBlueAmpZone; - } - } - return FieldConstants.robotAgainstRedAmpZone; - } - - public static Rotation2d getSourceHeading() { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return FieldConstants.blueSourceHeading; - case Red: - return FieldConstants.redSourceHeading; - } - } - return FieldConstants.redSourceHeading; - } - - /** Returns whether the speaker is significantly to the robot's left */ - public static boolean isLeftOfSpeaker(double robotY, double tolerance) { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return robotY > FieldConstants.fieldToBlueSpeaker.getY() + tolerance; - case Red: - return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; - } - } - return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; - } - - /** Returns whether the speaker is significantly to the robot's right */ - public static boolean isRightOfSpeaker(double robotY, double tolerance) { - if (!DriverStation.getAlliance().isEmpty()) { - switch (DriverStation.getAlliance().get()) { - case Blue: - return robotY < FieldConstants.fieldToBlueSpeaker.getY() - tolerance; - case Red: - return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; - } - } - return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; - } -} diff --git a/lib/src/main/java/coppercore/utils/Deadband.java b/lib/src/main/java/coppercore/utils/Deadband.java deleted file mode 100644 index 3794aa0..0000000 --- a/lib/src/main/java/coppercore/utils/Deadband.java +++ /dev/null @@ -1,41 +0,0 @@ -package coppercore.utils; - -public class Deadband { - // 1D Deadband in linear format - public static double oneAxisDeadband(double input, double deadband) { - if (Math.abs(input) < deadband) { - return 0; - } else { - return input; - } - } - - // 2D Deadband in a circular format - public static double[] twoAxisDeadband(double inputX, double inputY, double deadband) { - double[] output = new double[2]; - if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2)) < deadband) { - output[0] = 0; - output[1] = 0; - } else { - output[0] = inputX; - output[1] = inputY; - } - return output; - } - - // 3D Deadband in a spherical format - public static double[] threeAxisDeadband( - double inputX, double inputY, double inputZ, double deadband) { - double[] output = new double[3]; - if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2) + Math.pow(inputZ, 2)) < deadband) { - output[0] = 0; - output[1] = 0; - output[2] = 0; - } else { - output[0] = inputX; - output[1] = inputY; - output[2] = inputZ; - } - return output; - } -} diff --git a/lib/src/main/java/coppercore/utils/FieldFinder.java b/lib/src/main/java/coppercore/utils/FieldFinder.java deleted file mode 100644 index 304cf5d..0000000 --- a/lib/src/main/java/coppercore/utils/FieldFinder.java +++ /dev/null @@ -1,169 +0,0 @@ -package coppercore.utils; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import java.awt.geom.Line2D; - -public class FieldFinder { - private class FieldLocations2024 { - public static final double STAGE_RED_NEAR_SIDE_X = 13.6; // TODO: Make these less arbitrary - public static final double STAGE_RED_NEAR_SIDE_Y = 4.0; - public static final double STAGE_RED_LEFT_SIDE_X = 10.8; - public static final double STAGE_RED_LEFT_SIDE_Y = 2.2; - public static final double STAGE_RED_RIGHT_SIDE_X = 10.8; - public static final double STAGE_RED_RIGHT_SIDE_Y = 5.8; - - public static final double STAGE_BLUE_NEAR_SIDE_X = 3.0; - public static final double STAGE_BLUE_NEAR_SIDE_Y = 4.0; - public static final double STAGE_BLUE_LEFT_SIDE_X = 5.6; - public static final double STAGE_BLUE_LEFT_SIDE_Y = 5.8; - public static final double STAGE_BLUE_RIGHT_SIDE_X = 5.6; - public static final double STAGE_BLUE_RIGHT_SIDE_Y = 2.2; - - public static final double WING_RED_END_X = 12; // Actual 10.8 - - public static final double WING_BLUE_END_X = 4.6; // Actual 5.8 - } - - public enum FieldLocations { - RED_STAGE, - BLUE_STAGE, - RED_WING, - BLUE_WING, - MIDDLE - } - - /** - * This method is used to determine where the robot is on the field - * - * @param x The x position of the robot - * @param y The y position of the robot - * @return The location on the field - */ - public static FieldLocations whereAmI(double x, double y) { - - if (inTheTriangle( - x, - y, - FieldLocations2024.STAGE_RED_NEAR_SIDE_X, - FieldLocations2024.STAGE_RED_NEAR_SIDE_Y, - FieldLocations2024.STAGE_RED_LEFT_SIDE_X, - FieldLocations2024.STAGE_RED_LEFT_SIDE_Y, - FieldLocations2024.STAGE_RED_RIGHT_SIDE_X, - FieldLocations2024.STAGE_RED_RIGHT_SIDE_Y)) { - return FieldLocations.RED_STAGE; - } else if (inTheTriangle( - x, - y, - FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X, - FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y, - FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X, - FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y, - FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X, - FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y)) { - return FieldLocations.BLUE_STAGE; - } else if (x > FieldLocations2024.WING_RED_END_X) { - return FieldLocations.RED_WING; - } else if (x < FieldLocations2024.WING_BLUE_END_X) { - return FieldLocations.BLUE_WING; - } else { - return FieldLocations.MIDDLE; - } - } - - /** - * This method is used to determine where the robot is on the field - * - * @param pose The x pose of the robot - * @return The location on the field - */ - public static FieldLocations whereAmI(Pose2d pose) { - return whereAmI(pose.getX(), pose.getY()); - } - - /** - * This method is used to determine if the robot will hit a location on the field - * - * @param x The x position of the robot - * @param y The y position of the robot - * @param dx The change in, or delta, x of the robot (not to be confused with x velocity) - * @param dy The change in, or delta, y of the robot (not to be confused with y velocity) - * @param location The location on the field to check - * @return {@code true} if the robot will hit the location - */ - public static boolean willIHitThis( - double x, double y, double dx, double dy, FieldLocations location) { - switch (location) { - case RED_STAGE: - return willIHitTriangle( - x, - y, - dx, - dy, - FieldLocations2024.STAGE_RED_NEAR_SIDE_X, - FieldLocations2024.STAGE_RED_NEAR_SIDE_Y, - FieldLocations2024.STAGE_RED_LEFT_SIDE_X, - FieldLocations2024.STAGE_RED_LEFT_SIDE_Y, - FieldLocations2024.STAGE_RED_RIGHT_SIDE_X, - FieldLocations2024.STAGE_RED_RIGHT_SIDE_Y); - case BLUE_STAGE: - return willIHitTriangle( - x, - y, - dx, - dy, - FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X, - FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y, - FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X, - FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y, - FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X, - FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y); - case RED_WING: - return x > FieldLocations2024.WING_RED_END_X - || x + dx > FieldLocations2024.WING_RED_END_X; - case BLUE_WING: - return x < FieldLocations2024.WING_BLUE_END_X - || x + dx < FieldLocations2024.WING_BLUE_END_X; - case MIDDLE: - return (x < FieldLocations2024.WING_RED_END_X - || x + dx < FieldLocations2024.WING_RED_END_X) - && (x > FieldLocations2024.WING_BLUE_END_X - || x + dx > FieldLocations2024.WING_BLUE_END_X); - default: - return false; - } - } - - private static boolean willIHitTriangle( - double x, - double y, - double dx, - double dy, - double x1, - double y1, - double x2, - double y2, - double x3, - double y3) { - return inTheTriangle(x, y, x1, y1, x2, y2, x3, y3) - || Line2D.linesIntersect(x, y, x + dx, y + dy, x1, y1, x2, y2) - || Line2D.linesIntersect(x, y, x + dx, y + dy, x2, y2, x3, y3) - || Line2D.linesIntersect(x, y, x + dx, y + dy, x3, y3, x1, y1); - } - - private static boolean inTheTriangle( - double x, double y, double x1, double y1, double x2, double y2, double x3, double y3) { - double area = areaOfTriangle(x1, y1, x2, y2, x3, y3); - - double a1 = areaOfTriangle(x, y, x2, y2, x3, y3); - double a2 = areaOfTriangle(x1, y1, x, y, x3, y3); - double a3 = areaOfTriangle(x1, y1, x2, y2, x, y); - - return MathUtil.isNear(area, a1 + a2 + a3, 0.05); - } - - private static double areaOfTriangle( - double x1, double y1, double x2, double y2, double x3, double y3) { - return 0.5 * Math.abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)); - } -} diff --git a/lib/src/main/java/coppercore/utils/GeomUtil.java b/lib/src/main/java/coppercore/utils/GeomUtil.java deleted file mode 100644 index d55f0e7..0000000 --- a/lib/src/main/java/coppercore/utils/GeomUtil.java +++ /dev/null @@ -1,144 +0,0 @@ -package coppercore.utils; - -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; - -/** Geometry utilities for working with translations, rotations, transforms, and poses. */ -public class GeomUtil { - /** - * Creates a pure translating transform - * - * @param translation The translation to create the transform with - * @return The resulting transform - */ - public static Transform2d translationToTransform(Translation2d translation) { - return new Transform2d(translation, new Rotation2d()); - } - - /** - * Creates a pure translating transform - * - * @param x The x componenet of the translation - * @param y The y componenet of the translation - * @return The resulting transform - */ - public static Transform2d translationToTransform(double x, double y) { - return new Transform2d(new Translation2d(x, y), new Rotation2d()); - } - - /** - * Creates a pure rotating transform - * - * @param rotation The rotation to create the transform with - * @return The resulting transform - */ - public static Transform2d rotationToTransform(Rotation2d rotation) { - return new Transform2d(new Translation2d(), rotation); - } - - /** - * Converts a Pose2d to a Transform2d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform2d poseToTransform(Pose2d pose) { - return new Transform2d(pose.getTranslation(), pose.getRotation()); - } - - /** - * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose2d transformToPose(Transform2d transform) { - return new Pose2d(transform.getTranslation(), transform.getRotation()); - } - - /** - * Creates a pure translated pose - * - * @param translation The translation to create the pose with - * @return The resulting pose - */ - public static Pose2d translationToPose(Translation2d translation) { - return new Pose2d(translation, new Rotation2d()); - } - - /** - * Creates a pure rotated pose - * - * @param rotation The rotation to create the pose with - * @return The resulting pose - */ - public static Pose2d rotationToPose(Rotation2d rotation) { - return new Pose2d(new Translation2d(), rotation); - } - - /** - * Multiplies a twist by a scaling factor - * - * @param twist The twist to multiply - * @param factor The scaling factor for the twist components - * @return The new twist - */ - public static Twist2d multiplyTwist(Twist2d twist, double factor) { - return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); - } - - /** - * Converts a Pose3d to a Transform3d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform3d pose3dToTransform3d(Pose3d pose) { - return new Transform3d(pose.getTranslation(), pose.getRotation()); - } - - /** - * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose3d transform3dToPose3d(Transform3d transform) { - return new Pose3d(transform.getTranslation(), transform.getRotation()); - } - - /** - * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Y). chain - * - * @param transform The original translation - * @return The resulting translation - */ - public static Translation2d translation3dTo2dXY(Translation3d translation) { - return new Translation2d(translation.getX(), translation.getY()); - } - - /** - * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Z). chain - * - * @param transform The original translation - * @return The resulting translation - */ - public static Translation2d translation3dTo2dXZ(Translation3d translation) { - return new Translation2d(translation.getX(), translation.getZ()); - } -} diff --git a/lib/src/main/java/coppercore/utils/InterpolateDouble.java b/lib/src/main/java/coppercore/utils/InterpolateDouble.java deleted file mode 100644 index 9b64cf3..0000000 --- a/lib/src/main/java/coppercore/utils/InterpolateDouble.java +++ /dev/null @@ -1,89 +0,0 @@ -package coppercore.utils; - -import java.util.ArrayList; -import java.util.Collections; -import java.util.HashMap; - -public class InterpolateDouble { - private HashMap map; - private ArrayList sortedKeys; - - private final double minValue; - private final double maxValue; - - private final double minKey; - private final double maxKey; - - public InterpolateDouble(HashMap map) { - this(map, Double.MIN_VALUE, Double.MAX_VALUE); - } - - public InterpolateDouble(HashMap map, double minValue, double maxValue) { - this.map = map; - this.minValue = minValue; - this.maxValue = maxValue; - - sortedKeys = new ArrayList(); - for (Double k : map.keySet()) { - sortedKeys.add(k); - } - Collections.sort(sortedKeys); - - // Get lowest and highest keys of the HashMap - if (sortedKeys.size() > 0) { - minKey = sortedKeys.get(0); - maxKey = sortedKeys.get(sortedKeys.size() - 1); - } else { - throw new RuntimeException("Empty HashMap passed to InterpolateDouble"); - } - } - - /** - * Returns the interpolated value for the given key. If the key is not in the map, it will - * return the value for the closest key. - * - * @param key The key to interpolate - * @return The interpolated value - */ - public double getValue(double key) { - if (map.containsKey(key)) { - return map.get(key); - } - - // Ensure that key is within the bounds of the HashMap - if (key < minKey) { - return map.get(minKey); - } else if (key > maxKey) { - return map.get(maxKey); - } - - double lowerKey = 0; - double upperKey = 0; - for (double k : sortedKeys) { - if (k < key) { - lowerKey = k; - } else { - upperKey = k; - break; - } - } - - double lowerValue = map.get(lowerKey); - double upperValue = map.get(upperKey); - - // Edge case if keys equal each other - if (upperKey == lowerKey) { - upperKey += 0.01; - } - - double t = (key - lowerKey) / (upperKey - lowerKey); - double result = lowerValue * (1.0 - t) + t * upperValue; - if (result < minValue) { - return minValue; - } else if (result > maxValue) { - return maxValue; - } else { - return result; - } - } -} diff --git a/lib/src/main/java/coppercore/utils/SystemsTest.java b/lib/src/main/java/coppercore/utils/SystemsTest.java deleted file mode 100644 index a4727f4..0000000 --- a/lib/src/main/java/coppercore/utils/SystemsTest.java +++ /dev/null @@ -1,63 +0,0 @@ -// TODO: WIP - Not tested - -package coppercore.utils; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class SystemsTest extends Command { - private Tunable subsystem; - private boolean inverted; - - private int slot; - - private Timer timer; - - public SystemsTest(Tunable subsystem) { - this(subsystem, false, 0); - } - - public SystemsTest(Tunable subsystem, int slot) { - this(subsystem, false, slot); - } - - public SystemsTest(Tunable subsystem, boolean inverted) { - this(subsystem, inverted, 0); - } - - public SystemsTest(Tunable subsystem, boolean inverted, int slot) { - this.subsystem = subsystem; - this.inverted = inverted; - this.slot = slot; - - timer = new Timer(); - } - - @Override - public void initialize() { - subsystem.setVolts(inverted ? 0.5 : -0.5, slot); - timer.start(); - } - - @Override - public void execute() {} - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - if (timer.hasElapsed(0.5)) { - System.out.println( - "*********************************** Faliure with " - + subsystem.toString() - + " ***********************************"); - SmartDashboard.putBoolean("Test-Mode/" + subsystem.toString(), false); - return true; - } else if (Math.abs(subsystem.getVelocity(slot)) > 0.1) { - return true; - } - return false; - } -} diff --git a/lib/src/main/java/coppercore/utils/Tunable.java b/lib/src/main/java/coppercore/utils/Tunable.java deleted file mode 100644 index 5e354a5..0000000 --- a/lib/src/main/java/coppercore/utils/Tunable.java +++ /dev/null @@ -1,25 +0,0 @@ -package coppercore.utils; - -/** - * Interface for subsystems to be tuned. - * - *

Slots are used to differentiate between different motor-groups/io-elements in the same - * subsystem - */ -public interface Tunable { - public double getPosition(int slot); - - public double getVelocity(int slot); - - public double getConversionFactor(int slot); - - public void setVolts(double volts, int slot); - - public void setPID(double p, double i, double d, int slot); - - public void setFF(double kS, double kV, double kA, double kG, int slot); - - public void setMaxProfileProperties(double maxVelocity, double maxAcceleration, int slot); - - public void runToPosition(double position, int slot); -} diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java deleted file mode 100644 index bc8f3eb..0000000 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java +++ /dev/null @@ -1,49 +0,0 @@ -// TODO: WIP - Not tested - -package coppercore.utils.feedforward; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import coppercore.utils.Tunable; - -public class TuneG extends Command { - private Tunable subsystem; - - private int slot; - - double startPosition; - - double kG; - double kS; - - public TuneG(Tunable subsystem, int slot) { - this.subsystem = subsystem; - this.kS = SmartDashboard.getNumber("Test-Mode/kS", 0); - this.slot = slot; - - // this.withTimeout(5); TODO: Maybe add? - } - - @Override - public void initialize() { - startPosition = subsystem.getPosition(slot); - kG = kS; - } - - @Override - public void execute() { - subsystem.setVolts(kG, slot); - kG += 0.001; - } - - @Override - public void end(boolean interrupted) { - subsystem.setVolts(0.0, slot); - SmartDashboard.putNumber("Test-Mode/kG", kG - kS); - } - - @Override - public boolean isFinished() { - return subsystem.getPosition(slot) > Math.abs(startPosition - 0.1); - } -} diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java deleted file mode 100644 index 6819337..0000000 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java +++ /dev/null @@ -1,47 +0,0 @@ -// TODO: WIP - Not tested - -package coppercore.utils.feedforward; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import coppercore.utils.Tunable; - -public class TuneS extends Command { - private Tunable subsystem; - - private int slot; - - double startPosition; - - double appliedVolts; - - public TuneS(Tunable subsystem, int slot) { - this.subsystem = subsystem; - this.slot = slot; - - // this.withTimeout(5); TODO: Maybe add? - } - - @Override - public void initialize() { - startPosition = subsystem.getPosition(slot); - appliedVolts = 0; - } - - @Override - public void execute() { - subsystem.setVolts(appliedVolts, slot); - appliedVolts += 0.001; - } - - @Override - public void end(boolean interrupted) { - subsystem.setVolts(0.0, slot); - SmartDashboard.putNumber("Test-Mode/kS", appliedVolts); - } - - @Override - public boolean isFinished() { - return subsystem.getVelocity(slot) > 0.01; - } -} diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java deleted file mode 100644 index 7a7c0b2..0000000 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java +++ /dev/null @@ -1,73 +0,0 @@ -// TODO: WIP - Not tested - -package coppercore.utils.feedforward; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import coppercore.utils.Tunable; -import java.util.ArrayList; - -public class TuneV extends Command { - private Tunable subsystem; - - private double volts; - - private int slot; - private double conversionFactor; - - private ArrayList velocities; - - double startPosition; // TODO - - double kS; - double pastkV; - double average = 0; - double vel = 0; - - public TuneV(Tunable subsystem, double volts, int slot) { - this.subsystem = subsystem; - this.volts = volts; - this.slot = slot; - this.kS = SmartDashboard.getNumber("Test-Mode/kS", 0); - this.pastkV = SmartDashboard.getNumber("Test-Mode/kV", 0); - - this.conversionFactor = subsystem.getConversionFactor(slot); - - this.withTimeout(5); - } - - @Override - public void initialize() { - SmartDashboard.putBoolean("Test-Mode/Ended", false); - subsystem.setVolts(volts, slot); - velocities = new ArrayList(); - } - - @Override - public void execute() { - vel = subsystem.getVelocity(slot); - SmartDashboard.putNumber("Test-Mode/Velocity", vel); - if (Math.abs(subsystem.getPosition(slot)) < 0.6 * conversionFactor) { - velocities.add(vel); - } - } - - @Override - public void end(boolean interrupted) { - SmartDashboard.putBoolean("Test-Mode/Ended", true); - subsystem.setVolts(0.0, slot); - - for (double v : velocities) { - average += v; - } - - average /= velocities.size(); - - SmartDashboard.putNumber("Test-Mode/kV", ((volts - kS) / average) + pastkV); - } - - @Override - public boolean isFinished() { - return Math.abs(subsystem.getPosition(slot)) > 0.9 * conversionFactor; - } -} diff --git a/lib/vendordeps/WPILibNewCommands.json b/lib/vendordeps/WPILibNewCommands.json deleted file mode 100644 index 71b3953..0000000 --- a/lib/vendordeps/WPILibNewCommands.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", - "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" - ] - } - ] - } \ No newline at end of file diff --git a/settings.gradle b/settings.gradle index ec700e1..2dd484a 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,31 +1,9 @@ -import org.gradle.internal.os.OperatingSystem - -// manage wpilib path structure -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - 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 - } - } -} +/* + * This file was generated by the Gradle 'init' task. + * + * The settings file is used to specify which projects to include in your build. + * For more detailed information on multi-project builds, please refer to https://docs.gradle.org/8.7/userguide/multi_project_builds.html in the Gradle documentation. + */ plugins { // Apply the foojay-resolver plugin to allow automatic download of JDKs @@ -34,6 +12,3 @@ plugins { rootProject.name = 'coppercore' include('lib') - -Properties props = System.getProperties(); -props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); \ No newline at end of file diff --git a/wpi_interface/build.gradle b/wpi_interface/build.gradle new file mode 100644 index 0000000..fe89829 --- /dev/null +++ b/wpi_interface/build.gradle @@ -0,0 +1,41 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * This generated file contains a sample Java library project to get you started. + * For more details on building Java & JVM projects, please refer to https://docs.gradle.org/8.7/userguide/building_java_projects.html in the Gradle documentation. + */ + +plugins { + // Apply the java-library plugin for API and implementation separation. + id 'java-library' +} + +repositories { + // Use Maven Central for resolving dependencies. + mavenCentral() +} + +dependencies { + // Use JUnit Jupiter for testing. + testImplementation libs.junit.jupiter + + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + // This dependency is exported to consumers, that is to say found on their compile classpath. + api libs.commons.math3 + + // This dependency is used internally, and not exposed to consumers on their own compile classpath. + implementation libs.guava +} + +// Apply a specific Java toolchain to ease working on different environments. +java { + toolchain { + languageVersion = JavaLanguageVersion.of(17) + } +} + +tasks.named('test') { + // Use JUnit Platform for unit tests. + useJUnitPlatform() +}