From 1034de870c711076434de40ccd5f35ab2d56830d Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 16 Apr 2024 18:29:02 -0400 Subject: [PATCH 1/7] add utils folder from 2024-robot and add WPILib dependencies + plugin --- lib/build.gradle.kts | 8 +- lib/src/main/java/org/example/Library.java | 10 - lib/src/main/java/org/utils/AllianceUtil.java | 130 +++++++++++ lib/src/main/java/org/utils/Deadband.java | 41 ++++ lib/src/main/java/org/utils/FieldFinder.java | 169 +++++++++++++++ lib/src/main/java/org/utils/GeomUtil.java | 144 +++++++++++++ .../java/org/utils/InterpolateDouble.java | 89 ++++++++ lib/src/main/java/org/utils/SystemsTest.java | 63 ++++++ lib/src/main/java/org/utils/Tunable.java | 25 +++ .../java/org/utils/WaitForButtonCommand.java | 32 +++ .../java/org/utils/feedforward/TuneG.java | 49 +++++ .../java/org/utils/feedforward/TuneS.java | 47 ++++ .../java/org/utils/feedforward/TuneV.java | 73 +++++++ .../java/org/utils/notesimulator/Note.java | 204 ++++++++++++++++++ .../org/utils/notesimulator/NoteManager.java | 58 +++++ 15 files changed, 1131 insertions(+), 11 deletions(-) delete mode 100644 lib/src/main/java/org/example/Library.java create mode 100644 lib/src/main/java/org/utils/AllianceUtil.java create mode 100644 lib/src/main/java/org/utils/Deadband.java create mode 100644 lib/src/main/java/org/utils/FieldFinder.java create mode 100644 lib/src/main/java/org/utils/GeomUtil.java create mode 100644 lib/src/main/java/org/utils/InterpolateDouble.java create mode 100644 lib/src/main/java/org/utils/SystemsTest.java create mode 100644 lib/src/main/java/org/utils/Tunable.java create mode 100644 lib/src/main/java/org/utils/WaitForButtonCommand.java create mode 100644 lib/src/main/java/org/utils/feedforward/TuneG.java create mode 100644 lib/src/main/java/org/utils/feedforward/TuneS.java create mode 100644 lib/src/main/java/org/utils/feedforward/TuneV.java create mode 100644 lib/src/main/java/org/utils/notesimulator/Note.java create mode 100644 lib/src/main/java/org/utils/notesimulator/NoteManager.java diff --git a/lib/build.gradle.kts b/lib/build.gradle.kts index 7a236d1..f4e383c 100644 --- a/lib/build.gradle.kts +++ b/lib/build.gradle.kts @@ -7,7 +7,9 @@ plugins { // Apply the java-library plugin for API and implementation separation. - `java-library` + id `java-library` + // WPILib plugin + id `edu.wpi.first.GradleRIO` version `2024.3.2` } repositories { @@ -26,6 +28,10 @@ dependencies { // This dependency is used internally, and not exposed to consumers on their own compile classpath. implementation(libs.guava) + + // WPILib dependencies + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() } // Apply a specific Java toolchain to ease working on different environments. diff --git a/lib/src/main/java/org/example/Library.java b/lib/src/main/java/org/example/Library.java deleted file mode 100644 index b98461b..0000000 --- a/lib/src/main/java/org/example/Library.java +++ /dev/null @@ -1,10 +0,0 @@ -/* - * This source file was generated by the Gradle 'init' task - */ -package org.example; - -public class Library { - public boolean someLibraryMethod() { - return true; - } -} diff --git a/lib/src/main/java/org/utils/AllianceUtil.java b/lib/src/main/java/org/utils/AllianceUtil.java new file mode 100644 index 0000000..278a1ec --- /dev/null +++ b/lib/src/main/java/org/utils/AllianceUtil.java @@ -0,0 +1,130 @@ +package frc.robot.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 frc.robot.Constants.FieldConstants; +import org.littletonrobotics.junction.Logger; + +public class AllianceUtil { + + public static Translation2d getFieldToSpeaker() { + if (!DriverStation.getAlliance().isEmpty()) { + switch (DriverStation.getAlliance().get()) { + case Blue: + Logger.recordOutput("Field/speaker", FieldConstants.fieldToBlueSpeaker); + return FieldConstants.fieldToBlueSpeaker; + case Red: + Logger.recordOutput("Field/speaker", FieldConstants.fieldToRedSpeaker); + return FieldConstants.fieldToRedSpeaker; + } + } + return FieldConstants.fieldToRedSpeaker; + } + + public static Rotation2d getAmpHeading() { + Logger.recordOutput("Field/amp", FieldConstants.ampHeading); + 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: + Logger.recordOutput("Field/source", FieldConstants.blueSourceHeading); + return FieldConstants.blueSourceHeading; + case Red: + Logger.recordOutput("Field/source", FieldConstants.redSourceHeading); + 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/org/utils/Deadband.java b/lib/src/main/java/org/utils/Deadband.java new file mode 100644 index 0000000..ab6386a --- /dev/null +++ b/lib/src/main/java/org/utils/Deadband.java @@ -0,0 +1,41 @@ +package frc.robot.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/org/utils/FieldFinder.java b/lib/src/main/java/org/utils/FieldFinder.java new file mode 100644 index 0000000..201aa69 --- /dev/null +++ b/lib/src/main/java/org/utils/FieldFinder.java @@ -0,0 +1,169 @@ +package frc.robot.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/org/utils/GeomUtil.java b/lib/src/main/java/org/utils/GeomUtil.java new file mode 100644 index 0000000..59ff752 --- /dev/null +++ b/lib/src/main/java/org/utils/GeomUtil.java @@ -0,0 +1,144 @@ +package frc.robot.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/org/utils/InterpolateDouble.java b/lib/src/main/java/org/utils/InterpolateDouble.java new file mode 100644 index 0000000..ac093f9 --- /dev/null +++ b/lib/src/main/java/org/utils/InterpolateDouble.java @@ -0,0 +1,89 @@ +package frc.robot.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/org/utils/SystemsTest.java b/lib/src/main/java/org/utils/SystemsTest.java new file mode 100644 index 0000000..2bb21fa --- /dev/null +++ b/lib/src/main/java/org/utils/SystemsTest.java @@ -0,0 +1,63 @@ +// TODO: WIP - Not tested + +package frc.robot.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/org/utils/Tunable.java b/lib/src/main/java/org/utils/Tunable.java new file mode 100644 index 0000000..bec74b4 --- /dev/null +++ b/lib/src/main/java/org/utils/Tunable.java @@ -0,0 +1,25 @@ +package frc.robot.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/org/utils/WaitForButtonCommand.java b/lib/src/main/java/org/utils/WaitForButtonCommand.java new file mode 100644 index 0000000..c965641 --- /dev/null +++ b/lib/src/main/java/org/utils/WaitForButtonCommand.java @@ -0,0 +1,32 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class WaitForButtonCommand extends Command { + private CommandXboxController controller; + + private Timer time = new Timer(); + + public WaitForButtonCommand(CommandXboxController controller) { + this.controller = controller; + } + + @Override + public void initialize() { + time.reset(); + time.start(); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return controller.a().getAsBoolean() && time.get() > 0.5; + } +} diff --git a/lib/src/main/java/org/utils/feedforward/TuneG.java b/lib/src/main/java/org/utils/feedforward/TuneG.java new file mode 100644 index 0000000..36abbee --- /dev/null +++ b/lib/src/main/java/org/utils/feedforward/TuneG.java @@ -0,0 +1,49 @@ +// TODO: WIP - Not tested + +package frc.robot.utils.feedforward; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.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/org/utils/feedforward/TuneS.java b/lib/src/main/java/org/utils/feedforward/TuneS.java new file mode 100644 index 0000000..1dd08ca --- /dev/null +++ b/lib/src/main/java/org/utils/feedforward/TuneS.java @@ -0,0 +1,47 @@ +// TODO: WIP - Not tested + +package frc.robot.utils.feedforward; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.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/org/utils/feedforward/TuneV.java b/lib/src/main/java/org/utils/feedforward/TuneV.java new file mode 100644 index 0000000..76d9f70 --- /dev/null +++ b/lib/src/main/java/org/utils/feedforward/TuneV.java @@ -0,0 +1,73 @@ +// TODO: WIP - Not tested + +package frc.robot.utils.feedforward; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.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/src/main/java/org/utils/notesimulator/Note.java b/lib/src/main/java/org/utils/notesimulator/Note.java new file mode 100644 index 0000000..37991be --- /dev/null +++ b/lib/src/main/java/org/utils/notesimulator/Note.java @@ -0,0 +1,204 @@ +package frc.robot.utils.notesimulator; + +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.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; +import frc.robot.Constants; +import java.util.Set; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class Note { + + private Supplier robotPoseSupplier = () -> new Pose2d(); + + private Pose3d lastPose = new Pose3d(100, 100, 100, new Rotation3d()); + + private boolean noteInRobot = false; + + private String id = ""; + + public Note( + Supplier robotSupplier, + Pose2d noteStartingPosition, + boolean noteInRobot, + String id) { + robotPoseSupplier = robotSupplier; + this.id = id; + this.noteInRobot = noteInRobot; + if (!noteInRobot) { + lastPose = new Pose3d(noteStartingPosition); + Logger.recordOutput("NoteVisualizer" + id, lastPose); + } + } + + public Note(Supplier robotSupplier, boolean noteInRobot, String id) { + + this( + robotSupplier, + new Pose2d( + Math.random() * Constants.FieldConstants.lengthM, + Math.random() * Math.random() * Constants.FieldConstants.widthM, + new Rotation2d()), + noteInRobot, + id); + } + + public Note(Supplier robotSupplier, Pose2d noteStartingPosition, String id) { + + this(robotSupplier, noteStartingPosition, false, id); + } + + public Note(Supplier robotSupplier, String id) { + + this( + robotSupplier, + new Pose2d(Math.random() * 10 + 5, Math.random() * 4 + 1, new Rotation2d()), + false, + id); + } + + // flywheels are 3 inches + // spotless:off + public Command shoot(double aimRPM, double aimAngle) { + if (noteInRobot) { + return new ScheduleCommand( // Branch off and exit immediately + Commands.defer( + () -> { + Pose3d startPose = + new Pose3d(robotPoseSupplier.get()) + .transformBy(new Transform3d(0.35, 0, 1.2, new Rotation3d())); + Timer timeSinceLaunch = new Timer(); + timeSinceLaunch.start(); + double shotSpeed = + aimRPM + * 2 + * Math.PI + / 60 + * 0.5 * 0.075184; // are the values on the + // interpolate map correct? + + return Commands.run( + () -> { + lastPose = + new Pose3d( + shotSpeed + * Math.cos(aimAngle) + * Math.cos(robotPoseSupplier.get().getRotation().getRadians()) + * timeSinceLaunch.get() + + startPose.getX(), + shotSpeed + * Math.cos(aimAngle) + * Math.sin(robotPoseSupplier.get().getRotation().getRadians()) + * timeSinceLaunch.get() + + startPose.getY(), + shotSpeed + * Math.sin(aimAngle) + * timeSinceLaunch.get() + + startPose.getZ() + - 4.9 * Math.pow(timeSinceLaunch.get(),2), + startPose.getRotation()); + Logger.recordOutput( + "NoteVisualizer" + id, lastPose); + SmartDashboard.putNumber( + "angle", + robotPoseSupplier + .get() + .getRotation() + .getDegrees()); + }) + .until(() -> lastPose != null && inSpeaker() != 1) + .finallyDo( + () -> { + Logger.recordOutput( + "NoteVisualizer" + id, new Pose3d[] {}); + if (inSpeaker() == 2) SmartDashboard.putBoolean("inGoal", true); + else SmartDashboard.putBoolean("inGoal", false); + }); + }, + Set.of()) + .ignoringDisable(true)); + } + else return null; + }// spotless:on + + public boolean intakeNote() { + if (noteInRobot == false && robotWithinRange(1)) { + noteInRobot = true; + lastPose = null; + Logger.recordOutput("NoteVisualizer" + id, new Pose3d[] {}); + return true; + } + return false; + } + + public boolean inRobot() { + return noteInRobot; + } + + private boolean robotWithinRange(double radius) { + double dX = lastPose.getX() - robotPoseSupplier.get().getX(); + double dY = lastPose.getY() - robotPoseSupplier.get().getY(); + if (Math.sqrt(dX * dX + dY * dY) <= radius) return true; + return false; + } + + private int inSpeaker() { + boolean isRed = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get().equals(Alliance.Red); + double[][] redSpeakerOpening = { + {16.08, 5, 1.98}, {16.08, 5.55, 1.98}, {16.51, 5, 2.11}, {16.51, 5.55, 2.11} + }; + double[][] blueSpeakerOpening = { + {0.43, 5, 1.98}, {0.43, 5.55, 1.98}, {0, 5, 2.11}, {0, 5.55, 2.11} + }; + double[][] redSpeakerRoof = { + {16.51, 5, 2.11}, {16.51, 5.55, 2.11}, {16.08, 5, 2.5}, {16.08, 5.55, 2.5} + }; + double[][] blueSpeakerRoof = { + {0, 5, 2.11}, {0, 5.55, 2.11}, {0.43, 5, 2.5}, {0.43, 5.55, 2.5} + }; + if (passesThroughRectangle(isRed ? redSpeakerOpening : blueSpeakerOpening, lastPose)) + return 2; + if (lastPose.getZ() < 0) + return 0; // passesThroughRectangle(isRed ? redSpeakerRoof : blueSpeakerRoof, + // lastPose)|| + + // new Pose3d(isRed ? redSpeaker : blueSpeaker, startPose.getRotation()); + return 1; // in progress, unsure if there's an easy way to do pip for 3d or if there's + // an existing library or something + } + + // only works on rectangles parallel to z and tilted forward on x + private boolean passesThroughRectangle(double[][] rectangle, Pose3d noteLocation) { + double x = noteLocation.getX(); + double y = noteLocation.getY(); + double z = noteLocation.getZ(); + + if (y > Math.min(rectangle[0][1], rectangle[3][1]) - 0.3 + && y < Math.max(rectangle[0][1], rectangle[3][1]) + 0.3 + && z > Math.min(rectangle[0][2], rectangle[3][2]) - 0.3 + && z < Math.max(rectangle[0][2], rectangle[3][2]) + 0.3) { + double slope = + (rectangle[3][2] - rectangle[0][2]) / (rectangle[3][0] - rectangle[0][0]); + double predictedZ = slope * (x - rectangle[3][0]) + rectangle[3][2]; + SmartDashboard.putNumber("difference from goal", Math.abs(predictedZ - z)); + if (Math.abs(predictedZ - z) < 0.3) return true; + } + return false; + } + + public String getId() { + return id; + } +} diff --git a/lib/src/main/java/org/utils/notesimulator/NoteManager.java b/lib/src/main/java/org/utils/notesimulator/NoteManager.java new file mode 100644 index 0000000..143e60b --- /dev/null +++ b/lib/src/main/java/org/utils/notesimulator/NoteManager.java @@ -0,0 +1,58 @@ +package frc.robot.utils.notesimulator; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.ArrayList; + +public class NoteManager { + + private static ArrayList notesOnField = new ArrayList(); + private static Note noteInRobot; + + private static int numberOfExistingNotes = 0; + + private static NoteManager instance; + + public static NoteManager getInstance() { + if (instance == null) instance = new NoteManager(); + return instance; + } + + public static void addNote(Note note) { + if (note.inRobot() && noteInRobot == null) noteInRobot = note; + else { + notesOnField.add(note); + } + numberOfExistingNotes++; + } + + public static void intake() { + if (noteInRobot == null) { + for (Note note : notesOnField) { + if (note.intakeNote()) { + noteInRobot = note; + notesOnField.remove(note); + SmartDashboard.putBoolean("noteInRobot", noteInRobot != null); + return; + } + } + } + } + + public static Command shoot(double aimRPM, double aimAngle) { + if (noteInRobot != null) { + Note shotNote = noteInRobot; + noteInRobot = null; + SmartDashboard.putBoolean("noteInRobot", noteInRobot != null); + return shotNote.shoot(aimRPM, aimAngle); + } else return null; + } + + public static boolean noteInRobot() { + return noteInRobot != null; + } + + public static int numberOfExistingNotes() { + return numberOfExistingNotes; + } +} From b7a4ebcdef20ef6c4c7e887e40ddea356cb87c0d Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sat, 20 Apr 2024 17:20:43 -0400 Subject: [PATCH 2/7] remove note finder (wont carry over) + organize into package folders --- .../commands}/WaitForButtonCommand.java | 0 .../coppercore/constants/FieldConstants.java | 67 ++++++ .../utils/AllianceUtil.java | 10 +- .../{org => coppercore}/utils/Deadband.java | 0 .../utils/FieldFinder.java | 0 .../{org => coppercore}/utils/GeomUtil.java | 0 .../utils/InterpolateDouble.java | 0 .../utils/SystemsTest.java | 0 .../{org => coppercore}/utils/Tunable.java | 0 .../utils/feedforward/TuneG.java | 0 .../utils/feedforward/TuneS.java | 0 .../utils/feedforward/TuneV.java | 0 .../java/org/utils/notesimulator/Note.java | 204 ------------------ .../org/utils/notesimulator/NoteManager.java | 58 ----- 14 files changed, 69 insertions(+), 270 deletions(-) rename lib/src/main/java/{org/utils => coppercore/commands}/WaitForButtonCommand.java (100%) create mode 100644 lib/src/main/java/coppercore/constants/FieldConstants.java rename lib/src/main/java/{org => coppercore}/utils/AllianceUtil.java (89%) rename lib/src/main/java/{org => coppercore}/utils/Deadband.java (100%) rename lib/src/main/java/{org => coppercore}/utils/FieldFinder.java (100%) rename lib/src/main/java/{org => coppercore}/utils/GeomUtil.java (100%) rename lib/src/main/java/{org => coppercore}/utils/InterpolateDouble.java (100%) rename lib/src/main/java/{org => coppercore}/utils/SystemsTest.java (100%) rename lib/src/main/java/{org => coppercore}/utils/Tunable.java (100%) rename lib/src/main/java/{org => coppercore}/utils/feedforward/TuneG.java (100%) rename lib/src/main/java/{org => coppercore}/utils/feedforward/TuneS.java (100%) rename lib/src/main/java/{org => coppercore}/utils/feedforward/TuneV.java (100%) delete mode 100644 lib/src/main/java/org/utils/notesimulator/Note.java delete mode 100644 lib/src/main/java/org/utils/notesimulator/NoteManager.java diff --git a/lib/src/main/java/org/utils/WaitForButtonCommand.java b/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java similarity index 100% rename from lib/src/main/java/org/utils/WaitForButtonCommand.java rename to lib/src/main/java/coppercore/commands/WaitForButtonCommand.java diff --git a/lib/src/main/java/coppercore/constants/FieldConstants.java b/lib/src/main/java/coppercore/constants/FieldConstants.java new file mode 100644 index 0000000..c70c636 --- /dev/null +++ b/lib/src/main/java/coppercore/constants/FieldConstants.java @@ -0,0 +1,67 @@ +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 static 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/org/utils/AllianceUtil.java b/lib/src/main/java/coppercore/utils/AllianceUtil.java similarity index 89% rename from lib/src/main/java/org/utils/AllianceUtil.java rename to lib/src/main/java/coppercore/utils/AllianceUtil.java index 278a1ec..36a759e 100644 --- a/lib/src/main/java/org/utils/AllianceUtil.java +++ b/lib/src/main/java/coppercore/utils/AllianceUtil.java @@ -1,11 +1,10 @@ -package frc.robot.utils; +package 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 frc.robot.Constants.FieldConstants; -import org.littletonrobotics.junction.Logger; +import constants.FieldConstants; public class AllianceUtil { @@ -13,10 +12,8 @@ public static Translation2d getFieldToSpeaker() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/speaker", FieldConstants.fieldToBlueSpeaker); return FieldConstants.fieldToBlueSpeaker; case Red: - Logger.recordOutput("Field/speaker", FieldConstants.fieldToRedSpeaker); return FieldConstants.fieldToRedSpeaker; } } @@ -24,7 +21,6 @@ public static Translation2d getFieldToSpeaker() { } public static Rotation2d getAmpHeading() { - Logger.recordOutput("Field/amp", FieldConstants.ampHeading); return FieldConstants.ampHeading; } @@ -92,10 +88,8 @@ public static Rotation2d getSourceHeading() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/source", FieldConstants.blueSourceHeading); return FieldConstants.blueSourceHeading; case Red: - Logger.recordOutput("Field/source", FieldConstants.redSourceHeading); return FieldConstants.redSourceHeading; } } diff --git a/lib/src/main/java/org/utils/Deadband.java b/lib/src/main/java/coppercore/utils/Deadband.java similarity index 100% rename from lib/src/main/java/org/utils/Deadband.java rename to lib/src/main/java/coppercore/utils/Deadband.java diff --git a/lib/src/main/java/org/utils/FieldFinder.java b/lib/src/main/java/coppercore/utils/FieldFinder.java similarity index 100% rename from lib/src/main/java/org/utils/FieldFinder.java rename to lib/src/main/java/coppercore/utils/FieldFinder.java diff --git a/lib/src/main/java/org/utils/GeomUtil.java b/lib/src/main/java/coppercore/utils/GeomUtil.java similarity index 100% rename from lib/src/main/java/org/utils/GeomUtil.java rename to lib/src/main/java/coppercore/utils/GeomUtil.java diff --git a/lib/src/main/java/org/utils/InterpolateDouble.java b/lib/src/main/java/coppercore/utils/InterpolateDouble.java similarity index 100% rename from lib/src/main/java/org/utils/InterpolateDouble.java rename to lib/src/main/java/coppercore/utils/InterpolateDouble.java diff --git a/lib/src/main/java/org/utils/SystemsTest.java b/lib/src/main/java/coppercore/utils/SystemsTest.java similarity index 100% rename from lib/src/main/java/org/utils/SystemsTest.java rename to lib/src/main/java/coppercore/utils/SystemsTest.java diff --git a/lib/src/main/java/org/utils/Tunable.java b/lib/src/main/java/coppercore/utils/Tunable.java similarity index 100% rename from lib/src/main/java/org/utils/Tunable.java rename to lib/src/main/java/coppercore/utils/Tunable.java diff --git a/lib/src/main/java/org/utils/feedforward/TuneG.java b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java similarity index 100% rename from lib/src/main/java/org/utils/feedforward/TuneG.java rename to lib/src/main/java/coppercore/utils/feedforward/TuneG.java diff --git a/lib/src/main/java/org/utils/feedforward/TuneS.java b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java similarity index 100% rename from lib/src/main/java/org/utils/feedforward/TuneS.java rename to lib/src/main/java/coppercore/utils/feedforward/TuneS.java diff --git a/lib/src/main/java/org/utils/feedforward/TuneV.java b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java similarity index 100% rename from lib/src/main/java/org/utils/feedforward/TuneV.java rename to lib/src/main/java/coppercore/utils/feedforward/TuneV.java diff --git a/lib/src/main/java/org/utils/notesimulator/Note.java b/lib/src/main/java/org/utils/notesimulator/Note.java deleted file mode 100644 index 37991be..0000000 --- a/lib/src/main/java/org/utils/notesimulator/Note.java +++ /dev/null @@ -1,204 +0,0 @@ -package frc.robot.utils.notesimulator; - -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.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ScheduleCommand; -import frc.robot.Constants; -import java.util.Set; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class Note { - - private Supplier robotPoseSupplier = () -> new Pose2d(); - - private Pose3d lastPose = new Pose3d(100, 100, 100, new Rotation3d()); - - private boolean noteInRobot = false; - - private String id = ""; - - public Note( - Supplier robotSupplier, - Pose2d noteStartingPosition, - boolean noteInRobot, - String id) { - robotPoseSupplier = robotSupplier; - this.id = id; - this.noteInRobot = noteInRobot; - if (!noteInRobot) { - lastPose = new Pose3d(noteStartingPosition); - Logger.recordOutput("NoteVisualizer" + id, lastPose); - } - } - - public Note(Supplier robotSupplier, boolean noteInRobot, String id) { - - this( - robotSupplier, - new Pose2d( - Math.random() * Constants.FieldConstants.lengthM, - Math.random() * Math.random() * Constants.FieldConstants.widthM, - new Rotation2d()), - noteInRobot, - id); - } - - public Note(Supplier robotSupplier, Pose2d noteStartingPosition, String id) { - - this(robotSupplier, noteStartingPosition, false, id); - } - - public Note(Supplier robotSupplier, String id) { - - this( - robotSupplier, - new Pose2d(Math.random() * 10 + 5, Math.random() * 4 + 1, new Rotation2d()), - false, - id); - } - - // flywheels are 3 inches - // spotless:off - public Command shoot(double aimRPM, double aimAngle) { - if (noteInRobot) { - return new ScheduleCommand( // Branch off and exit immediately - Commands.defer( - () -> { - Pose3d startPose = - new Pose3d(robotPoseSupplier.get()) - .transformBy(new Transform3d(0.35, 0, 1.2, new Rotation3d())); - Timer timeSinceLaunch = new Timer(); - timeSinceLaunch.start(); - double shotSpeed = - aimRPM - * 2 - * Math.PI - / 60 - * 0.5 * 0.075184; // are the values on the - // interpolate map correct? - - return Commands.run( - () -> { - lastPose = - new Pose3d( - shotSpeed - * Math.cos(aimAngle) - * Math.cos(robotPoseSupplier.get().getRotation().getRadians()) - * timeSinceLaunch.get() - + startPose.getX(), - shotSpeed - * Math.cos(aimAngle) - * Math.sin(robotPoseSupplier.get().getRotation().getRadians()) - * timeSinceLaunch.get() - + startPose.getY(), - shotSpeed - * Math.sin(aimAngle) - * timeSinceLaunch.get() - + startPose.getZ() - - 4.9 * Math.pow(timeSinceLaunch.get(),2), - startPose.getRotation()); - Logger.recordOutput( - "NoteVisualizer" + id, lastPose); - SmartDashboard.putNumber( - "angle", - robotPoseSupplier - .get() - .getRotation() - .getDegrees()); - }) - .until(() -> lastPose != null && inSpeaker() != 1) - .finallyDo( - () -> { - Logger.recordOutput( - "NoteVisualizer" + id, new Pose3d[] {}); - if (inSpeaker() == 2) SmartDashboard.putBoolean("inGoal", true); - else SmartDashboard.putBoolean("inGoal", false); - }); - }, - Set.of()) - .ignoringDisable(true)); - } - else return null; - }// spotless:on - - public boolean intakeNote() { - if (noteInRobot == false && robotWithinRange(1)) { - noteInRobot = true; - lastPose = null; - Logger.recordOutput("NoteVisualizer" + id, new Pose3d[] {}); - return true; - } - return false; - } - - public boolean inRobot() { - return noteInRobot; - } - - private boolean robotWithinRange(double radius) { - double dX = lastPose.getX() - robotPoseSupplier.get().getX(); - double dY = lastPose.getY() - robotPoseSupplier.get().getY(); - if (Math.sqrt(dX * dX + dY * dY) <= radius) return true; - return false; - } - - private int inSpeaker() { - boolean isRed = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get().equals(Alliance.Red); - double[][] redSpeakerOpening = { - {16.08, 5, 1.98}, {16.08, 5.55, 1.98}, {16.51, 5, 2.11}, {16.51, 5.55, 2.11} - }; - double[][] blueSpeakerOpening = { - {0.43, 5, 1.98}, {0.43, 5.55, 1.98}, {0, 5, 2.11}, {0, 5.55, 2.11} - }; - double[][] redSpeakerRoof = { - {16.51, 5, 2.11}, {16.51, 5.55, 2.11}, {16.08, 5, 2.5}, {16.08, 5.55, 2.5} - }; - double[][] blueSpeakerRoof = { - {0, 5, 2.11}, {0, 5.55, 2.11}, {0.43, 5, 2.5}, {0.43, 5.55, 2.5} - }; - if (passesThroughRectangle(isRed ? redSpeakerOpening : blueSpeakerOpening, lastPose)) - return 2; - if (lastPose.getZ() < 0) - return 0; // passesThroughRectangle(isRed ? redSpeakerRoof : blueSpeakerRoof, - // lastPose)|| - - // new Pose3d(isRed ? redSpeaker : blueSpeaker, startPose.getRotation()); - return 1; // in progress, unsure if there's an easy way to do pip for 3d or if there's - // an existing library or something - } - - // only works on rectangles parallel to z and tilted forward on x - private boolean passesThroughRectangle(double[][] rectangle, Pose3d noteLocation) { - double x = noteLocation.getX(); - double y = noteLocation.getY(); - double z = noteLocation.getZ(); - - if (y > Math.min(rectangle[0][1], rectangle[3][1]) - 0.3 - && y < Math.max(rectangle[0][1], rectangle[3][1]) + 0.3 - && z > Math.min(rectangle[0][2], rectangle[3][2]) - 0.3 - && z < Math.max(rectangle[0][2], rectangle[3][2]) + 0.3) { - double slope = - (rectangle[3][2] - rectangle[0][2]) / (rectangle[3][0] - rectangle[0][0]); - double predictedZ = slope * (x - rectangle[3][0]) + rectangle[3][2]; - SmartDashboard.putNumber("difference from goal", Math.abs(predictedZ - z)); - if (Math.abs(predictedZ - z) < 0.3) return true; - } - return false; - } - - public String getId() { - return id; - } -} diff --git a/lib/src/main/java/org/utils/notesimulator/NoteManager.java b/lib/src/main/java/org/utils/notesimulator/NoteManager.java deleted file mode 100644 index 143e60b..0000000 --- a/lib/src/main/java/org/utils/notesimulator/NoteManager.java +++ /dev/null @@ -1,58 +0,0 @@ -package frc.robot.utils.notesimulator; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.ArrayList; - -public class NoteManager { - - private static ArrayList notesOnField = new ArrayList(); - private static Note noteInRobot; - - private static int numberOfExistingNotes = 0; - - private static NoteManager instance; - - public static NoteManager getInstance() { - if (instance == null) instance = new NoteManager(); - return instance; - } - - public static void addNote(Note note) { - if (note.inRobot() && noteInRobot == null) noteInRobot = note; - else { - notesOnField.add(note); - } - numberOfExistingNotes++; - } - - public static void intake() { - if (noteInRobot == null) { - for (Note note : notesOnField) { - if (note.intakeNote()) { - noteInRobot = note; - notesOnField.remove(note); - SmartDashboard.putBoolean("noteInRobot", noteInRobot != null); - return; - } - } - } - } - - public static Command shoot(double aimRPM, double aimAngle) { - if (noteInRobot != null) { - Note shotNote = noteInRobot; - noteInRobot = null; - SmartDashboard.putBoolean("noteInRobot", noteInRobot != null); - return shotNote.shoot(aimRPM, aimAngle); - } else return null; - } - - public static boolean noteInRobot() { - return noteInRobot != null; - } - - public static int numberOfExistingNotes() { - return numberOfExistingNotes; - } -} From 6bc5824edfe028d6d5807676ab3fe0af0ef647bf Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sat, 20 Apr 2024 17:34:58 -0400 Subject: [PATCH 3/7] begin fixing some build errors --- lib/build.gradle.kts | 9 +++++---- .../java/coppercore/commands/WaitForButtonCommand.java | 2 +- .../main/java/coppercore/constants/FieldConstants.java | 2 +- lib/src/main/java/coppercore/utils/AllianceUtil.java | 2 +- lib/src/main/java/coppercore/utils/Deadband.java | 2 +- lib/src/main/java/coppercore/utils/FieldFinder.java | 2 +- lib/src/main/java/coppercore/utils/GeomUtil.java | 2 +- .../main/java/coppercore/utils/InterpolateDouble.java | 2 +- lib/src/main/java/coppercore/utils/SystemsTest.java | 2 +- lib/src/main/java/coppercore/utils/Tunable.java | 2 +- .../main/java/coppercore/utils/feedforward/TuneG.java | 2 +- .../main/java/coppercore/utils/feedforward/TuneS.java | 2 +- .../main/java/coppercore/utils/feedforward/TuneV.java | 2 +- 13 files changed, 17 insertions(+), 16 deletions(-) diff --git a/lib/build.gradle.kts b/lib/build.gradle.kts index f4e383c..222e358 100644 --- a/lib/build.gradle.kts +++ b/lib/build.gradle.kts @@ -7,9 +7,10 @@ plugins { // Apply the java-library plugin for API and implementation separation. - id `java-library` + id ("java-library") + // WPILib plugin - id `edu.wpi.first.GradleRIO` version `2024.3.2` + id ("edu.wpi.first.GradleRIO") version ("2024.3.2") } repositories { @@ -30,8 +31,8 @@ dependencies { implementation(libs.guava) // WPILib dependencies - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() + implementation(wpi.java.deps.wpilib()) + implementation(wpi.java.vendor.java()) } // Apply a specific Java toolchain to ease working on different environments. diff --git a/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java b/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java index c965641..2ab4473 100644 --- a/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java +++ b/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.commands; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; diff --git a/lib/src/main/java/coppercore/constants/FieldConstants.java b/lib/src/main/java/coppercore/constants/FieldConstants.java index c70c636..cf6f9f5 100644 --- a/lib/src/main/java/coppercore/constants/FieldConstants.java +++ b/lib/src/main/java/coppercore/constants/FieldConstants.java @@ -1,4 +1,4 @@ -package constants; +package coppercore.constants; import edu.wpi.first.math.util.Units; import edu.wpi.first.math.geometry.Pose2d; diff --git a/lib/src/main/java/coppercore/utils/AllianceUtil.java b/lib/src/main/java/coppercore/utils/AllianceUtil.java index 36a759e..f9cdfc3 100644 --- a/lib/src/main/java/coppercore/utils/AllianceUtil.java +++ b/lib/src/main/java/coppercore/utils/AllianceUtil.java @@ -1,4 +1,4 @@ -package utils; +package coppercore.utils; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/lib/src/main/java/coppercore/utils/Deadband.java b/lib/src/main/java/coppercore/utils/Deadband.java index ab6386a..3794aa0 100644 --- a/lib/src/main/java/coppercore/utils/Deadband.java +++ b/lib/src/main/java/coppercore/utils/Deadband.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.utils; public class Deadband { // 1D Deadband in linear format diff --git a/lib/src/main/java/coppercore/utils/FieldFinder.java b/lib/src/main/java/coppercore/utils/FieldFinder.java index 201aa69..304cf5d 100644 --- a/lib/src/main/java/coppercore/utils/FieldFinder.java +++ b/lib/src/main/java/coppercore/utils/FieldFinder.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.utils; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; diff --git a/lib/src/main/java/coppercore/utils/GeomUtil.java b/lib/src/main/java/coppercore/utils/GeomUtil.java index 59ff752..d55f0e7 100644 --- a/lib/src/main/java/coppercore/utils/GeomUtil.java +++ b/lib/src/main/java/coppercore/utils/GeomUtil.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.utils; // Copyright (c) 2023 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/lib/src/main/java/coppercore/utils/InterpolateDouble.java b/lib/src/main/java/coppercore/utils/InterpolateDouble.java index ac093f9..9b64cf3 100644 --- a/lib/src/main/java/coppercore/utils/InterpolateDouble.java +++ b/lib/src/main/java/coppercore/utils/InterpolateDouble.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.utils; import java.util.ArrayList; import java.util.Collections; diff --git a/lib/src/main/java/coppercore/utils/SystemsTest.java b/lib/src/main/java/coppercore/utils/SystemsTest.java index 2bb21fa..a4727f4 100644 --- a/lib/src/main/java/coppercore/utils/SystemsTest.java +++ b/lib/src/main/java/coppercore/utils/SystemsTest.java @@ -1,6 +1,6 @@ // TODO: WIP - Not tested -package frc.robot.utils; +package coppercore.utils; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/lib/src/main/java/coppercore/utils/Tunable.java b/lib/src/main/java/coppercore/utils/Tunable.java index bec74b4..5e354a5 100644 --- a/lib/src/main/java/coppercore/utils/Tunable.java +++ b/lib/src/main/java/coppercore/utils/Tunable.java @@ -1,4 +1,4 @@ -package frc.robot.utils; +package coppercore.utils; /** * Interface for subsystems to be tuned. diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java index 36abbee..9835d93 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java @@ -1,6 +1,6 @@ // TODO: WIP - Not tested -package frc.robot.utils.feedforward; +package coppercore.utils.feedforward; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java index 1dd08ca..7546335 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java @@ -1,6 +1,6 @@ // TODO: WIP - Not tested -package frc.robot.utils.feedforward; +package coppercore.utils.feedforward; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java index 76d9f70..e7d7929 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java @@ -1,6 +1,6 @@ // TODO: WIP - Not tested -package frc.robot.utils.feedforward; +package coppercore.utils.feedforward; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; From b4e440bb2083f876857dfd33fae4fcb848d48b80 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sun, 21 Apr 2024 08:48:43 -0400 Subject: [PATCH 4/7] add libraries.kt (might work better) --- lib/Libraries.kt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 lib/Libraries.kt diff --git a/lib/Libraries.kt b/lib/Libraries.kt new file mode 100644 index 0000000..1f7eb08 --- /dev/null +++ b/lib/Libraries.kt @@ -0,0 +1,4 @@ +object Libraries { + const val wpilibDeps = "" + const val wpilibJava = "" +} \ No newline at end of file From 4b6e46d718871784129070007ad98250b7d3722a Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sun, 28 Apr 2024 12:18:34 -0400 Subject: [PATCH 5/7] work on setting up build.gradle --- lib/Libraries.kt | 4 -- lib/build.gradle | 116 +++++++++++++++++++++++++++++++++++++++++++ lib/build.gradle.kts | 48 ------------------ settings.gradle | 39 +++++++++++++++ settings.gradle.kts | 14 ------ 5 files changed, 155 insertions(+), 66 deletions(-) delete mode 100644 lib/Libraries.kt create mode 100644 lib/build.gradle delete mode 100644 lib/build.gradle.kts create mode 100644 settings.gradle delete mode 100644 settings.gradle.kts diff --git a/lib/Libraries.kt b/lib/Libraries.kt deleted file mode 100644 index 1f7eb08..0000000 --- a/lib/Libraries.kt +++ /dev/null @@ -1,4 +0,0 @@ -object Libraries { - const val wpilibDeps = "" - const val wpilibJava = "" -} \ No newline at end of file diff --git a/lib/build.gradle b/lib/build.gradle new file mode 100644 index 0000000..90233c6 --- /dev/null +++ b/lib/build.gradle @@ -0,0 +1,116 @@ +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' +} + +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "lib/src/main/java" + classPackage = "coppercore" + dateFormat = "yyyy-MM-dd HH:mm:ss Z" + timeZone = "America/New_York" + indent = " " +} diff --git a/lib/build.gradle.kts b/lib/build.gradle.kts deleted file mode 100644 index 222e358..0000000 --- a/lib/build.gradle.kts +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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") - - // WPILib plugin - id ("edu.wpi.first.GradleRIO") version ("2024.3.2") -} - -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) - - // WPILib dependencies - implementation(wpi.java.deps.wpilib()) - implementation(wpi.java.vendor.java()) -} - -// 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/settings.gradle b/settings.gradle new file mode 100644 index 0000000..ec700e1 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,39 @@ +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 + } + } +} + +plugins { + // Apply the foojay-resolver plugin to allow automatic download of JDKs + id 'org.gradle.toolchains.foojay-resolver-convention' version '0.8.0' +} + +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/settings.gradle.kts b/settings.gradle.kts deleted file mode 100644 index 0bfec0d..0000000 --- a/settings.gradle.kts +++ /dev/null @@ -1,14 +0,0 @@ -/* - * 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 - id("org.gradle.toolchains.foojay-resolver-convention") version "0.8.0" -} - -rootProject.name = "coppercore" -include("lib") From 8d8ec1e422406533ca22818dbd3b73ea266de6d9 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 30 Apr 2024 16:03:00 -0400 Subject: [PATCH 6/7] fix build.gradle --- lib/build.gradle | 9 ----- .../coppercore/constants/FieldConstants.java | 4 +- .../coppercore/utils/feedforward/TuneG.java | 2 +- .../coppercore/utils/feedforward/TuneS.java | 2 +- .../coppercore/utils/feedforward/TuneV.java | 2 +- .../test/java/org/example/LibraryTest.java | 14 ------- lib/vendordeps/WPILibNewCommands.json | 38 +++++++++++++++++++ 7 files changed, 43 insertions(+), 28 deletions(-) delete mode 100644 lib/src/test/java/org/example/LibraryTest.java create mode 100644 lib/vendordeps/WPILibNewCommands.json diff --git a/lib/build.gradle b/lib/build.gradle index 90233c6..45d1178 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -105,12 +105,3 @@ wpi.java.configureExecutableTasks(jar) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } - -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "lib/src/main/java" - classPackage = "coppercore" - dateFormat = "yyyy-MM-dd HH:mm:ss Z" - timeZone = "America/New_York" - indent = " " -} diff --git a/lib/src/main/java/coppercore/constants/FieldConstants.java b/lib/src/main/java/coppercore/constants/FieldConstants.java index cf6f9f5..fac2a6f 100644 --- a/lib/src/main/java/coppercore/constants/FieldConstants.java +++ b/lib/src/main/java/coppercore/constants/FieldConstants.java @@ -1,11 +1,11 @@ -package coppercore.constants; +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 static final class FieldConstants { +public final class FieldConstants { public static final double lengthM = 16.451; public static final double widthM = 8.211; diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java index 9835d93..bc8f3eb 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneG.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneG.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.Tunable; +import coppercore.utils.Tunable; public class TuneG extends Command { private Tunable subsystem; diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java index 7546335..6819337 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneS.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneS.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.Tunable; +import coppercore.utils.Tunable; public class TuneS extends Command { private Tunable subsystem; diff --git a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java index e7d7929..7a7c0b2 100644 --- a/lib/src/main/java/coppercore/utils/feedforward/TuneV.java +++ b/lib/src/main/java/coppercore/utils/feedforward/TuneV.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.Tunable; +import coppercore.utils.Tunable; import java.util.ArrayList; public class TuneV extends Command { diff --git a/lib/src/test/java/org/example/LibraryTest.java b/lib/src/test/java/org/example/LibraryTest.java deleted file mode 100644 index ef34950..0000000 --- a/lib/src/test/java/org/example/LibraryTest.java +++ /dev/null @@ -1,14 +0,0 @@ -/* - * This source file was generated by the Gradle 'init' task - */ -package org.example; - -import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.*; - -class LibraryTest { - @Test void someLibraryMethodReturnsTrue() { - Library classUnderTest = new Library(); - assertTrue(classUnderTest.someLibraryMethod(), "someLibraryMethod should return 'true'"); - } -} diff --git a/lib/vendordeps/WPILibNewCommands.json b/lib/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..71b3953 --- /dev/null +++ b/lib/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "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 From b6444924473ce9e56aeb4c479d6620fc2f39a6f4 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 30 Apr 2024 17:07:57 -0400 Subject: [PATCH 7/7] remove wait for button command --- .../commands/WaitForButtonCommand.java | 32 ------------------- 1 file changed, 32 deletions(-) delete mode 100644 lib/src/main/java/coppercore/commands/WaitForButtonCommand.java diff --git a/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java b/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java deleted file mode 100644 index 2ab4473..0000000 --- a/lib/src/main/java/coppercore/commands/WaitForButtonCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package coppercore.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - -public class WaitForButtonCommand extends Command { - private CommandXboxController controller; - - private Timer time = new Timer(); - - public WaitForButtonCommand(CommandXboxController controller) { - this.controller = controller; - } - - @Override - public void initialize() { - time.reset(); - time.start(); - } - - @Override - public void execute() {} - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - return controller.a().getAsBoolean() && time.get() > 0.5; - } -}