children;
+
+ void writeJSON(JsonGenerator generator, double timeDivisor, long parent_total,
+ long root_total) throws IOException {
+ generator.writeStartObject();
+ generator.writeNumberField("visitCount", visits);
+ generator.writeNumberField("totalTime", totalTime / timeDivisor);
+ if (parent_total > 0) {
+ double percent = (double) totalTime / (double) parent_total * 100.0;
+ generator.writeNumberField("percentOfParent", percent);
+ }
+ if (root_total > 0) {
+ double percent = (double) totalTime / (double) root_total * 100.0;
+ generator.writeNumberField("percentOfRoot", percent);
+ }
+ generator.writeNumberField("maxTime", maxTime / timeDivisor);
+ generator.writeNumberField("minTime", minTime / timeDivisor);
+ generator.writeNumberField("avgTime", totalTime / timeDivisor / visits);
+ generator.writeFieldName("children");
+ generator.writeStartObject();
+ for (var entry : children.entrySet()) {
+ generator.writeFieldName(entry.getKey());
+ entry.getValue().writeJSON(generator, timeDivisor, totalTime, root_total);
+ }
+ generator.writeEndObject();
+ generator.writeEndObject();
+ }
+ }
+
+ @Override
+ public void reset() {
+ this.locationInfos.clear();
+ this.fullPath = "";
+ this.path.clear();
+ this.timeList.clear();
+ }
+
+}
diff --git a/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java b/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java
new file mode 100644
index 0000000..8bd5cf6
--- /dev/null
+++ b/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java
@@ -0,0 +1,31 @@
+package frc.lib.profiling;
+
+import java.io.OutputStream;
+import frc.lib.profiling.LoggingProfiler.LocatedInfo;
+
+/**
+ * Describes how a profile is written to a file.
+ */
+public interface ProfileLoggingFormat {
+
+ /**
+ * Comparison function used for sorting profile information.
+ */
+ int compare(String leftName, LocatedInfo leftInfo, String rightName, LocatedInfo rightInfo);
+
+ /**
+ * Write a profile to the file specified in {@code outStream}.
+ */
+ void write(String name, LocatedInfo info, double timeDivisor, OutputStream outStream);
+
+ /**
+ * Write information before seeing any profiles.
+ */
+ void begin(OutputStream outputStream);
+
+ /**
+ * Write information after seeing all profiles.
+ */
+ void end(OutputStream outputStream);
+
+}
diff --git a/src/main/java/frc/lib/profiling/Profiler.java b/src/main/java/frc/lib/profiling/Profiler.java
new file mode 100644
index 0000000..8455df9
--- /dev/null
+++ b/src/main/java/frc/lib/profiling/Profiler.java
@@ -0,0 +1,95 @@
+package frc.lib.profiling;
+
+/**
+ * Interface supporting profiling calls for measuring and saving runtime performance.
+ *
+ *
+ * Operates on the idea of a "profile stack", which is just a stack of names with associated
+ * profiling metrics. For instance, the following code
+ *
+ *
+ * {@code
+ * Profiler profiler = someProfiler();
+ * profiler.startTick();
+ * profiler.push("Hello")
+ * someExpensiveFunction();
+ * profiler.swap("World")
+ * someLessExpensiveFunction();
+ * profiler.pop();
+ * profiler.endTick();
+ * }
+ *
+ *
+ *
+ * will produce a profile with three sections:
+ *
+ * - {@code root}, which describes the time between {@link #startTick() profiler.startTick()} and
+ * {@link #endTick() profiler.endTick()},
+ * - {@code root.Hello}, which describes the time between {@link #push(String)
+ * profiler.push("Hello")} and {@link #swap(String) profiler.swap("World")}, and
+ * - {@code root.World}, which describes the time between {@link #swap(String)
+ * profiler.swap("World")} and {@link #pop() profiler.pop()}.
+ *
+ *
+ *
+ * A call to {@link #save() profiler.save()} would output the performance metrics of these sections
+ * in the case of a {@link LoggingProfiler}.
+ */
+public sealed interface Profiler permits EmptyProfiler, LoggingProfiler, ValidatingProfiler {
+ /**
+ * Called at the top of the main loop. Indicates the profiler is at "root" and records the start
+ * time. Within the main loop, no meaningful work should occur before this call.
+ *
+ * @throws RuntimeException if called twice without a call to {@link #endTick() endTick} in
+ * between.
+ */
+ void startTick();
+
+ /**
+ * Called at the bottom of the main loop. Indicates the profiler's "root" is finished, and its
+ * time usage metrics are updated. Within the main loop, no meaningful work should occur after
+ * this call.
+ *
+ * @throws RuntimeException if the profile stack is not empty or {@link #startTick() startTick}
+ * has not been called.
+ */
+ void endTick();
+
+ /**
+ * The profile stack is pushed with the {@code location} as its top. Must be accompanied by a
+ * call to {@link #pop() pop}.
+ *
+ * @throws RuntimeException if {@link #startTick() startTick} hasn't been called yet.
+ */
+ void push(String location);
+
+ /**
+ * The profile stack is popped. Must be preceded (at some point) by a call to
+ * {@link #push(String) push}.
+ *
+ * @throws RuntimeException if {@link #startTick() startTick} hasn't been called yet or the
+ * profile stack is empty.
+ */
+ void pop();
+
+ /**
+ * The top of the profile stack is replaced with {@code location}. This is equivalent to a call
+ * to {@link #pop() pop} followed immediately by a call to {@link #push(String) push}.
+ *
+ * @throws RuntimeException if the profile stack is empty.
+ */
+ default void swap(String location) {
+ pop();
+ push(location);
+ }
+
+ /**
+ * Write profile data to a file.
+ */
+ void save();
+
+ /**
+ * Reset logger.
+ */
+ void reset();
+}
diff --git a/src/main/java/frc/lib/profiling/ValidatingProfiler.java b/src/main/java/frc/lib/profiling/ValidatingProfiler.java
new file mode 100644
index 0000000..8c4ae7c
--- /dev/null
+++ b/src/main/java/frc/lib/profiling/ValidatingProfiler.java
@@ -0,0 +1,141 @@
+package frc.lib.profiling;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * This logger only performs validation (i.e. throws if {@link #pop() pop} is called with an empty
+ * stack). Otherwise, does nothing. Because there is no variance in doing nothing, a single instance
+ * is provided as {@link #INSTANCE}.
+ */
+public final class ValidatingProfiler implements Profiler {
+
+ /**
+ * The sole instance of {@link ValidatingProfiler}.
+ */
+ public static final ValidatingProfiler INSTANCE = new ValidatingProfiler();
+
+ private ValidatingProfiler() {}
+
+ /**
+ * Keeps track of methods and how many times they call push/pop.
+ */
+ private Map methodPushHelper = new HashMap<>();
+
+ private boolean tickStarted = false;
+ private int pathLen = 0;
+ private boolean fullPathEmpty = true;
+
+ @Override
+ public void save() {
+
+ }
+
+ @Override
+ public void startTick() {
+ if (this.tickStarted) {
+ throw new RuntimeException("Profiler tick already started. Missing endTick()?");
+ } else {
+ tickStarted = true;
+ pathLen = 0;
+ fullPathEmpty = true;
+ this.push("root");
+ }
+ }
+
+ @Override
+ public void endTick() {
+ if (!this.tickStarted) {
+ StringBuilder builder = new StringBuilder();
+ for (var entry : methodPushHelper.entrySet()) {
+ if (entry.getValue() != 0) {
+ builder.append(entry.getKey());
+ builder.append(" has ");
+ builder.append(entry.getValue());
+ builder.append(" more pushes that pops.\n");
+ }
+ }
+ throw new RuntimeException(
+ "Profiler tick already ended. Missing startTick()?\n" + builder.toString());
+ } else {
+ this.pop();
+ this.tickStarted = false;
+ if (!this.fullPathEmpty) {
+ StringBuilder builder = new StringBuilder();
+ for (var entry : methodPushHelper.entrySet()) {
+ if (entry.getValue() != 0) {
+ builder.append(entry.getKey());
+ builder.append(" has ");
+ builder.append(entry.getValue());
+ builder.append(" more pushes that pops.\n");
+ }
+ }
+ throw new RuntimeException(
+ "Profiler tick ended before path was fully popped. Mismatched push/pop?\n"
+ + builder.toString());
+ }
+ }
+ }
+
+ @Override
+ public void push(String location) {
+ if (!this.tickStarted) {
+ throw new RuntimeException("Cannot push '" + location
+ + "' to the profiler if profiler tick hasn't started. Missing startTick()?");
+ } else {
+ for (var stack : Thread.currentThread().getStackTrace()) {
+ if (stack.getClassName().equals(ValidatingProfiler.class.getCanonicalName())
+ || stack.getClassName().equals(Profiler.class.getCanonicalName())
+ || stack.getClassName().equals(Thread.class.getCanonicalName())) {
+ continue;
+ }
+ String key = stack.getClassName() + "." + stack.getMethodName();
+ methodPushHelper.put(key, methodPushHelper.computeIfAbsent(key, (_k) -> 0) + 1);
+ break;
+ }
+ fullPathEmpty = false;
+ pathLen += 1;
+ }
+ }
+
+ @Override
+ public void pop() {
+ if (!this.tickStarted) {
+ throw new RuntimeException(
+ "Cannot pop from profiler if profiler tick hasn't started. Missing startTick()?");
+ } else if (pathLen == 0) {
+ StringBuilder builder = new StringBuilder();
+ for (var entry : methodPushHelper.entrySet()) {
+ if (entry.getValue() != 0) {
+ builder.append(entry.getKey());
+ builder.append(" has ");
+ builder.append(-entry.getValue());
+ builder.append(" more pops than pushes.\n");
+ }
+ }
+ throw new RuntimeException(
+ "Tried to pop one too many times! Mismatched push() and pop()?\n"
+ + builder.toString());
+ } else {
+ for (var stack : Thread.currentThread().getStackTrace()) {
+ if (stack.getClassName().equals(ValidatingProfiler.class.getCanonicalName())
+ || stack.getClassName().equals(Profiler.class.getCanonicalName())
+ || stack.getClassName().equals(Thread.class.getCanonicalName())) {
+ continue;
+ }
+ String key = stack.getClassName() + "." + stack.getMethodName();
+ methodPushHelper.put(key, methodPushHelper.computeIfAbsent(key, (_k) -> 0) - 1);
+ break;
+ }
+ pathLen -= 1;
+ if (pathLen == 0) {
+ fullPathEmpty = true;
+ }
+ }
+ }
+
+ @Override
+ public void reset() {
+
+ }
+}
diff --git a/src/main/java/frc/lib/util/FieldConstants.java b/src/main/java/frc/lib/util/FieldConstants.java
index 9e32e77..70a3570 100644
--- a/src/main/java/frc/lib/util/FieldConstants.java
+++ b/src/main/java/frc/lib/util/FieldConstants.java
@@ -1,34 +1,12 @@
-/*
- * MIT License
- *
- * Copyright (c) 2023 FRC 6328
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
- * associated documentation files (the "Software"), to deal in the Software without restriction,
- * including without limitation the rights to use, copy, modify, merge, publish, distribute,
- * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all copies or
- * substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT
- * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
- */
-
-// Modified by 5572 to include fieldSdf and associated Signed Distance Field functions
package frc.lib.util;
-import java.util.Map;
-import edu.wpi.first.math.MathUtil;
+import static edu.wpi.first.apriltag.AprilTagFields.k2024Crescendo;
+import java.io.IOException;
+import java.util.Optional;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
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.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
@@ -36,219 +14,87 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
/**
- * Field Constants
+ * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets
+ * of corners start in the lower left moving clockwise. All units in Meters
+ *
+ *
+ *
+ * All translations and poses are stored with the origin at the rightmost point on the BLUE ALLIANCE
+ * wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib)
*/
public class FieldConstants {
- public static final double fieldLength = Units.inchesToMeters(651.25);
- public static final double fieldWidth = Units.inchesToMeters(315.5);
- public static final double tapeWidth = Units.inchesToMeters(2.0);
-
- /**
- * Dimensions for community and charging station, including the tape.
- */
- public static final class Community {
- // Region dimensions
- public static final double innerX = 0.0;
- // Tape to the left of charging station
- public static final double midX = Units.inchesToMeters(132.375);
- // Tape to the right of charging station
- public static final double outerX = Units.inchesToMeters(193.25);
- public static final double leftY = Units.feetToMeters(18.0);
- public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth;
- public static final double rightY = 0.0;
- public static final Translation2d[] regionCorners = new Translation2d[] {
- new Translation2d(innerX, rightY), new Translation2d(innerX, leftY),
- new Translation2d(midX, leftY), new Translation2d(midX, midY),
- new Translation2d(outerX, midY), new Translation2d(outerX, rightY)};
+ public static double fieldLength = Units.inchesToMeters(651.223);
+ public static double fieldWidth = Units.inchesToMeters(323.277);
+ public static double wingX = Units.inchesToMeters(229.201);
+ public static double podiumX = Units.inchesToMeters(126.75);
+ public static double startingLineX = Units.inchesToMeters(74.111);
- // Charging station dimensions
- public static final double chargingStationLength = Units.inchesToMeters(76.125);
- public static final double chargingStationWidth = Units.inchesToMeters(97.25);
- public static final double chargingStationOuterX = outerX - tapeWidth;
- public static final double chargingStationInnerX =
- chargingStationOuterX - chargingStationLength;
- public static final double chargingStationLeftY = midY - tapeWidth;
- public static final double chargingStationRightY =
- chargingStationLeftY - chargingStationWidth;
- public static final Translation2d[] chargingStationCorners =
- new Translation2d[] {new Translation2d(chargingStationInnerX, chargingStationRightY),
- new Translation2d(chargingStationInnerX, chargingStationLeftY),
- new Translation2d(chargingStationOuterX, chargingStationRightY),
- new Translation2d(chargingStationOuterX, chargingStationLeftY)};
+ public static Translation2d ampCenter =
+ new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996));
- // Cable bump
- public static final double cableBumpInnerX =
- innerX + Grids.outerX + Units.inchesToMeters(95.25);
- public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7);
- public static final Translation2d[] cableBumpCorners =
- new Translation2d[] {new Translation2d(cableBumpInnerX, 0.0),
- new Translation2d(cableBumpInnerX, chargingStationRightY),
- new Translation2d(cableBumpOuterX, 0.0),
- new Translation2d(cableBumpOuterX, chargingStationRightY)};
- }
-
- /**
- * Dimensions for grids and nodes
- */
- public static final class Grids {
- // X layout
- public static final double outerX = Units.inchesToMeters(54.25);
- // Centered when under cube nodes
- public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0);
- public static final double midX = outerX - Units.inchesToMeters(22.75);
- public static final double highX = outerX - Units.inchesToMeters(39.75);
-
- // Y layout
- public static final int nodeRowCount = 9;
- public static final double nodeFirstY = Units.inchesToMeters(20.19);
- public static final double nodeSeparationY = Units.inchesToMeters(22.0);
+ /** Staging locations for each note */
+ public static final class StagingLocations {
+ public static double centerlineX = fieldLength / 2.0;
- // Z layout
- public static final double cubeEdgeHigh = Units.inchesToMeters(3.0);
- public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh;
- public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh;
- public static final double highConeZ = Units.inchesToMeters(46.0);
- public static final double midConeZ = Units.inchesToMeters(34.0);
+ // need to update
+ public static double centerlineFirstY = Units.inchesToMeters(29.638);
+ public static double centerlineSeparationY = Units.inchesToMeters(66);
+ public static double spikeX = Units.inchesToMeters(114);
+ // need
+ public static double spikeFirstY = Units.inchesToMeters(161.638);
+ public static double spikeSeparationY = Units.inchesToMeters(57);
- // Translations (all nodes in the same column/row have the same X/Y coordinate)
- public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount];
- public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount];
- public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount];
- public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount];
- public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount];
+ public static Translation2d[] centerlineTranslations = new Translation2d[5];
+ public static Translation2d[] spikeTranslations = new Translation2d[3];
static {
- for (int i = 0; i < nodeRowCount; i++) {
- boolean isCube = i == 1 || i == 4 || i == 7;
- lowTranslations[i] = new Translation2d(lowX, nodeFirstY + nodeSeparationY * i);
- midTranslations[i] = new Translation2d(midX, nodeFirstY + nodeSeparationY * i);
- mid3dTranslations[i] = new Translation3d(midX, nodeFirstY + nodeSeparationY * i,
- isCube ? midCubeZ : midConeZ);
- high3dTranslations[i] = new Translation3d(highX, nodeFirstY + nodeSeparationY * i,
- isCube ? highCubeZ : highConeZ);
- highTranslations[i] = new Translation2d(highX, nodeFirstY + nodeSeparationY * i);
+ for (int i = 0; i < centerlineTranslations.length; i++) {
+ centerlineTranslations[i] =
+ new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY));
}
}
- // Complex low layout
- // shifted to account for cube vs cone rows and wide edge nodes
- public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0;
- // Centered X under cone nodes
- public static final double complexLowXCubes = lowX;
- // Centered X under cube nodes
- public static final double complexLowOuterYOffset =
- nodeFirstY - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0);
-
- public static final Translation2d[] complexLowTranslations = new Translation2d[] {
- new Translation2d(complexLowXCones, nodeFirstY - complexLowOuterYOffset),
- new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 1),
- new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 2),
- new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 3),
- new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 4),
- new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 5),
- new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 6),
- new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 7),
- new Translation2d(complexLowXCones,
- nodeFirstY + nodeSeparationY * 8 + complexLowOuterYOffset)};
+ static {
+ for (int i = 0; i < spikeTranslations.length; i++) {
+ spikeTranslations[i] =
+ new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY));
+ }
+ }
}
- /**
- * Dimensions for loading zone and substations, including the tape
- */
- public static final class LoadingZone {
- // Region dimensions
- public static final double width = Units.inchesToMeters(99.0);
- public static final double innerX = FieldConstants.fieldLength;
- public static final double midX = fieldLength - Units.inchesToMeters(132.25);
- public static final double outerX = fieldLength - Units.inchesToMeters(264.25);
- public static final double leftY = FieldConstants.fieldWidth;
- public static final double midY = leftY - Units.inchesToMeters(50.5);
- public static final double rightY = leftY - width;
- // Start at lower left next to border with opponent community
- public static final Translation2d[] regionCorners =
- new Translation2d[] {new Translation2d(midX, rightY), new Translation2d(midX, midY),
- new Translation2d(outerX, midY), new Translation2d(outerX, leftY),
- new Translation2d(innerX, leftY), new Translation2d(innerX, rightY)};
-
- // Double substation dimensions
- public static final double doubleSubstationLength = Units.inchesToMeters(14.0);
- public static final double doubleSubstationX = innerX - doubleSubstationLength;
- public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375);
-
- // Single substation dimensions
- public static final double singleSubstationWidth = Units.inchesToMeters(22.75);
- public static final double singleSubstationLeftX =
- FieldConstants.fieldLength - doubleSubstationLength - Units.inchesToMeters(88.77);
- public static final double singleSubstationCenterX =
- singleSubstationLeftX + (singleSubstationWidth / 2.0);
- public static final double singleSubstationRightX =
- singleSubstationLeftX + singleSubstationWidth;
- public static final Translation2d singleSubstationTranslation =
- new Translation2d(singleSubstationCenterX, leftY);
+ /** Each corner of the speaker * */
+ public static final class Speaker {
- public static final double singleSubstationHeight = Units.inchesToMeters(18.0);
- public static final double singleSubstationLowZ = Units.inchesToMeters(27.125);
- public static final double singleSubstationCenterZ =
- singleSubstationLowZ + (singleSubstationHeight / 2.0);
- public static final double singleSubstationHighZ =
- singleSubstationLowZ + singleSubstationHeight;
+ /** Center of the speaker opening (blue alliance) */
+ public static Pose2d centerSpeakerOpening =
+ new Pose2d(0.0, fieldWidth - Units.inchesToMeters(104.0), new Rotation2d());
}
- /**
- * Locations of staged game pieces
- */
- public static final class StagingLocations {
- public static final double centerOffsetX = Units.inchesToMeters(47.36);
- public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36);
- public static final double firstY = Units.inchesToMeters(36.19);
- public static final double separationY = Units.inchesToMeters(48.0);
- public static final Translation2d[] translations = new Translation2d[4];
+ // corners (blue alliance origin)
+ public static Translation3d topRightSpeaker = new Translation3d(Units.inchesToMeters(18.055),
+ Units.inchesToMeters(238.815), Units.inchesToMeters(83.091));
- static {
- for (int i = 0; i < translations.length; i++) {
- translations[i] = new Translation2d(positionX, firstY + (i * separationY));
- }
- }
- }
+ public static Translation3d topLeftSpeaker = new Translation3d(Units.inchesToMeters(18.055),
+ Units.inchesToMeters(197.765), Units.inchesToMeters(83.091));
- /**
- * AprilTag locations (do not flip for red alliance)
- */
- public static final Map aprilTags = Map.of(1,
- new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19),
- Units.inchesToMeters(18.22), new Rotation3d(0.0, 0.0, Math.PI)),
- 2,
- new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19),
- Units.inchesToMeters(18.22), new Rotation3d(0.0, 0.0, Math.PI)),
- 3,
- new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19),
- Units.inchesToMeters(18.22), new Rotation3d(0.0, 0.0, Math.PI)),
- 4,
- new Pose3d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74),
- Units.inchesToMeters(27.38), new Rotation3d(0.0, 0.0, Math.PI)),
- 5,
- new Pose3d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74),
- Units.inchesToMeters(27.38), new Rotation3d()),
- 6,
- new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19),
- Units.inchesToMeters(18.22), new Rotation3d()),
- 7,
- new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19),
- Units.inchesToMeters(18.22), new Rotation3d()),
- 8, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19),
- Units.inchesToMeters(18.22), new Rotation3d()));
+ public static Translation3d bottomRightSpeaker =
+ new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324));
+ public static Translation3d bottomLeftSpeaker =
+ new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324));
+ public static double centerSpeaker = topLeftSpeaker.getZ() - bottomLeftSpeaker.getZ();
- /**
- * Flips a translation to the correct side of the field based on the current alliance color. By
- * default, all translations and poses in {@link FieldConstants} are stored with the origin at
- * the rightmost point on the BLUE ALLIANCE wall.
- */
- public static Translation2d allianceFlip(Translation2d translation) {
- if (DriverStation.getAlliance().get() == Alliance.Red) {
- return new Translation2d(fieldLength - translation.getX(), translation.getY());
- } else {
- return translation;
+ public static double aprilTagWidth = Units.inchesToMeters(6.50);
+ public static AprilTagFieldLayout aprilTags;
+
+ static {
+ try {
+ aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile);
+ } catch (IOException e) {
+ throw new RuntimeException(e);
}
}
@@ -261,125 +107,49 @@ public static Translation2d allianceFlip(Translation2d translation) {
* @return Pose2d flipped to Red Alliance
*/
public static Pose2d allianceFlip(Pose2d pose) {
- if (DriverStation.getAlliance().get() == Alliance.Red) {
- return new Pose2d(fieldLength - pose.getX(), pose.getY(),
- new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin()));
- } else {
- return pose;
+ Optional ally = DriverStation.getAlliance();
+ if (ally.isPresent()) {
+ if (ally.get() == Alliance.Red) {
+ pose = new Pose2d(fieldLength - pose.getX(), pose.getY(),
+ new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin()));
+ }
}
+ return pose;
}
/**
- * elementwise maximum
+ * Flips a pose to the correct side of the field based on the current alliance color. By
+ * default, all translations and poses in {@link FieldConstants} are stored with the origin at
+ * the rightmost point on the BLUE ALLIANCE wall.
*
- * @param x Translation
- * @return Max Translation
+ * @param pose Initial Pose
+ * @return Pose2d flipped to Red Alliance
*/
- private static Translation2d max(Translation2d... x) {
- Translation2d r = x[0];
- for (int i = 1; i < x.length; i++) {
- if (x[i].getX() > r.getX()) {
- r = new Translation2d(x[i].getX(), r.getY());
- }
- if (x[i].getY() > r.getY()) {
- r = new Translation2d(r.getX(), x[i].getY());
+ public static Translation2d allianceFlip(Translation2d pose) {
+ Optional ally = DriverStation.getAlliance();
+ if (ally.isPresent()) {
+ if (ally.get() == Alliance.Red) {
+ pose = new Translation2d(fieldLength - pose.getX(), pose.getY());
}
}
- return r;
- }
-
- /**
- * Vector dot product for Translation2d types
- *
- * @param a Translation A
- * @param b Translation B
- * @return Dot
- */
- private static double dot(Translation2d a, Translation2d b) {
- return a.getX() * b.getX() + a.getY() * b.getY();
- }
-
- /**
- * https://www.youtube.com/watch?v=PMltMdi1Wzg
- *
- * @param P P
- * @param p0 P0
- * @param p1 P1
- * @return Line SDF
- */
- private static double lineSdf(Translation2d p, Translation2d p0, Translation2d p1) {
- Translation2d pa = p.minus(p0);
- Translation2d ba = p1.minus(p0);
- double h = MathUtil.clamp(dot(pa, ba) / dot(ba, ba), 0.0, 1.0);
- return pa.minus(ba.times(h)).getNorm();
- }
-
- /**
- * https://www.youtube.com/watch?v=62-pRVZuS5c
- *
- * @param P P
- * @param p0 P0
- * @param p1 P1
- * @return Box SDF
- */
- private static double boxSdf(Translation2d P, Translation2d p0, Translation2d p1) {
- Translation2d min =
- new Translation2d(Math.min(p0.getX(), p1.getX()), Math.min(p0.getY(), p1.getY()));
- Translation2d max =
- new Translation2d(Math.max(p0.getX(), p1.getX()), Math.max(p0.getY(), p1.getY()));
- Translation2d b = max.minus(min).times(0.5);
- Translation2d p = P.minus(b).minus(min);
- Translation2d d = new Translation2d(Math.abs(p.getX()), Math.abs(p.getY())).minus(b);
- return max(d, new Translation2d()).getNorm() + Math.min(Math.max(d.getX(), d.getY()), 0.0);
+ return pose;
}
/**
- * find minimum distance given a set of distances (variadic min)
+ * Flips a pose to the correct side of the field based on the current alliance color. By
+ * default, all translations and poses in {@link FieldConstants} are stored with the origin at
+ * the rightmost point on the BLUE ALLIANCE wall.
*
- * @param x Something
- * @return UNion SDF
+ * @param rotation Initial Pose
+ * @return Rotation2d flipped to Red Alliance
*/
- private static double unionSdf(double... x) {
- double y = x[0];
- for (int i = 1; i < x.length; i++) {
- if (x[i] < y) {
- y = x[i];
+ public static Rotation2d allianceFlip(Rotation2d rotation) {
+ Optional ally = DriverStation.getAlliance();
+ if (ally.isPresent()) {
+ if (ally.get() == Alliance.Red) {
+ rotation = new Rotation2d(-rotation.getCos(), rotation.getSin());
}
}
- return y;
+ return rotation;
}
-
- /**
- * Find the minimum distance from a circular robot to an obstacle on the field.
- *
- * @param p Starting translation
- * @param radius Radius
- * @return field SDF
- */
- public static double fieldSdf(Translation2d p, double radius) {
- return fieldSdf(p) - radius;
- }
-
- /**
- * Find the minimum distance from a point to an obstacle on the field.
- */
- public static double fieldSdf(Translation2d p) {
- Translation2d q = new Translation2d(fieldLength - p.getX(), p.getY());
- Translation2d gridCorner = new Translation2d(Grids.outerX, Community.leftY);
-
- double inField =
- -boxSdf(p, new Translation2d(), new Translation2d(fieldLength, fieldWidth));
-
- double blueGrid = boxSdf(p, new Translation2d(), gridCorner);
- double blueRail = lineSdf(p, gridCorner, Community.regionCorners[2]);
- double blueRamp =
- boxSdf(p, Community.chargingStationCorners[0], Community.chargingStationCorners[3]);
- double redGrid = boxSdf(q, new Translation2d(), gridCorner);
- double redRail = lineSdf(q, gridCorner, Community.regionCorners[2]);
- double redRamp =
- boxSdf(q, Community.chargingStationCorners[0], Community.chargingStationCorners[3]);
-
- return unionSdf(inField, blueGrid, blueRail, blueRamp, redGrid, redRail, redRamp);
- }
-
}
diff --git a/src/main/java/frc/lib/util/LazySelectCommand.java b/src/main/java/frc/lib/util/LazySelectCommand.java
new file mode 100644
index 0000000..ae5e8ef
--- /dev/null
+++ b/src/main/java/frc/lib/util/LazySelectCommand.java
@@ -0,0 +1,73 @@
+package frc.lib.util;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+import java.util.Map;
+import java.util.function.Supplier;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
+
+/**
+ * Similar to {@link SelectCommand} but allows commands for all variants of an enum instead. Also
+ * doesn't require all subsystems of all commands, only the selected command on run time.
+ */
+public class LazySelectCommand extends Command {
+ private final Map m_commands;
+ private final Supplier m_selector;
+ private Command m_command;
+
+ /**
+ * Create new LazySelectCommand. Note that the length of enumValues and commandValues must be
+ * the same.
+ *
+ * @throws IllegalArgumentException when enumValues and commandValues have different lengths.
+ */
+ public LazySelectCommand(Map commands, Supplier selector) {
+ m_commands = requireNonNullParam(commands, "commands", "LazySelectCommand");
+ m_selector = requireNonNullParam(selector, "selector", "LazySelectCommand");
+ }
+
+ @Override
+ public void initialize() {
+ m_command = m_commands.getOrDefault(m_selector.get(), Commands.none());
+ m_command.schedule();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (interrupted) {
+ m_command.cancel();
+ }
+ m_command = null;
+ }
+
+ @Override
+ public void execute() {}
+
+ @Override
+ public boolean isFinished() {
+ // because we're between `initialize` and `end`, `m_command` is necessarily not null
+ // but if called otherwise and m_command is null,
+ // it's UB, so we can do whatever we want -- like return true.
+ return m_command == null || !m_command.isScheduled();
+ }
+
+ /**
+ * Whether the given command should run when the robot is disabled. Override to return true if
+ * the command should run when disabled.
+ *
+ * @return true. Otherwise, this proxy would cancel commands that do run when disabled.
+ */
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ super.initSendable(builder);
+ builder.addStringProperty("lazy select command",
+ () -> m_command == null ? "null" : m_command.getName(), null);
+ }
+}
diff --git a/src/main/java/frc/lib/util/LongArrayList.java b/src/main/java/frc/lib/util/LongArrayList.java
new file mode 100644
index 0000000..cb05dac
--- /dev/null
+++ b/src/main/java/frc/lib/util/LongArrayList.java
@@ -0,0 +1,117 @@
+package frc.lib.util;
+
+import java.util.List;
+import java.util.Objects;
+import java.util.RandomAccess;
+import frc.lib.profiling.LoggingProfiler;
+
+/**
+ * An optimized long array. Used in lieu of an {@code ArrayList} to avoid boxing (and the
+ * performance implications that come with it).
+ *
+ *
+ * Current interface is limited to the methods used by {@link LoggingProfiler}, though its interface
+ * may expand in the future to match the full {@link List} interface.
+ */
+public class LongArrayList implements RandomAccess, Cloneable, java.io.Serializable {
+ private static final long serialVersionUID = -7046029254386353130L;
+
+ /**
+ * During list creation, many reallocations at low capacities may be common, so we jump from 0
+ * to 10 before our normal reallocation scheme.
+ */
+ public static final int DEFAULT_INITIAL_CAPACITY = 10;
+
+ protected transient long[] backing;
+ protected int size;
+
+ /**
+ * A static, final, empty array to be used as default array in allocations.
+ */
+ private static final long[] DEFAULT_EMPTY_ARRAY = {};
+
+
+ /**
+ * Creates a new array list with {@link #DEFAULT_INITIAL_CAPACITY} capacity.
+ */
+ public LongArrayList() {
+ backing = DEFAULT_EMPTY_ARRAY;
+ }
+
+ /**
+ * Grows this arraylist, ensuring it can contain {@code capacity} longs.
+ *
+ * @param capacity the new minimum capacity for this array list.
+ */
+ private void grow(int capacity) {
+ if (capacity <= backing.length) {
+ return;
+ }
+ if (backing != DEFAULT_EMPTY_ARRAY) {
+ capacity = (int) Math.max(
+ Math.min((long) backing.length + (backing.length >> 1), Integer.MAX_VALUE - 8),
+ capacity);
+ } else if (capacity < DEFAULT_INITIAL_CAPACITY) {
+ capacity = DEFAULT_INITIAL_CAPACITY;
+ }
+ backing = forceCapacity(backing, capacity, size);
+ assert size <= backing.length;
+ }
+
+ /**
+ * Appends {@code k} to the end of this list.
+ */
+ public void add(final long k) {
+ grow(size + 1);
+ backing[size++] = k;
+ assert size <= backing.length;
+ }
+
+ /**
+ * Get long element at index {@code index}.
+ */
+ public long get(int index) {
+ Objects.checkIndex(index, size);
+ return backing[index];
+ }
+
+ /**
+ * Checks if the list has no elements.
+ */
+ public boolean isEmpty() {
+ return size == 0;
+ }
+
+ /**
+ * Returns the number of elements in this list.
+ */
+ public int size() {
+ return size;
+ }
+
+ /**
+ * Removes the element at the specified position in this list.
+ */
+ public long remove(final int index) {
+ Objects.checkIndex(index, size);
+ final long[] a = this.backing;
+ final long old = a[index];
+ size--;
+ if (index != size) {
+ System.arraycopy(a, index + 1, a, index, size);
+ }
+ assert size <= a.length;
+ return old;
+ }
+
+ public void clear() {
+ this.size = 0;
+ }
+
+ private static long[] forceCapacity(final long[] array, final int length, final int preserve) {
+ final long[] newArray = new long[length];
+ System.arraycopy(array, 0, newArray, 0, preserve);
+ return newArray;
+ }
+
+}
diff --git a/src/main/java/frc/lib/util/ReconfigurableSimpleMotorFeedforward.java b/src/main/java/frc/lib/util/ReconfigurableSimpleMotorFeedforward.java
new file mode 100644
index 0000000..4294871
--- /dev/null
+++ b/src/main/java/frc/lib/util/ReconfigurableSimpleMotorFeedforward.java
@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.lib.util;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
+public class ReconfigurableSimpleMotorFeedforward {
+ /** The static gain. */
+ public double ks;
+
+ /** The velocity gain. */
+ public double kv;
+
+ /** The acceleration gain. */
+ public double ka;
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
+ * dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kv The velocity gain.
+ * @param ka The acceleration gain.
+ * @throws IllegalArgumentException for kv < zero.
+ * @throws IllegalArgumentException for ka < zero.
+ */
+ public ReconfigurableSimpleMotorFeedforward(double ks, double kv, double ka) {
+ this.ks = ks;
+ this.kv = kv;
+ this.ka = ka;
+ }
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
+ * to zero. Units of the gain values will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kv The velocity gain.
+ */
+ public ReconfigurableSimpleMotorFeedforward(double ks, double kv) {
+ this(ks, kv, 0);
+ }
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint.
+ * @param acceleration The acceleration setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity, double acceleration) {
+ return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+ }
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param currentVelocity The current velocity setpoint.
+ * @param nextVelocity The next velocity setpoint.
+ * @param dtSeconds Time between velocity setpoints in seconds.
+ * @return The computed feedforward.
+ */
+ public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+ var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+ var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+ var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
+ var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
+
+ return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+ * be zero).
+ *
+ * @param velocity The velocity setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity) {
+ return calculate(velocity, 0);
+ }
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply and an
+ * acceleration. Useful for ensuring that velocity and acceleration constraints for a
+ * trapezoidal profile are simultaneously achievable - enter the acceleration constraint, and
+ * this will give you a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - ks - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply and an
+ * acceleration. Useful for ensuring that velocity and acceleration constraints for a
+ * trapezoidal profile are simultaneously achievable - enter the acceleration constraint, and
+ * this will give you a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The minimum possible velocity at the given acceleration.
+ */
+ public double minAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + ks - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+ * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+ * simultaneously achievable - enter the velocity constraint, and this will give you a
+ * simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+ return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
+ * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+ * simultaneously achievable - enter the velocity constraint, and this will give you a
+ * simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ public double minAchievableAcceleration(double maxVoltage, double velocity) {
+ return maxAchievableAcceleration(-maxVoltage, velocity);
+ }
+}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
index a1fa103..492e905 100644
--- a/src/main/java/frc/lib/util/swerve/SwerveModule.java
+++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -1,123 +1,53 @@
package frc.lib.util.swerve;
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.math.Conversions;
import frc.robot.Constants;
+import frc.robot.Robot;
/**
- * Swerve Module
+ * Swerve Module Subsystem
*/
public class SwerveModule {
public int moduleNumber;
private Rotation2d angleOffset;
+ // private double lastAngle;
- private TalonFX mAngleMotor;
- private TalonFX mDriveMotor;
- private CANcoder angleEncoder;
- private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration();
- private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration();
- private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration();
-
- private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(
- Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);
-
- /* drive motor control requests */
- private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
- private final VelocityVoltage driveVelocity = new VelocityVoltage(0);
-
- /* angle motor control requests */
- private final PositionVoltage anglePosition = new PositionVoltage(0);
+ private SwerveModuleIO io;
+ private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged();
/**
* Swerve Module
*
* @param moduleNumber Module Number
- * @param driveMotorID CAN ID of the Drive Motor
- * @param angleMotorID CAN ID of the Angle Motor
- * @param cancoderID CAN ID of the CANCoder
* @param angleOffset Angle Offset of the CANCoder to align the wheels
*/
- public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
- Rotation2d angleOffset) {
- this.moduleNumber = moduleNumber;
- this.angleOffset = angleOffset;
+ public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io) {
+ this.io = io;
- /* Angle Encoder Config */
- swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert;
-
- angleEncoder = new CANcoder(cancoderID);
- angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
-
- /* Angle Motor Config */
- /* Motor Inverts and Neutral Mode */
- swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert;
- swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode;
-
- /* Gear Ratio and Wrapping Config */
- swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio;
- swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true;
+ this.moduleNumber = moduleNumber;
- /* Current Limiting */
- swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
- Constants.Swerve.angleEnableCurrentLimit;
- swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit;
- swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold =
- Constants.Swerve.angleCurrentThreshold;
- swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold =
- Constants.Swerve.angleCurrentThresholdTime;
+ this.angleOffset = angleOffset;
- /* PID Config */
- swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP;
- swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI;
- swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD;
- mAngleMotor = new TalonFX(angleMotorID);
- mAngleMotor.getConfigurator().apply(swerveAngleFXConfig);
+ // lastAngle = getState().angle.getDegrees();
+ io.updateInputs(inputs);
resetToAbsolute();
+ Logger.processInputs("SwerveModule" + moduleNumber, inputs);
+ }
- /* Drive Motor Config */
- /* Motor Inverts and Neutral Mode */
- swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert;
- swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode;
-
- /* Gear Ratio Config */
- swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio;
-
- /* Current Limiting */
- swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
- Constants.Swerve.driveEnableCurrentLimit;
- swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit;
- swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold =
- Constants.Swerve.driveCurrentThreshold;
- swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold =
- Constants.Swerve.driveCurrentThresholdTime;
-
- /* PID Config */
- swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP;
- swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI;
- swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD;
-
- /* Open and Closed Loop Ramping */
- swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod =
- Constants.Swerve.openLoopRamp;
- swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp;
-
- swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod =
- Constants.Swerve.closedLoopRamp;
- swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod =
- Constants.Swerve.closedLoopRamp;
- mDriveMotor = new TalonFX(driveMotorID);
- mDriveMotor.getConfigurator().apply(swerveDriveFXConfig);
- mDriveMotor.getConfigurator().setPosition(0.0);
+ /**
+ * Update inputs for a Swerve Module.
+ */
+ public void periodic() {
+ Robot.profiler.push("updateInputs");
+ io.updateInputs(inputs);
+ Robot.profiler.swap("processInputs");
+ Logger.processInputs("SwerveModule" + moduleNumber, inputs);
+ Robot.profiler.pop();
}
/**
@@ -128,8 +58,12 @@ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int ca
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
- mAngleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations()));
+ io.setAngleMotor(desiredState.angle.getRotations());
setSpeed(desiredState, isOpenLoop);
+ SmartDashboard.putNumber("desired state speed/" + moduleNumber,
+ desiredState.speedMetersPerSecond);
+ SmartDashboard.putNumber("desired state angle/" + moduleNumber,
+ desiredState.angle.getDegrees());
}
/**
@@ -140,14 +74,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
*/
private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
if (isOpenLoop) {
- driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
- mDriveMotor.setControl(driveDutyCycle);
+ double power = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
+ io.setDriveMotorPower(power);
} else {
- driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond,
- Constants.Swerve.wheelCircumference);
- driveVelocity.FeedForward =
- driveFeedForward.calculate(desiredState.speedMetersPerSecond);
- mDriveMotor.setControl(driveVelocity);
+ io.setDriveMotor(desiredState.speedMetersPerSecond);
}
}
@@ -157,7 +87,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
* @return The rotation of the CANCoder in {@link Rotation2d}
*/
public Rotation2d getCANcoder() {
- return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue());
+ return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder);
}
/**
@@ -165,7 +95,8 @@ public Rotation2d getCANcoder() {
*/
public void resetToAbsolute() {
double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations();
- mAngleMotor.setPosition(absolutePosition);
+ io.setPositionAngleMotor(absolutePosition);
+ inputs.angleMotorSelectedPosition = absolutePosition;
}
/**
@@ -175,9 +106,9 @@ public void resetToAbsolute() {
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
- Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(),
+ Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity,
Constants.Swerve.wheelCircumference),
- Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()));
+ Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
}
/**
@@ -187,8 +118,9 @@ public SwerveModuleState getState() {
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
- Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(),
+ Conversions.rotationsToMeters(inputs.driveMotorSelectedPosition,
Constants.Swerve.wheelCircumference),
- Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()));
+ Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
+
}
}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
new file mode 100644
index 0000000..011a051
--- /dev/null
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
@@ -0,0 +1,33 @@
+package frc.lib.util.swerve;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** IO Class for SwerveModule */
+public interface SwerveModuleIO {
+ /** Inputs Class for SwerveModule */
+ @AutoLog
+ public static class SwerveModuleInputs {
+ public double driveMotorSelectedPosition;
+ public double driveMotorSelectedSensorVelocity;
+ public double angleMotorSelectedPosition;
+ public double absolutePositionAngleEncoder;
+ public double[] odometryTimestamps;
+ // public double driveMotorTemp;
+ // public double angleMotorTemp;
+ }
+
+ public default void setModNumber(int number) {}
+
+ public default void updateInputs(SwerveModuleInputs inputs) {}
+
+ public default void setDriveMotor(double mps) {}
+
+ public default void setDriveMotorPower(double power) {}
+
+ public default void setAngleMotor(double angle) {}
+
+ public default void setAngleSelectedSensorPosition(double angle) {}
+
+ public default void setPositionAngleMotor(double absolutePosition) {}
+
+}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
new file mode 100644
index 0000000..c2f3d38
--- /dev/null
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
@@ -0,0 +1,161 @@
+package frc.lib.util.swerve;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.lib.math.Conversions;
+import frc.robot.Constants;
+
+/**
+ * Swerve Module IO
+ */
+public class SwerveModuleReal implements SwerveModuleIO {
+
+ private TalonFX mAngleMotor;
+ private TalonFX mDriveMotor;
+ private CANcoder angleEncoder;
+ private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration();
+ private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration();
+ private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration();
+
+ private StatusSignal driveMotorSelectedPosition;
+ private StatusSignal driveMotorSelectedSensorVelocity;
+ private StatusSignal angleMotorSelectedPosition;
+ private StatusSignal absolutePositionAngleEncoder;
+
+ /* drive motor control requests */
+ private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
+ private final VelocityVoltage driveVelocity = new VelocityVoltage(0);
+
+ /* angle motor control requests */
+ private final PositionVoltage anglePosition = new PositionVoltage(0);
+
+ /** Instantiating motors and Encoders */
+ public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID) {
+
+ angleEncoder = new CANcoder(cancoderID, "canivore");
+ mDriveMotor = new TalonFX(driveMotorID, "canivore");
+ mAngleMotor = new TalonFX(angleMotorID, "canivore");
+
+ configAngleEncoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveMotorSelectedPosition = mDriveMotor.getPosition();
+ driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity();
+ angleMotorSelectedPosition = mAngleMotor.getPosition();
+ absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition();
+ }
+
+ private void configAngleMotor() {
+ /* Angle Motor Config */
+ /* Motor Inverts and Neutral Mode */
+ swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert;
+ swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode;
+
+ /* Gear Ratio and Wrapping Config */
+ swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio;
+ swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true;
+
+ /* Current Limiting */
+ swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
+ Constants.Swerve.angleEnableCurrentLimit;
+ swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit;
+ swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold =
+ Constants.Swerve.angleCurrentThreshold;
+ swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold =
+ Constants.Swerve.angleCurrentThresholdTime;
+
+ /* PID Config */
+ swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP;
+ swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI;
+ swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD;
+
+ mAngleMotor.getConfigurator().apply(swerveAngleFXConfig);
+ }
+
+ private void configDriveMotor() {
+ /* Drive Motor Config */
+ /* Motor Inverts and Neutral Mode */
+ swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert;
+ swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode;
+
+ /* Gear Ratio Config */
+ swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio;
+
+ /* Current Limiting */
+ swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
+ Constants.Swerve.driveEnableCurrentLimit;
+ swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit;
+ swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold =
+ Constants.Swerve.driveCurrentThreshold;
+ swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold =
+ Constants.Swerve.driveCurrentThresholdTime;
+
+ /* PID Config */
+ swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP;
+ swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI;
+ swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD;
+ swerveDriveFXConfig.Slot0.kS = Constants.Swerve.driveKS;
+ swerveDriveFXConfig.Slot0.kV = Constants.Swerve.driveKV;
+ swerveDriveFXConfig.Slot0.kA = Constants.Swerve.driveKA;
+
+ /* Open and Closed Loop Ramping */
+ swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod =
+ Constants.Swerve.openLoopRamp;
+ swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp;
+
+ swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod =
+ Constants.Swerve.closedLoopRamp;
+ swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod =
+ Constants.Swerve.closedLoopRamp;
+
+ mDriveMotor.getConfigurator().apply(swerveDriveFXConfig);
+ mDriveMotor.getConfigurator().setPosition(0.0);
+ }
+
+ private void configAngleEncoder() {
+ /* Angle Encoder Config */
+ swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert;
+
+ angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
+ }
+
+ @Override
+ public void setAngleMotor(double angle) {
+ mAngleMotor.setControl(anglePosition.withPosition(angle));
+ }
+
+ @Override
+ public void setDriveMotor(double mps) {
+ // driveVelocity.FeedForward = feedforward;
+ double driveRPS = Conversions.metersPerSecondToRotationPerSecond(mps,
+ Constants.Swerve.wheelCircumference);
+ driveVelocity.Velocity = driveRPS;
+ mDriveMotor.setControl(driveVelocity);
+ }
+
+ @Override
+ public void updateInputs(SwerveModuleInputs inputs) {
+ BaseStatusSignal.refreshAll(driveMotorSelectedPosition, driveMotorSelectedSensorVelocity,
+ angleMotorSelectedPosition, absolutePositionAngleEncoder);
+ inputs.driveMotorSelectedPosition = driveMotorSelectedPosition.getValue();
+ inputs.driveMotorSelectedSensorVelocity = driveMotorSelectedSensorVelocity.getValue();
+ inputs.angleMotorSelectedPosition = angleMotorSelectedPosition.getValue();
+ inputs.absolutePositionAngleEncoder = absolutePositionAngleEncoder.getValue();
+ // inputs.driveMotorTemp = mDriveMotor.getDeviceTemp().getValueAsDouble();
+ // inputs.angleMotorTemp = mAngleMotor.getDeviceTemp().getValueAsDouble();
+ }
+
+ @Override
+ public void setPositionAngleMotor(double absolutePosition) {
+ mAngleMotor.setPosition(absolutePosition);
+ }
+
+}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java
new file mode 100644
index 0000000..7cc8166
--- /dev/null
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java
@@ -0,0 +1,92 @@
+package frc.lib.util.swerve;
+
+import org.littletonrobotics.junction.LoggedRobot;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.lib.math.Conversions;
+import frc.robot.Constants;
+
+/**
+ * Swerve Module Sim
+ */
+public class SwerveModuleSim implements SwerveModuleIO {
+ public int moduleNumber;
+
+ private FlywheelSim driveSim =
+ new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, 0.025);
+
+ private double angle;
+ private double distance;
+
+ private double driveAppliedVolts = 0.0;
+ private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
+ private PIDController driveFeedback = new PIDController(0.5, 0.0, 0.0);
+
+ /**
+ * Swerve Module Sim
+ */
+ public SwerveModuleSim() {}
+
+ @Override
+ public void setModNumber(int number) {
+ this.moduleNumber = number;
+ }
+
+ @Override
+ public void updateInputs(SwerveModuleInputs inputs) {
+ driveSim.update(LoggedRobot.defaultPeriodSecs);
+ double driveSpeed = Units.radiansToRotations(driveSim.getAngularVelocityRadPerSec());
+ this.distance += driveSpeed * LoggedRobot.defaultPeriodSecs;
+ inputs.driveMotorSelectedPosition = this.distance;
+ inputs.driveMotorSelectedSensorVelocity = driveSpeed;
+
+ inputs.angleMotorSelectedPosition = angle;
+
+ inputs.absolutePositionAngleEncoder = angle;
+ inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+ }
+
+ /**
+ * Set drive motor in Meter per Sec
+ *
+ * @param mps Meters per Second
+ */
+ public void setDriveMotor(double mps) {
+ double rpm = Conversions.metersPerSecondToRotationPerSecond(mps,
+ Constants.Swerve.wheelCircumference);
+ driveFeedback.setSetpoint(rpm);
+ double driveFF = driveFeedforward.calculate(mps);
+ SmartDashboard.putNumber("ff/" + moduleNumber, driveFF);
+ double volts = driveFeedback.calculate(mps) + driveFF;
+ if (rpm == 0) {
+ volts = 0;
+ }
+ SmartDashboard.putNumber("Drive volts/" + moduleNumber, volts);
+ setDriveVoltage(volts);
+ }
+
+ /**
+ * Set Angle for steering motor
+ *
+ * @param angle Angle to set
+ */
+ public void setAngleMotor(double angle) {
+ this.angle = angle;
+ }
+
+ /**
+ * Set Drive Voltage
+ *
+ * @param volts Voltage
+ */
+ public void setDriveVoltage(double volts) {
+ driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+ driveSim.setInputVoltage(driveAppliedVolts);
+ }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f9ac7fb..82b77c0 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -7,31 +7,195 @@
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;
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.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.util.Color;
+import frc.lib.util.FieldConstants;
/**
- * Project Constants
+ * Constants file.
*/
public final class Constants {
- public static final double stickDeadband = 0.1;
+ /**
+ * Stick Deadband
+ */
+ public static final double STICK_DEADBAND = 0.1;
+
+ /**
+ * Driver ID
+ */
+ public static final int DRIVER_ID = 0;
+
+ /**
+ * Operator ID
+ */
+ public static final int OPERATOR_ID = 1;
+
+ /**
+ * How far in the future we should "lead" the aiming of the shooter for shooting while moving.
+ */
+ public static final double LEAD_GAIN = 0.3;
+
+
+ /**
+ * MoveToPos constants.
+ */
+ public static class SwerveTransformPID {
+ public static final double PID_XKP = 3.5;
+ public static final double PID_XKI = 0.0;
+ public static final double PID_XKD = 0.0;
+ public static final double PID_YKP = 3.5;
+ public static final double PID_YKI = 0.0;
+ public static final double PID_YKD = 0.0;
+ public static final double PID_TKP = 3.0;
+ public static final double PID_TKI = 0.0;
+ public static final double PID_TKD = 0.0;
+
+ public static final double MAX_ANGULAR_VELOCITY = 9.0;
+ public static final double MAX_ANGULAR_ACCELERATION = 9 * 5;
+ public static final double STD_DEV_MOD = 2.0;
+ }
+
+ /**
+ * Motor CAN id's.
+ */
+ public static final class Motors {
+ /**
+ * Shooter Id's
+ */
+ public static final class Shooter {
+ public static final int SHOOTER_TOP_ID = 13;
+ public static final int SHOOTER_BOTTOM_ID = 15;
+ }
+
+ /**
+ * Class for elevator and wrist motor constants
+ */
+ public static final class ElevatorWrist {
+ public static final int ELEVATOR_RIGHT_NEO_ID = 57;
+ public static final int ELEVATOR_LEFT_NEO_ID = 12;
+ public static final int WRIST_NEO_ID = 52;
+ }
+
+ /**
+ * Intake and indexer motor constants
+ */
+ public static final class Intake {
+ public static final int INTAKE_MOTOR_ID_LEFT = 14;
+ public static final int INTAKE_MOTOR_ID_RIGHT = 61;
+ public static final int INDEXER_MOTOR_ID = 32;
+ }
+
+ /**
+ * Climber motor constants
+ */
+ public static final class Climber {
+ public static final int LEFT_MOTOR_ID = 60;
+ public static final int RIGHT_MOTOR_ID = 48;
+ }
+ }
+
+ /**
+ * Camera offset constants.
+ */
+ public static class CameraConstants {
+
+ public static double XY_STD_DEV_COEFF = 0.005;
+ public static double THETA_STD_DEV_COEFF = 0.01;
+
+ /**
+ * Constants for Front Left Camera
+ */
+ public static class FrontLeftFacingCamera {
+ public static final double ROLL = 0.0;
+ public static final double PITCH = Math.toRadians(0);
+ public static final double YAW = Math.toRadians(12.0);
+ public static final Transform3d KCAMERA_TO_ROBOT = new Transform3d(
+ new Translation3d(Units.inchesToMeters(17), Units.inchesToMeters(4.0),
+ Units.inchesToMeters(15)),
+ new Rotation3d(ROLL, PITCH, YAW));
+
+ public static final String CAMERA_NAME = "front-left";
+ public static final String CAMERA_IP = "10.55.72.12";
+ public static final double LARGEST_DISTANCE = 0.1;
+ }
+
+ /**
+ * Constants for Front Right Camera
+ */
+ public static class FrontRightFacingCamera {
+ public static final double ROLL = Math.toRadians(180);
+ public static final double PITCH = Math.toRadians(0);
+ public static final double YAW = Math.toRadians(-6);
+ public static final Transform3d KCAMERA_TO_ROBOT = new Transform3d(
+ new Translation3d(Units.inchesToMeters(17), Units.inchesToMeters(-5),
+ Units.inchesToMeters(15)),
+ new Rotation3d(ROLL, PITCH, YAW));
+
+ public static final String CAMERA_NAME = "front-right";
+ public static final String CAMERA_IP = "10.55.72.10";
+ public static final double LARGEST_DISTANCE = 0.1;
+ }
+
+ /**
+ * Constants for Back Left Camera
+ */
+ public static class BackLeftFacingCamera {
+ public static final double ROLL = 0.0;
+ public static final double PITCH = Math.toRadians(0);
+ public static final double YAW = Math.toRadians(184);
+ public static final Transform3d KCAMERA_TO_ROBOT = new Transform3d(
+ new Translation3d(Units.inchesToMeters(-13.0), Units.inchesToMeters(14),
+ Units.inchesToMeters(0)),
+ new Rotation3d(ROLL, PITCH, YAW));
+
+ public static final String CAMERA_NAME = "back-left";
+ public static final String CAMERA_IP = "10.55.72.13";
+ public static final double LARGEST_DISTANCE = 0.1;
+ }
+
+ // /**
+ // * Constants for Back Right Camera
+ // */
+ // public static class BackRightFacingCamera {
+ // public static final double ROLL = 0.0;
+ // public static final double PITCH = Math.toRadians(0);
+ // public static final double YAW = Math.toRadians(180);
+ // public static final Transform3d KCAMERA_TO_ROBOT =
+ // new Transform3d(new Translation3d(Units.inchesToMeters(12.831),
+ // Units.inchesToMeters(-8.56), Units.inchesToMeters(17.85)),
+ // new Rotation3d(ROLL, PITCH, YAW)).inverse();
+
+ // public static final String CAMERA_NAME = "back-right";
+ // // public static final String CAMERA_IP = "10.55.72.10";
+ // public static final double LARGEST_DISTANCE = 0.1;
+ // }
+
+ }
/**
* Swerve Constants
*/
public static final class Swerve {
+ public static final double AUTO_ROTATION_KP = 5.0;
+ public static final double AUTO_ROTATION_KI = 0.0;
+ public static final double AUTO_ROTATION_KD = 0.0;
+
public static final edu.wpi.first.wpilibj.SPI.Port navXID =
edu.wpi.first.wpilibj.SPI.Port.kMXP;
- public static final boolean invertGyro = false;
+ public static final boolean invertGyro = true;
public static final boolean isFieldRelative = true;
public static final boolean isOpenLoop = false;
/* Drivetrain Constants */
- public static final double trackWidth = Units.inchesToMeters(22.5);
- public static final double wheelBase = Units.inchesToMeters(29.0);
- public static final double wheelDiameter = Units.inchesToMeters(4.0);
+ public static final double trackWidth = Units.inchesToMeters(23.75); // TODO
+ public static final double wheelBase = Units.inchesToMeters(17.75); // TODO
+ public static final double wheelDiameter = Units.inchesToMeters(3.8); // TODO
public static final double wheelCircumference = wheelDiameter * Math.PI;
public static final Translation2d MOD0_MODOFFSET =
new Translation2d(wheelBase / 2.0, trackWidth / 2.0);
@@ -72,7 +236,7 @@ public static final class Swerve {
/*
* These values are used by the drive falcon to ramp in open loop and closed loop driving.
- * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc
+ * We found a small open loop ramp (0.25) helps with wear, tipping, etc
*/
public static final double openLoopRamp = 0.25;
public static final double closedLoopRamp = 0.0;
@@ -95,64 +259,94 @@ public static final class Swerve {
/* Swerve Profiling Values */
/** Meters per Second */
- public static final double maxSpeed = 4.5;
+ public static final double maxSpeed = 10.0;
+ public static final double AUTO_MAX_SPEED = 3.0;
/** Radians per Second */
- public static final double maxAngularVelocity = 10.0;
+ public static final double maxAngularVelocity = 15.0;
/* Neutral Modes */
public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast;
public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake;
/* Module Specific Constants */
+
/**
* Front Left Module - Module 0
*/
public static final class Mod0 {
- public static final int driveMotorID = 10;
- public static final int angleMotorID = 8;
- public static final int canCoderID = 10;
- public static final Rotation2d angleOffset = Rotation2d.fromDegrees(115.400390625);
+ public static final int driveMotorID = 6;
+ public static final int angleMotorID = 51;
+ public static final int canCoderID = 4;
+ // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(183.955078125);
+ public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.496826);
+
}
/**
* Front Right Module - Module 1
*/
public static final class Mod1 {
- public static final int driveMotorID = 3;
- public static final int angleMotorID = 9;
- public static final int canCoderID = 1;
- public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-55.37109375);
+ public static final int driveMotorID = 2;
+ public static final int angleMotorID = 40;
+ public static final int canCoderID = 2;
+ // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(325.01953125);
+ public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.405518 + 0.5);
+
}
/**
* Back Left Module - Module 2
*/
public static final class Mod2 {
- public static final int driveMotorID = 2;
- public static final int angleMotorID = 40;
- public static final int canCoderID = 2;
- public static final Rotation2d angleOffset = Rotation2d.fromDegrees(145.01953125);
+ public static final int driveMotorID = 3;
+ public static final int angleMotorID = 9;
+ public static final int canCoderID = 1;
+ // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(124.62890625);
+ public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.348145);
+
}
/**
* Back Right Module - Module 3
*/
public static final class Mod3 {
- public static final int driveMotorID = 6;
- public static final int angleMotorID = 51;
- public static final int canCoderID = 4;
- public static final Rotation2d angleOffset = Rotation2d.fromDegrees(3.955078125);
+ public static final int driveMotorID = 10;
+ public static final int angleMotorID = 8;
+ public static final int canCoderID = 10;
+ // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(295.400390625);
+ public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.317627 + 0.5);
}
public static final HolonomicPathFollowerConfig pathFollowerConfig =
- new HolonomicPathFollowerConfig(new PIDConstants(5.0, 0, 0), // Translation constants
- new PIDConstants(5.0, 0, 0), // Rotation constants
+ new HolonomicPathFollowerConfig(new PIDConstants(5.0, 0, 0),
+ new PIDConstants(AUTO_ROTATION_KP, AUTO_ROTATION_KI, AUTO_ROTATION_KD),
// Drive base radius (distance from center to furthest module)
- maxSpeed, MOD0_MODOFFSET.getNorm(), new ReplanningConfig());
+ AUTO_MAX_SPEED, MOD0_MODOFFSET.getNorm(), new ReplanningConfig());
+ }
+
+ /**
+ * Climber constants
+ */
+ public static final class ClimberConstants {
+ public static final double CLIMBER_KP = 0.1;
+ public static final double CLIMBER_KI = 0.1;
+ public static final double CLIMBER_KD = 0.1;
+ public static final double CLIMBER_MAX_VELOCITY = 0;
+ public static final double CLIMBER_MAX_ACCELERATION = 0;
+ public static final double CLIMBER_KS = 0.1;
+ public static final double CLIMBER_KG = 0.1;
+ public static final double CLIMBER_KV = 0.1;
+ public static final double CLIMBER_POWER = 0.8;
+
+ public static final double CLIMBING_DISTANCE = Units.inchesToMeters(15);
+ public static final double MAX_CLIMBING_DISTANCE = Units.inchesToMeters(21);
+
+ // 2pi * radius
+ public static final double LINEAR_DISTANCE = Units.inchesToMeters(2 * Math.PI * 1);
}
/**
- * Auto Constants
+ * Auto constants
*/
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
@@ -169,4 +363,160 @@ public static final class AutoConstants {
new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
kMaxAngularSpeedRadiansPerSecondSquared);
}
+
+ /**
+ * Class for elevator and wrist constants
+ */
+ public static final class ElevatorWristConstants {
+
+ public static final Rotation2d WRIST_REF_1_ANGLE_MEASURED = Rotation2d.fromRotations(0.454);
+ public static final Rotation2d WRIST_REF_2_ANGLE_MEASURED = Rotation2d.fromRotations(0.585);
+ public static final Rotation2d WRIST_REF_1_ANGLE_ACTUAL = Rotation2d.fromDegrees(0.0);
+ public static final Rotation2d WRIST_REF_2_ANGLE_ACTUAL = Rotation2d.fromDegrees(45.0);
+
+ public static final double WRIST_M;
+ public static final double WRIST_B;
+
+ static {
+ WRIST_M =
+ (WRIST_REF_2_ANGLE_ACTUAL.getRotations() - WRIST_REF_1_ANGLE_ACTUAL.getRotations())
+ / (WRIST_REF_2_ANGLE_MEASURED.getRotations()
+ - WRIST_REF_1_ANGLE_MEASURED.getRotations());
+ // meas_1 * m + b = act_1
+ // b = act_1 - meas_1 * m
+ WRIST_B = WRIST_REF_1_ANGLE_ACTUAL.getRotations()
+ - WRIST_REF_1_ANGLE_MEASURED.getRotations() * WRIST_M;
+ }
+
+ public static final Rotation2d ELEVATOR_REF_1_ANGLE_MEASURED =
+ Rotation2d.fromRotations(0.0);
+ public static final Rotation2d ELEVATOR_REF_2_ANGLE_MEASURED =
+ Rotation2d.fromRotations(-10030.96);
+ public static final double ELEVATOR_REF_1_HEIGHT = 24.0;
+ public static final double ELEVATOR_REF_2_HEIGHT = 44.75;
+
+ public static final double ELEVATOR_M;
+ public static final double ELEVATOR_B;
+
+ static {
+ ELEVATOR_M = (ELEVATOR_REF_2_HEIGHT - ELEVATOR_REF_1_HEIGHT)
+ / (ELEVATOR_REF_2_ANGLE_MEASURED.getRotations()
+ - ELEVATOR_REF_1_ANGLE_MEASURED.getRotations());
+ // meas_1 * m + b = act_1
+ // b = act_1 - meas_1 * m
+ ELEVATOR_B =
+ ELEVATOR_REF_1_HEIGHT - ELEVATOR_REF_1_ANGLE_MEASURED.getRotations() * ELEVATOR_M;
+ }
+
+ /**
+ * PID constants
+ */
+ public static final class PID {
+
+ public static final double ELEVATOR_KP = 10.0;
+ public static final double ELEVATOR_KI = 0;
+ public static final double ELEVATOR_KD = 0;
+ public static final double ELEVATOR_MAX_VELOCITY = 320;
+ public static final double ELEVATOR_MAX_ACCELERATION = 200;
+ public static final double ELEVATOR_KS = 0;
+ public static final double ELEVATOR_KG = 0;
+ public static final double ELEVATOR_KV = 0;
+
+ public static final double WRIST_KP = 110;
+ public static final double WRIST_AMP_KP = 150;
+ public static final double WRIST_LARGE_KP = 60;
+ public static final double WRIST_KI = 0.1;
+ public static final double WRIST_AMP_KI = 0;
+ public static final double WRIST_KD = 0.15;
+ public static final double WRIST_AMP_KD = 0;
+ public static final double WRIST_MAX_VELOCITY = 0.000001;
+ public static final double WRIST_MAX_ACCELERATION = 0.0000000001;
+ public static final double WRIST_KS = 0;
+ public static final double WRIST_KG = 0;
+ public static final double WRIST_KV = 0;
+ public static final double WRIST_LOWPASS = 1;
+ }
+
+ /**
+ * Set points constants for elevator and wrist
+ */
+ public static final class SetPoints {
+
+ public static final double HOME_HEIGHT = 24;
+ public static final Rotation2d INTAKE_ANGLE = Rotation2d.fromDegrees(39);
+ public static final Rotation2d HOME_ANGLE = Rotation2d.fromDegrees(5);
+ // public static final double AMP_HEIGHT = Units.inchesToMeters(34);
+ public static final double AMP_HEIGHT = 43.5;
+ public static final Rotation2d AMP_ANGLE = Rotation2d.fromDegrees(-29.95);
+ // public static final double TRAP_HEIGHT = Units.inchesToMeters(40);
+ public static final double TRAP_HEIGHT = 44;
+ public static final Rotation2d TRAP_ANGLE = Rotation2d.fromDegrees(-10);
+ // public static final double MAX_EXTENSION = Units.inchesToMeters(48);
+ public static final double MAX_EXTENSION = 43.5;
+ public static final double CLIMBING_HEIGHT = 40.0;
+ public static final Rotation2d CLIMBING_ANGLE = Rotation2d.fromDegrees(-40);
+ public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(-44);
+ public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(50);
+
+ public static final double LINEAR_DISTANCE = Units.inchesToMeters(2 * Math.PI * 659);
+
+ public static final Rotation2d PODIUM_ANGLE = Rotation2d.fromDegrees(33.0);
+
+ }
+
+
+ }
+ /**
+ * Pneumatics CAN id constants.
+ */
+
+ public static final class Pneumatics {
+ }
+
+
+
+ /**
+ * Constants of Shooters
+ */
+ public static final class ShooterConstants {
+ public static final double KP = 0.0014;
+ public static final double KI = 0;
+ public static final double KD = 0;
+ public static final double KS = 0;
+ public static final double TOP_KV = 6.18e-4;
+ public static final double BOTTOM_KV = 6.18e-4;
+ public static final double GEAR_RATIO = 3;
+ public static final double HEIGHT_FROM_LOWEST_POS = Units.inchesToMeters(32.0);
+ public static final double HEIGHT_FROM_SPEAKER =
+ FieldConstants.centerSpeaker - HEIGHT_FROM_LOWEST_POS;
+ public static final double SHOOT_SPEAKER_RPM = 11250.0;
+ }
+
+ /**
+ * Constants for intake
+ */
+ public static final class IntakeConstants {
+ public static final double INTAKE_MOTOR_FORWARD = 0;
+ public static final double INTAKE_MOTOR_BACKWARD = -0;
+ public static final double INTAKE_MOTOR_STOP = 0;
+ public static final double INDEX_MOTOR_FORWARD = .2;
+ public static final double INDEX_MOTOR_BACKWARD = -0;
+ public static final double INDEX_MOTOR_STOP = 0;
+ public static final boolean INTAKE_MOTOR_INVERTED = true;
+
+ public static final int INDEXER_BEAM_BRAKE_DIO_PORT = 8;
+ public static final int INTAKE_BEAM_BRAKE_DIO_PORT = 6;
+ }
+
+ /**
+ * LED constants.
+ */
+ public static final class LEDConstants {
+ public static final int PWM_PORT = 9;
+ public static final int LED_COUNT = 60;
+ public static final Color INTAKE_COLOR = Color.kGreen;
+ public static final Color INDEXER_COLOR = Color.kPurple;
+ public static final Color ALERT_COLOR = Color.kWhite;
+ }
+
}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index 04437ec..66a8c84 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -19,6 +19,8 @@ private Main() {}
*
*
* If you change your main robot class, change the parameter type.
+ *
+ * @param args String args
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9f71594..142cf3d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,32 +4,118 @@
package frc.robot;
-import edu.wpi.first.wpilibj.TimedRobot;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.Scanner;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.lib.profiling.EmptyProfiler;
+import frc.lib.profiling.LoggingProfiler;
+import frc.lib.profiling.Profiler;
/**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
- * project.
+ * Runs tasks on Roborio in this file.
*/
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
+ private RobotContainer robotContainer;
+ private Command autoChooser;
- private Command m_autonomousCommand;
+ public static Profiler profiler;
- private RobotContainer m_robotContainer;
-
- // private Ultrasonic ultrasonic = new Ultrasonic();
/**
- * This function is run when the robot is first started up and should be used for any
- * initialization code.
+ * Robot Run type
*/
- @Override
- public void robotInit() {
- // Instantiate our RobotContainer. This will perform all our button bindings, and put our
- // autonomous chooser on the dashboard.
- m_robotContainer = new RobotContainer();
+ public static enum RobotRunType {
+ /** Real Robot. */
+ kReal,
+ /** Simulation runtime. */
+ kSimulation,
+ /** Replay runtime. */
+ kReplay;
+ }
+
+ public static boolean inAuto = false;
+ public RobotRunType robotRunType = RobotRunType.kReal;
+ private Timer gcTimer = new Timer();
+ private Timer profileTimer = new Timer();
+ // We don't want to write empty profiles, so we have a boolean that only becomes true once
+ // teleop or auto has started.
+ private boolean hasDoneSomething = false;
+ private boolean hasStarted = false;
+
+ /** Set up logging, profiling, and robotContainer. */
+ @SuppressWarnings("resource")
+ public Robot() {
+ // Record metadata
+ Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+ switch (BuildConstants.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncommitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
+ }
+
+
+
+ if (isReal()) {
+ Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); // Log to a USB stick
+ Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+ new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+ setUseTiming(true);
+ robotRunType = RobotRunType.kReal;
+ } else {
+ String logPath = findReplayLog();
+ if (logPath == null) {
+ Logger.addDataReceiver(
+ new WPILOGWriter(Filesystem.getOperatingDirectory().getAbsolutePath()));
+ Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+ setUseTiming(true);
+ robotRunType = RobotRunType.kSimulation;
+ } else {
+ Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+ Logger
+ .addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ // Save outputs to a new log
+ setUseTiming(false); // Run as fast as possible
+ robotRunType = RobotRunType.kReplay;
+ }
+ }
+ Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values
+ switch (robotRunType) {
+ case kReal -> profiler =
+ new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0);
+ case kReplay -> profiler = EmptyProfiler.INSTANCE;
+ case kSimulation -> profiler =
+ new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0);
+ default -> {
+ }
+ }
+ // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the
+ // "Understanding Data Flow" page
+
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our autonomous chooser on the dashboard.
+ robotContainer = new RobotContainer(robotRunType);
}
/**
@@ -40,16 +126,40 @@ public void robotInit() {
* This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
+
@Override
public void robotPeriodic() {
+ if (hasStarted) {
+ profiler.endTick();
+ if (profileTimer.advanceIfElapsed(1)) {
+ if (hasDoneSomething) {
+ profiler.save();
+ profiler.reset();
+ }
+ }
+ } else {
+ hasStarted = true;
+ }
+ profiler.startTick();
+ profiler.push("robotPeriodic()");
+ profiler.push("draw_state_to_shuffleboard");
+
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
- // commands, running already-scheduled commands, removing finished or interrupted commands,
- // and running subsystem periodic() methods. This must be called from the robot's periodic
- // block in order for anything in the Command-based framework to work.
+ // commands,
+ // running already-scheduled commands, removing finished or interrupted commands, and
+ // running
+ // subsystem periodic() methods. This must be called from the robot's periodic block in
+ // order for
+ // anything in the Command-based framework to work.
+ profiler.swap("command_scheduler");
CommandScheduler.getInstance().run();
+ profiler.swap("manual-gc");
+ if (gcTimer.advanceIfElapsed(5)) {
+ System.gc();
+ }
+ profiler.pop();
}
- /** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@@ -61,12 +171,18 @@ public void disabledPeriodic() {}
*/
@Override
public void autonomousInit() {
- m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+ hasDoneSomething = true;
+ profiler.push("autonomousInit()");
+ inAuto = true;
+
+ robotContainer.getAutonomousCommand().schedule();
+ autoChooser = robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
- if (m_autonomousCommand != null) {
- m_autonomousCommand.schedule();
+ if (autoChooser != null) {
+ autoChooser.schedule();
}
+ profiler.pop();
}
/** This function is called periodically during autonomous. */
@@ -75,18 +191,19 @@ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
- // This makes sure that the autonomous stops running when
- // teleop starts running. If you want the autonomous to
- // continue until interrupted by another command, remove
- // this line or comment it out.
- if (m_autonomousCommand != null) {
- m_autonomousCommand.cancel();
+ hasDoneSomething = true;
+ profiler.push("teleopInit()");
+ inAuto = false;
+ if (autoChooser != null) {
+ autoChooser.cancel();
}
+ profiler.pop();
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
+ // robotContainer.elevatorWrist.setWristPower(robotContainer.driver.getRightY() * 0.2);
// vision.update();
}
@@ -99,4 +216,41 @@ public void testInit() {
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
+
+ @Override
+ public void simulationPeriodic() {}
+
+
+ private static final String environmentVariable = "AKIT_LOG_PATH";
+ private static final String advantageScopeFileName = "akit-log-path.txt";
+
+ /**
+ * Finds the path to a log file for replay, using the following priorities: 1. The value of the
+ * "AKIT_LOG_PATH" environment variable, if set 2. The file currently open in AdvantageScope, if
+ * available 3. The result of the prompt displayed to the user
+ */
+ public static String findReplayLog() {
+ // Read environment variables
+ String envPath = System.getenv(environmentVariable);
+ if (envPath != null) {
+ System.out.println("Using log from " + environmentVariable
+ + " environment variable - \"" + envPath + "\"");
+ return envPath;
+ }
+
+ // Read file from AdvantageScope
+ Path advantageScopeTempPath =
+ Paths.get(System.getProperty("java.io.tmpdir"), advantageScopeFileName);
+ String advantageScopeLogPath = null;
+ try (Scanner fileScanner = new Scanner(advantageScopeTempPath)) {
+ advantageScopeLogPath = fileScanner.nextLine();
+ } catch (IOException e) {
+ System.out.println("Something went wrong");
+ }
+ if (advantageScopeLogPath != null) {
+ System.out.println("Using log from AdvantageScope - \"" + advantageScopeLogPath + "\"");
+ return advantageScopeLogPath;
+ }
+ return null;
+ }
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 50617d4..75fcf89 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,23 +1,18 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
package frc.robot;
-import java.util.List;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.path.PathPlannerPath;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Robot.RobotRunType;
import frc.robot.commands.TeleopSwerve;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
+import frc.robot.subsystems.swerve.SwerveIO;
+import frc.robot.subsystems.swerve.SwerveReal;
+import frc.robot.subsystems.swerve.SwerveSim;
+
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -26,60 +21,60 @@
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
- /* Controllers */
- private final CommandXboxController driver = new CommandXboxController(0);
- private final SendableChooser autoChooser = new SendableChooser<>();
+ /* Shuffleboard */
+ private final SendableChooser autoChooser = new SendableChooser<>();
+
+ /* Controllers */
+ public final CommandXboxController driver = new CommandXboxController(Constants.DRIVER_ID);
/* Subsystems */
- private final Swerve s_Swerve = new Swerve();
+ private Swerve s_Swerve;
+
+ /**
+ */
+ public RobotContainer(RobotRunType runtimeType) {
+ switch (runtimeType) {
+ case kReal:
+ s_Swerve = new Swerve(new SwerveReal());
+ break;
+ case kSimulation:
+ s_Swerve = new Swerve(new SwerveSim());
+ break;
+ default:
+ s_Swerve = new Swerve(new SwerveIO() {});
+ }
+
+ // autoChooser.addOption("P32", new P32(s_Swerve, elevatorWrist, intake, shooter));
+ // autoChooser.addOption("P675", new P675(s_Swerve, elevatorWrist, intake, shooter));
+ // autoChooser.addOption("P3675", new P3675(s_Swerve, elevatorWrist, intake, shooter));
- /** The container for the robot. Contains subsystems, OI devices, and commands. */
- public RobotContainer() {
s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop));
- // autoChooser.addOption(resnickAuto, new ResnickAuto(s_Swerve));
- SmartDashboard.putData("Choose Auto: ", autoChooser);
// Configure the button bindings
configureButtonBindings();
}
- public void periodic() {
-
- }
-
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses
- * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * Use this method to vol your button->command mappings. Buttons can be created by instantiating
+ * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
+ * {@link XboxController}), and then passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
+
+
/* Driver Buttons */
- driver.y().whileTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
+ driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
}
/**
- * Use this to pass the autonomous command to the main {@link Robot} class.
+ * Gets the user's selected autonomous command.
*
- * @return the command to run in autonomous
+ * @return Returns autonomous command selected.
*/
-
public Command getAutonomousCommand() {
- String stuff = autoChooser.getSelected();
- Command autocommand;
- switch (stuff) {
- case "Test Auto":
- List paths = PathPlannerAuto.getPathGroupFromAutoFile("New Auto");
- Pose2d initialState = paths.get(0).getPreviewStartingHolonomicPose();
- s_Swerve.resetOdometry(initialState);
- autocommand = new InstantCommand(() -> s_Swerve.resetOdometry(initialState))
- .andThen(new PathPlannerAuto("New Auto"));
-
- break;
- default:
- autocommand = new WaitCommand(1.0);
- }
+ Command autocommand = autoChooser.getSelected();
return autocommand;
}
}
diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java
new file mode 100644
index 0000000..59783ee
--- /dev/null
+++ b/src/main/java/frc/robot/commands/CommandFactory.java
@@ -0,0 +1,8 @@
+package frc.robot.commands;
+
+/**
+ * File to create commands using factories
+ */
+public class CommandFactory {
+
+}
diff --git a/src/main/java/frc/robot/commands/MoveToPos.java b/src/main/java/frc/robot/commands/MoveToPos.java
new file mode 100644
index 0000000..4fc5d92
--- /dev/null
+++ b/src/main/java/frc/robot/commands/MoveToPos.java
@@ -0,0 +1,122 @@
+package frc.robot.commands;
+
+import java.util.function.Supplier;
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.lib.util.FieldConstants;
+import frc.robot.Constants;
+import frc.robot.subsystems.swerve.Swerve;
+
+/**
+ * Move to Position
+ */
+public class MoveToPos extends Command {
+
+ public Swerve swerve;
+ public Pose2d pose2d;
+ public Supplier pose2dSupplier;
+ private boolean flipForRed = true;
+
+ HolonomicDriveController holonomicDriveController = new HolonomicDriveController(
+ new PIDController(Constants.SwerveTransformPID.PID_XKP,
+ Constants.SwerveTransformPID.PID_XKI, Constants.SwerveTransformPID.PID_XKD),
+ new PIDController(Constants.SwerveTransformPID.PID_YKP,
+ Constants.SwerveTransformPID.PID_YKI, Constants.SwerveTransformPID.PID_YKD),
+ new ProfiledPIDController(Constants.SwerveTransformPID.PID_TKP,
+ Constants.SwerveTransformPID.PID_TKI, Constants.SwerveTransformPID.PID_TKD,
+ new TrapezoidProfile.Constraints(Constants.SwerveTransformPID.MAX_ANGULAR_VELOCITY,
+ Constants.SwerveTransformPID.MAX_ANGULAR_ACCELERATION)));
+
+ /**
+ * Move to Position
+ *
+ * @param swerve Swerve Drive Subsystem
+ * @param pose2dSupplier Supplier of Pose2d
+ * @param flipForRed Flip the Pose2d relative to the Red Alliance
+ */
+ public MoveToPos(Swerve swerve, Supplier pose2dSupplier, boolean flipForRed,
+ double tol) {
+ this(swerve, pose2dSupplier, flipForRed, tol, Constants.SwerveTransformPID.PID_TKP);
+ }
+
+ /**
+ * Move to Position
+ *
+ * @param swerve Swerve Drive Subsystem
+ * @param pose2dSupplier Supplier of Pose2d
+ * @param flipForRed Flip the Pose2d relative to the Red Alliance
+ */
+ public MoveToPos(Swerve swerve, Supplier pose2dSupplier, boolean flipForRed, double tol,
+ double turnP) {
+ this.swerve = swerve;
+ this.pose2dSupplier = pose2dSupplier;
+ this.flipForRed = flipForRed;
+ this.addRequirements(swerve);
+ holonomicDriveController.getThetaController().setP(turnP);
+ holonomicDriveController.setTolerance(new Pose2d(tol, tol, Rotation2d.fromDegrees(1)));
+ }
+
+ /**
+ * Move to Position
+ *
+ * @param swerve Swerve Drive Subsystem
+ * @param pose2dSupplier Supplier of Pose2d
+ * @param flipForRed Flip the Pose2d relative to the Red Alliance
+ */
+ public MoveToPos(Swerve swerve, Supplier pose2dSupplier, boolean flipForRed) {
+ this(swerve, pose2dSupplier, flipForRed, 0.05);
+ }
+
+
+ /**
+ * Move to Position
+ *
+ * @param swerve Swerve Drive Subsystem
+ * @param pose2dSupplier Supplier of Pose2d
+ */
+ public MoveToPos(Swerve swerve, Supplier pose2dSupplier) {
+ this(swerve, pose2dSupplier, true);
+ }
+
+ /**
+ * Move to Position
+ *
+ * @param swerve Swerve Drive Subsystem
+ */
+ public MoveToPos(Swerve swerve) {
+ this(swerve, () -> new Pose2d());
+ }
+
+ @Override
+ public void initialize() {
+ pose2d = pose2dSupplier.get();
+ if (flipForRed && DriverStation.getAlliance().get() == Alliance.Red) {
+ pose2d = FieldConstants.allianceFlip(pose2d);
+ }
+ }
+
+ @Override
+ public void execute() {
+ ChassisSpeeds ctrlEffort =
+ holonomicDriveController.calculate(swerve.getPose(), pose2d, 0, pose2d.getRotation());
+ swerve.setModuleStates(ctrlEffort);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ swerve.setMotorsZero();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return holonomicDriveController.atReference();
+ }
+}
diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java
index 4310916..dad2a80 100644
--- a/src/main/java/frc/robot/commands/TeleopSwerve.java
+++ b/src/main/java/frc/robot/commands/TeleopSwerve.java
@@ -4,7 +4,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.Robot;
+import frc.robot.subsystems.swerve.Swerve;
/**
* Creates an command for driving the swerve drive during tele-op
@@ -15,9 +16,10 @@ public class TeleopSwerve extends Command {
private boolean openLoop;
private Swerve swerveDrive;
private CommandXboxController controller;
+ private double speedMultiplier = 1;
/**
- * Creates an command for driving the swerve drive during tele-op
+ * Creates a command for driving the swerve drive during tele-op
*
* @param swerveDrive The instance of the swerve drive subsystem
* @param fieldRelative Whether the movement is relative to the field or absolute
@@ -32,21 +34,41 @@ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolea
this.controller = controller;
}
+ /**
+ * Creates a command for driving the swerve drive during tele-op
+ *
+ * @param swerveDrive The instance of the swerve drive subsystem
+ * @param fieldRelative Whether the movement is relative to the field or absolute
+ * @param openLoop Open or closed loop system
+ * @param speedMultiplier Speed multiplier to increase or decrease speed
+ */
+ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolean fieldRelative,
+ boolean openLoop, double speedMultiplier) {
+ this(swerveDrive, controller, fieldRelative, openLoop);
+ this.speedMultiplier = speedMultiplier;
+ }
+
@Override
public void execute() {
- double yaxis = -controller.getLeftY();
- double xaxis = -controller.getLeftX();
- double raxis = -controller.getRightX();
+ Robot.profiler.push("teleop_swerve");
+ double yaxis = -controller.getLeftY() * speedMultiplier;
+ double xaxis = -controller.getLeftX() * speedMultiplier;
+ double raxis = -controller.getRightX() * speedMultiplier;
/* Deadbands */
- yaxis = (Math.abs(yaxis) < Constants.stickDeadband) ? 0 : yaxis;
- xaxis = (Math.abs(xaxis) < Constants.stickDeadband) ? 0 : xaxis;
- raxis = (Math.abs(raxis) < Constants.stickDeadband) ? 0 : raxis;
+ yaxis = (Math.abs(yaxis) < Constants.STICK_DEADBAND) ? 0
+ : (yaxis - Constants.STICK_DEADBAND) / (1.0 - Constants.STICK_DEADBAND);
+ xaxis = (Math.abs(xaxis) < Constants.STICK_DEADBAND) ? 0
+ : (xaxis - Constants.STICK_DEADBAND) / (1.0 - Constants.STICK_DEADBAND);
+ xaxis *= xaxis * Math.signum(xaxis);
+ yaxis *= yaxis * Math.signum(yaxis);
+ raxis = (Math.abs(raxis) < Constants.STICK_DEADBAND) ? 0 : raxis;
// System.out.println(swerveDrive.getStringYaw());
Translation2d translation =
new Translation2d(yaxis, xaxis).times(Constants.Swerve.maxSpeed);
double rotation = raxis * Constants.Swerve.maxAngularVelocity;
swerveDrive.drive(translation, rotation, fieldRelative, openLoop);
+ Robot.profiler.pop();
}
}
diff --git a/src/main/java/frc/robot/commands/TurnToAngle.java b/src/main/java/frc/robot/commands/TurnToAngle.java
new file mode 100644
index 0000000..db2e469
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TurnToAngle.java
@@ -0,0 +1,70 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.swerve.Swerve;
+
+/**
+ * This command will turn the robot to a specified angle.
+ */
+public class TurnToAngle extends Command {
+
+ private Swerve swerve;
+ private HolonomicDriveController holonomicDriveController =
+ new HolonomicDriveController(new PIDController(0, 0, 0), new PIDController(0, 0, 0),
+ new ProfiledPIDController(Constants.SwerveTransformPID.PID_TKP / 2,
+ Constants.SwerveTransformPID.PID_TKI, Constants.SwerveTransformPID.PID_TKD,
+ new TrapezoidProfile.Constraints(Constants.SwerveTransformPID.MAX_ANGULAR_VELOCITY,
+ Constants.SwerveTransformPID.MAX_ANGULAR_ACCELERATION)));
+ private Rotation2d targetRotation2d;
+ private Pose2d position;
+ private Rotation2d offset = new Rotation2d();
+
+ /**
+ * Turns robot to specified angle. Uses absolute rotation on field.
+ *
+ * @param swerve Swerve subsystem
+ * @param angle Requested angle to turn to
+ */
+ public TurnToAngle(Swerve swerve, Rotation2d angle) {
+ addRequirements(swerve);
+ this.swerve = swerve;
+ this.offset = angle;
+ holonomicDriveController.setTolerance(new Pose2d(1, 1, Rotation2d.fromDegrees(1)));
+ }
+
+ @Override
+ public void initialize() {
+ this.targetRotation2d = offset.plus(Rotation2d.fromDegrees(180));
+ if (DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
+ this.targetRotation2d = Rotation2d.fromDegrees(180).minus(offset);
+ }
+ }
+
+ @Override
+ public void execute() {
+ position = new Pose2d(0, 0, swerve.getFieldRelativeHeading());
+ ChassisSpeeds chassisSpeeds = this.holonomicDriveController.calculate(position,
+ new Pose2d(0, 0, targetRotation2d), 0, targetRotation2d);
+ swerve.setModuleStates(chassisSpeeds);
+ }
+
+ @Override
+ public void end(boolean interrupt) {
+ swerve.setMotorsZero();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return holonomicDriveController.atReference();
+ }
+}
diff --git a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java
index 1082587..8305e1c 100644
--- a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java
+++ b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java
@@ -3,8 +3,7 @@
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
/**
* Zeros motors then executes normal wait command.
@@ -19,15 +18,15 @@ public class ZeroMotorsWaitCommand extends Command {
*
* @param seconds how long the wait command should run
*/
- public ZeroMotorsWaitCommand(double seconds) {
+ public ZeroMotorsWaitCommand(double seconds, Swerve swerve) {
this.m_duration = seconds;
SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
- this.s_Swerve = new Swerve();
+ this.s_Swerve = swerve;
}
@Override
public void initialize() {
- s_Swerve.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative);
+ s_Swerve.setMotorsZero();
m_timer.reset();
m_timer.start();
}
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
similarity index 62%
rename from src/main/java/frc/robot/subsystems/Swerve.java
rename to src/main/java/frc/robot/subsystems/swerve/Swerve.java
index bb13942..10c569d 100644
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -1,56 +1,68 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems.swerve;
import java.util.Optional;
-import com.kauailabs.navx.frc.AHRS;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
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.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.util.FieldConstants;
import frc.lib.util.swerve.SwerveModule;
import frc.robot.Constants;
+import frc.robot.Robot;
/**
* Swerve Subsystem
*/
public class Swerve extends SubsystemBase {
- public SwerveDriveOdometry swerveOdometry;
+ public SwerveDrivePoseEstimator swerveOdometry;
public SwerveModule[] swerveMods;
- public AHRS gyro = new AHRS(Constants.Swerve.navXID);
- private double fieldOffset = gyro.getYaw();
+ private final Field2d field = new Field2d();
+ private double fieldOffset;
+ private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged();
+ private SwerveIO swerveIO;
+
/**
* Swerve Subsystem
*/
- public Swerve() {
- swerveMods = new SwerveModule[] {
- new SwerveModule(0, Constants.Swerve.Mod0.driveMotorID,
- Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID,
- Constants.Swerve.Mod0.angleOffset),
- new SwerveModule(1, Constants.Swerve.Mod1.driveMotorID,
- Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID,
- Constants.Swerve.Mod1.angleOffset),
- new SwerveModule(2, Constants.Swerve.Mod2.driveMotorID,
- Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID,
- Constants.Swerve.Mod2.angleOffset),
- new SwerveModule(3, Constants.Swerve.Mod3.driveMotorID,
- Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID,
- Constants.Swerve.Mod3.angleOffset)};
-
- swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(),
- getModulePositions());
+ public Swerve(SwerveIO swerveIO) {
+ this.swerveIO = swerveIO;
+ swerveMods = swerveIO.createModules();
+ fieldOffset = getGyroYaw().getDegrees();
+
+ swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics,
+ getGyroYaw(), getModulePositions(), new Pose2d());
+
+ swerveIO.updateInputs(inputs);
AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, this::getChassisSpeeds,
this::setModuleStates, Constants.Swerve.pathFollowerConfig, () -> shouldFlipPath(),
this);
+
+ // Logging callback for target robot pose
+ PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
+ // Do whatever you want with the pose here
+ field.getObject("target pose").setPose(pose);
+ });
+
+ // Logging callback for the active path, this is sent as a list of poses
+ PathPlannerLogging.setLogActivePathCallback((poses) -> {
+ // Do whatever you want with the poses here
+ field.getObject("path").setPoses(poses);
+ });
}
/**
@@ -63,12 +75,14 @@ public Swerve() {
*/
public void drive(Translation2d translation, double rotation, boolean fieldRelative,
boolean isOpenLoop) {
+ Robot.profiler.push("swerve.drive()");
ChassisSpeeds chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(),
rotation, getFieldRelativeHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
setModuleStates(chassisSpeeds);
+ Robot.profiler.pop();
}
/**
@@ -78,7 +92,7 @@ rotation, getFieldRelativeHeading())
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed);
-
+ Logger.recordOutput("/Swerve/DesiredStates", desiredStates);
for (SwerveModule mod : swerveMods) {
mod.setDesiredState(desiredStates[mod.moduleNumber], false);
}
@@ -136,8 +150,9 @@ public SwerveModulePosition[] getModulePositions() {
*
* @return Pose2d on the field
*/
+ @AutoLogOutput(key = "Odometry/Robot")
public Pose2d getPose() {
- return swerveOdometry.getPoseMeters();
+ return swerveOdometry.getEstimatedPosition();
}
/**
@@ -147,6 +162,7 @@ public Pose2d getPose() {
*/
public void resetOdometry(Pose2d pose) {
swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), pose);
+ this.swerveIO.setPose(pose);
}
/**
@@ -164,7 +180,7 @@ public Rotation2d getHeading() {
* @return Current rotation/yaw of gyro as {@link Rotation2d}
*/
public Rotation2d getGyroYaw() {
- float yaw = gyro.getYaw();
+ float yaw = inputs.yaw;
return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw)
: Rotation2d.fromDegrees(yaw);
}
@@ -183,7 +199,7 @@ public Rotation2d getFieldRelativeHeading() {
*/
public void resetFieldRelativeOffset() {
// gyro.zeroYaw();
- fieldOffset = getGyroYaw().getDegrees();
+ fieldOffset = getGyroYaw().getDegrees() + 180;
}
/**
@@ -197,25 +213,39 @@ public void resetModulesToAbsolute() {
@Override
public void periodic() {
+ Robot.profiler.push("swerve_periodic");
+ Robot.profiler.push("update_inputs");
+ swerveIO.updateInputs(inputs);
+ Robot.profiler.swap("update_swerve_mods");
+ for (var mod : swerveMods) {
+ mod.periodic();
+ }
+ Robot.profiler.swap("update_swerve_odometry");
swerveOdometry.update(getGyroYaw(), getModulePositions());
+ Robot.profiler.swap("process_inputs");
+ Logger.processInputs("Swerve", inputs);
+ Robot.profiler.swap("update_shuffleboard");
+ Robot.profiler.push("field");
+ field.setRobotPose(getPose());
+ Robot.profiler.swap("apriltag");
- for (SwerveModule mod : swerveMods) {
- SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder",
- mod.getCANcoder().getDegrees());
- SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle",
- mod.getPosition().angle.getDegrees());
- SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity",
- mod.getState().speedMetersPerSecond);
- }
+ Robot.profiler.swap("dist-to-speaker");
+ SmartDashboard.putNumber("Distance to Speaker",
+ FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening)
+ .getTranslation().minus(getPose().getTranslation()).getNorm());
+ Robot.profiler.swap("simple");
+ SmartDashboard.putNumber("Gyro Yaw", getGyroYaw().getDegrees());
+ Logger.recordOutput("/Swerve/ActualStates", getModuleStates());
+ Robot.profiler.pop();
+ Robot.profiler.pop();
+ Robot.profiler.swap("viz");
+ Robot.profiler.pop();
}
/**
* Sets motors to 0 or inactive.
- *
- * @param isOpenLoop Open or closed loop system
- * @param fieldRelative Whether the movement is relative to the field or absolute
*/
- public void setMotorsZero(boolean isOpenLoop, boolean fieldRelative) {
+ public void setMotorsZero() {
System.out.println("Setting Zero!!!!!!");
setModuleStates(new ChassisSpeeds(0, 0, 0));
}
@@ -229,7 +259,18 @@ public void wheelsIn() {
swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), false);
swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)),
false);
- this.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative);
+ this.setMotorsZero();
+ }
+
+ /**
+ * Gets a list containing all 4 swerve module positions
+ */
+ public SwerveModulePosition[] getSwerveModulePositions() {
+ SwerveModulePosition[] positions = new SwerveModulePosition[4];
+ for (SwerveModule mod : swerveMods) {
+ positions[mod.moduleNumber] = mod.getPosition();
+ }
+ return positions;
}
/**
@@ -244,4 +285,17 @@ public static boolean shouldFlipPath() {
}
return false;
}
+
+ /**
+ * Returns the distance between the speaker and Swerve
+ *
+ * @return the difference between the pose of speaker and swerve
+ */
+ public double distanceFromSpeaker() {
+ double distance =
+ Math.hypot(FieldConstants.Speaker.centerSpeakerOpening.getY() - getPose().getY(),
+ FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening).getX()
+ - getPose().getX());
+ return distance;
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
new file mode 100644
index 0000000..b0ae91b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.swerve;
+
+import java.util.Optional;
+import org.littletonrobotics.junction.AutoLog;
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.lib.util.swerve.SwerveModule;
+
+/** IO Class for Swerve */
+public interface SwerveIO {
+
+ /** Inputs Class for Swerve */
+
+ @AutoLog
+ public static class SwerveInputs {
+ public float yaw;
+ public float roll;
+ public float pitch;
+ }
+
+ public default void updateInputs(SwerveInputs inputs) {}
+
+ // /** Instantiating SwerveModules */
+ // public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID,
+ // int angleMotorID, int cancoderID, Rotation2d angleOffset) {
+ // return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset,
+ // new SwerveModuleIO() {});
+ // }
+
+ public default SwerveModule[] createModules() {
+ return new SwerveModule[] {};
+ }
+
+ public default Optional getInitialPose() {
+ return Optional.empty();
+ }
+
+ public default void setPose(Pose2d pose) {}
+
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
new file mode 100644
index 0000000..410e8ca
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
@@ -0,0 +1,48 @@
+package frc.robot.subsystems.swerve;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.lib.util.swerve.SwerveModule;
+import frc.lib.util.swerve.SwerveModuleReal;
+import frc.robot.Constants;
+
+/** Real Class for Swerve */
+public class SwerveReal implements SwerveIO {
+
+ private AHRS gyro = new AHRS(Constants.Swerve.navXID);
+
+ /** Real Swerve Initializer */
+ public SwerveReal() {}
+
+ @Override
+ public void updateInputs(SwerveInputs inputs) {
+ inputs.yaw = gyro.getYaw();
+ inputs.pitch = gyro.getPitch();
+ inputs.roll = gyro.getRoll();
+
+ }
+
+ public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
+ int cancoderID, Rotation2d angleOffset) {
+ return new SwerveModule(moduleNumber, angleOffset,
+ new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID));
+ }
+
+ @Override
+ public SwerveModule[] createModules() {
+ return new SwerveModule[] {
+ createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID,
+ Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID,
+ Constants.Swerve.Mod0.angleOffset),
+ createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID,
+ Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID,
+ Constants.Swerve.Mod1.angleOffset),
+ createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID,
+ Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID,
+ Constants.Swerve.Mod2.angleOffset),
+ createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID,
+ Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID,
+ Constants.Swerve.Mod3.angleOffset)};
+ }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java
new file mode 100644
index 0000000..1218970
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java
@@ -0,0 +1,94 @@
+package frc.robot.subsystems.swerve;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import frc.lib.util.swerve.SwerveModule;
+import frc.lib.util.swerve.SwerveModuleSim;
+import frc.robot.Constants;
+
+/**
+ * Real Class for Swerve
+ */
+public class SwerveSim implements SwerveIO {
+
+ private SwerveModule[] swerveMods;
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+ private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
+ private Rotation2d rawGyroRotation = new Rotation2d();
+ private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ private SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+ private SwerveModulePosition[] lastModulePositions = // For delta tracking
+ new SwerveModulePosition[] {new SwerveModulePosition(), new SwerveModulePosition(),
+ new SwerveModulePosition(), new SwerveModulePosition()};
+
+ private Pose2d currentPose = new Pose2d();
+
+ /**
+ * Real Swerve Initializer
+ */
+ public SwerveSim() {}
+
+ @Override
+ public void updateInputs(SwerveInputs inputs) {
+ m_gyroSim.setAngle(getHeading().getDegrees());
+ if (Constants.Swerve.invertGyro) {
+ inputs.yaw = (float) -m_gyroSim.getAngle();
+ } else {
+ inputs.yaw = (float) m_gyroSim.getAngle();
+ }
+ // inputs.pitch = 0;
+ // inputs.roll = 0;
+
+ }
+
+ /**
+ * Create Swerve Module
+ *
+ * @param moduleNumber Module Number
+ * @return Swerve Module
+ */
+ public SwerveModule createSwerveModule(int moduleNumber) {
+ SwerveModuleSim modIO = new SwerveModuleSim();
+ modIO.setModNumber(moduleNumber);
+ SwerveModule mod =
+ new SwerveModule(moduleNumber, Rotation2d.fromRotations(Math.random()), modIO);
+ return mod;
+ }
+
+ @Override
+ public SwerveModule[] createModules() {
+ swerveMods = new SwerveModule[] {createSwerveModule(0), createSwerveModule(1),
+ createSwerveModule(2), createSwerveModule(3)};
+ return swerveMods;
+ }
+
+ /**
+ * Get Heading
+ *
+ * @return Get robot heading
+ */
+ private Rotation2d getHeading() {
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = swerveMods[moduleIndex].getPosition();
+ moduleDeltas[moduleIndex] = new SwerveModulePosition(
+ modulePositions[moduleIndex].distanceMeters
+ - lastModulePositions[moduleIndex].distanceMeters,
+ modulePositions[moduleIndex].angle);
+ lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+ }
+ Twist2d twist = Constants.Swerve.swerveKinematics.toTwist2d(moduleDeltas);
+ this.currentPose = this.currentPose.exp(twist);
+ rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+ return rawGyroRotation;
+ }
+
+ @Override
+ public void setPose(Pose2d pose) {
+ this.currentPose = pose;
+ }
+
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644
index 0000000..63dacbb
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,42 @@
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "3.2.1",
+ "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+ "frcYear": "2024",
+ "mavenUrls": [],
+ "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit.junction",
+ "artifactId": "wpilib-shim",
+ "version": "3.2.1"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.junction",
+ "artifactId": "junction-core",
+ "version": "3.2.1"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-api",
+ "version": "3.2.1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-wpilibio",
+ "version": "3.2.1",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxathena",
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 0bf11fb..6dc648d 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib.json",
"name": "PathplannerLib",
- "version": "2024.1.4",
+ "version": "2024.2.8",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2024",
"mavenUrls": [
@@ -12,7 +12,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
- "version": "2024.1.4"
+ "version": "2024.2.8"
}
],
"jniDependencies": [],
@@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
- "version": "2024.1.4",
+ "version": "2024.2.8",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
index 69a4079..0322385 100644
--- a/vendordeps/Phoenix6.json
+++ b/vendordeps/Phoenix6.json
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix6.json",
"name": "CTRE-Phoenix (v6)",
- "version": "24.1.0",
+ "version": "24.3.0",
"frcYear": 2024,
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [
@@ -19,14 +19,14 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
- "version": "24.1.0"
+ "version": "24.3.0"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -39,7 +39,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -52,7 +52,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -65,7 +65,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonFX",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -78,7 +78,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -91,7 +91,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -104,7 +104,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -117,7 +117,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -130,7 +130,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -143,7 +143,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "24.1.0",
+ "version": "24.3.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -158,7 +158,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -173,7 +173,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -188,7 +188,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -203,7 +203,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -218,7 +218,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -233,7 +233,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonFX",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -248,7 +248,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -263,7 +263,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -278,7 +278,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimCANCoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -293,7 +293,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -308,7 +308,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -323,7 +323,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "24.1.0",
+ "version": "24.3.0",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
deleted file mode 100644
index 0f3520e..0000000
--- a/vendordeps/REVLib.json
+++ /dev/null
@@ -1,74 +0,0 @@
-{
- "fileName": "REVLib.json",
- "name": "REVLib",
- "version": "2024.2.0",
- "frcYear": "2024",
- "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
- "mavenUrls": [
- "https://maven.revrobotics.com/"
- ],
- "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
- "javaDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-java",
- "version": "2024.2.0"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-driver",
- "version": "2024.2.0",
- "skipInvalidPlatforms": true,
- "isJar": false,
- "validPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-cpp",
- "version": "2024.2.0",
- "libName": "REVLib",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- },
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-driver",
- "version": "2024.2.0",
- "libName": "REVLibDriver",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ]
-}
\ No newline at end of file
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
deleted file mode 100644
index 282cfa8..0000000
--- a/vendordeps/photonlib.json
+++ /dev/null
@@ -1,57 +0,0 @@
-{
- "fileName": "photonlib.json",
- "name": "photonlib",
- "version": "v2024.2.0",
- "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
- "frcYear": "2024",
- "mavenUrls": [
- "https://maven.photonvision.org/repository/internal",
- "https://maven.photonvision.org/repository/snapshots"
- ],
- "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "photonlib-cpp",
- "version": "v2024.2.0",
- "libName": "photonlib",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- },
- {
- "groupId": "org.photonvision",
- "artifactId": "photontargeting-cpp",
- "version": "v2024.2.0",
- "libName": "photontargeting",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- }
- ],
- "javaDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "photonlib-java",
- "version": "v2024.2.0"
- },
- {
- "groupId": "org.photonvision",
- "artifactId": "photontargeting-java",
- "version": "v2024.2.0"
- }
- ]
-}
\ No newline at end of file