diff --git a/.vscode/settings.json b/.vscode/settings.json index 7a8a458..9d1f165 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -49,7 +49,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "svelte.enable-ts-plugin": true, "files.exclude": { "**/.git": true, "**/.svn": true, @@ -63,26 +62,29 @@ "**/.factorypath": true, "**/*~": true }, + "wpilib.autoStartRioLog": false, "cSpell.words": [ + "ADIS", "Accl", "Accum", - "ADIS", "Autoset", "Backports", "Bezier", "Botpose", "Brushless", + "CANcoder", + "CANdi", + "CANrange", + "CTRE", "Canandcolor", "Canandgyro", "Canandmag", "Canbus", - "CANcoder", - "CANdi", "Checkstyle", "Choreo", "ChoreoLib", "Cnfg", - "CTRE", + "DTheta", "Deadband", "Deadbands", "Deadzone", @@ -99,39 +101,39 @@ "Discretize", "Discretizing", "Doppel", - "DTheta", "Dunkin", "EEPROM", "Expdelta", "Falsi", "Feedforward", "Fullscreen", + "GSON", "Gradlew", "Grav", - "GRRDashboard", - "GSON", "Holonomic", "Intaking", "Integ", "Interpolatable", "Itor", - "JoystickProfiles", "Kalman", "Keepalive", "Lerp", - "Motorcontrol", "Msgpack", "Mult", "Multiturn", - "NetworkTables", "NTURI", + "NetworkTables", "Odometry", "Overcurrent", + "PAPF", "PIDF", + "PhotonVision", "Powerup", "Protobuf", "Pubuid", "Quasistatic", + "REVPH", + "REVRobotics", "Ratelimit", "Ratelimited", "Ratelimiter", @@ -140,9 +142,8 @@ "ReduxLib", "Reefscape", "Regula", + "Repulsor", "RevLib", - "REVPH", - "REVRobotics", "RoboRIO", "Sendables", "Setpoint", @@ -154,7 +155,6 @@ "Subuid", "TalonFX", "TalonSRX", - "Tauri", "Teleop", "Timesync", "Topicsonly", @@ -166,14 +166,11 @@ "Unsubscriber", "Unsubscribers", "Unused", - "URCL", "Vbat", "Vmax", - "WPIBlue", "WPILib", "WPILibJ", "WPIMath", - "WPIRed", "Writables" ] } diff --git a/README.md b/README.md index 32fc374..8e1c2a3 100644 --- a/README.md +++ b/README.md @@ -39,19 +39,19 @@ Node.js is required to support linting via [Spotless](https://github.com/diffplu ```diff diff --git a/build.gradle b/build.gradle -index 155f017..7670ad8 100644 +index 834d562..a2837c8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,6 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.3.2" -- id "com.diffplug.spotless" version "7.0.2" +- id "com.diffplug.spotless" version "7.0.3" } java { -@@ -83,30 +82,6 @@ dependencies { - implementation 'com.google.code.gson:gson:2.11.0' +@@ -91,30 +90,6 @@ dependencies { + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } -// Code formatting via spotless @@ -81,7 +81,7 @@ index 155f017..7670ad8 100644 test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -@@ -134,5 +109,4 @@ wpi.java.configureTestTasks(test) +@@ -142,5 +117,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' diff --git a/build.gradle b/build.gradle index a98f347..834d562 100644 --- a/build.gradle +++ b/build.gradle @@ -28,7 +28,17 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - // Uncomment to enable VisualVM connections + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // Only enable these arguments when using a RIO 2 + jvmArgs.add("-Xmx100M") + jvmArgs.add("-Xms100M") + jvmArgs.add("-XX:+AlwaysPreTouch") + + // Enable VisualVM connection // jvmArgs.add("-Dcom.sun.management.jmxremote=true") // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") diff --git a/src/main/java/org/team340/lib/logging/EpilogueProxy.java b/src/main/java/org/team340/lib/logging/EpilogueProxy.java new file mode 100644 index 0000000..b73a114 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/EpilogueProxy.java @@ -0,0 +1,88 @@ +package org.team340.lib.logging; + +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.EpilogueConfiguration; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.wpilibj.DriverStation; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.util.function.Consumer; +import org.team340.lib.util.Mutable; + +// +// Do not fret if your IDE reports errors in this file! +// +// Epilogue is a generated class and may not have been seen yet by your +// editor, but your code will still build and function as normal. +// + +/** + * Utility for interfacing with the generated {@code Epilogue} + * class. Keeps the occasional false negatives from the language + * server in one file. + */ +public final class EpilogueProxy { + + private EpilogueProxy() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Configures Epilogue. + * @param configurator A consumer that mutates the provided {@link EpilogueConfiguration}. + */ + public static void configure(Consumer configurator) { + Epilogue.configure(configurator); + } + + /** + * Gets the current epilogue configuration. + */ + public static EpilogueConfiguration getConfig() { + return Epilogue.getConfig(); + } + + /** + * Gets the configured root backend. + */ + public static EpilogueBackend getRootBackend() { + return getConfig().backend.getNested(getConfig().root); + } + + /** + * Generates a logging function for the provided object via reflection. This + * method should not be invoked periodically due to performance implications. + * @param obj The object to generate the logger for. + */ + @SuppressWarnings("unchecked") + public static Consumer getLogger(Object obj) { + Field[] fields = Epilogue.class.getDeclaredFields(); + Mutable> objLogger = new Mutable<>(null); + + for (Field field : fields) { + if (Modifier.isStatic(field.getModifiers())) { + try { + Object value = field.get(obj); + if (value instanceof ClassSpecificLogger logger) { + Class type = logger.getLoggedType(); + if (type.equals(obj.getClass())) { + objLogger.value = logger; + break; + } + } + } catch (Exception e) {} + } + } + + if (objLogger.value != null) { + return backend -> objLogger.value.tryUpdate(backend, obj, Epilogue.getConfig().errorHandler); + } else { + DriverStation.reportWarning( + "[EpilogueProxy] Unable to find logger for class \"" + obj.getClass().getSimpleName() + "\"", + true + ); + return backend -> {}; + } + } +} diff --git a/src/main/java/org/team340/lib/logging/LoggedRobot.java b/src/main/java/org/team340/lib/logging/LoggedRobot.java new file mode 100644 index 0000000..549a2a3 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/LoggedRobot.java @@ -0,0 +1,124 @@ +package org.team340.lib.logging; + +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.IterativeRobotBase; +import edu.wpi.first.wpilibj.RobotController; +import java.util.function.Consumer; +import org.team340.lib.util.DisableWatchdog; +import org.team340.lib.util.Tunable; + +/** + * LoggedRobot implements the IterativeRobotBase robot program framework, + * and is intended to be subclassed by a user creating a robot program. + * + *

The LoggedRobot class configures the program for logging data to NetworkTables + * via Epilogue, which is captured by the {@link DataLogManager} alongside Driver + * Station values and saved to file. Loop timings are also recorded using the + * {@link Profiler} class. + * + *

{@code periodic()} functions from the base class are called on an interval + * by a Notifier instance. Additionally, utility classes with periodic methods + * such as {@link Tunable} are updated on the same interval. + */ +public class LoggedRobot extends IterativeRobotBase { + + public static final double DEFAULT_PERIOD = 0.02; + + private final int notifier = NotifierJNI.initializeNotifier(); + private final Consumer logger; + + private long nextCycle = 0L; + + /** + * Constructor for LoggedRobot. + */ + protected LoggedRobot() { + this(DEFAULT_PERIOD); + } + + /** + * Constructor for LoggedRobot. + * @param period Period in seconds. + */ + protected LoggedRobot(double period) { + super(period); + NotifierJNI.setNotifierName(notifier, "LoggedRobot"); + + DriverStation.silenceJoystickConnectionWarning(true); + DisableWatchdog.in(this, "m_watchdog"); + + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + SignalLogger.enableAutoLogging(false); + + EpilogueProxy.getConfig().root = "/Telemetry"; + logger = EpilogueProxy.getLogger(this); + } + + @Override + public void startCompetition() { + robotInit(); + if (isSimulation()) { + simulationInit(); + } + + // Tell the DS that the robot is ready to be enabled + System.out.println("********** Robot program startup complete **********"); + DriverStationJNI.observeUserProgramStarting(); + + // Loop forever, calling the appropriate mode-dependent function + while (true) { + long now = RobotController.getFPGATime(); + if (nextCycle < now) { + nextCycle = now; + } else { + NotifierJNI.updateNotifierAlarm(notifier, nextCycle); + if (NotifierJNI.waitForNotifierAlarm(notifier) == 0L) { + break; + } + } + + nextCycle += (long) (getPeriod() * 1000000.0); + + Profiler.start("robot"); + Profiler.run("loopFunction", () -> loopFunc()); + Profiler.run("epilogue", () -> logger.accept(EpilogueProxy.getRootBackend())); + Profiler.run("tunables", Tunable::update); + Profiler.end(); + } + } + + @Override + public void endCompetition() { + NotifierJNI.stopNotifier(notifier); + } + + @Override + public void close() { + NotifierJNI.stopNotifier(notifier); + NotifierJNI.cleanNotifier(notifier); + } + + @Override + public void robotPeriodic() {} + + @Override + public void simulationPeriodic() {} + + @Override + public void disabledPeriodic() {} + + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopPeriodic() {} + + @Override + public void testPeriodic() {} +} diff --git a/src/main/java/org/team340/lib/util/Profiler.java b/src/main/java/org/team340/lib/logging/Profiler.java similarity index 89% rename from src/main/java/org/team340/lib/util/Profiler.java rename to src/main/java/org/team340/lib/logging/Profiler.java index f9225d4..280c73b 100644 --- a/src/main/java/org/team340/lib/util/Profiler.java +++ b/src/main/java/org/team340/lib/logging/Profiler.java @@ -1,4 +1,4 @@ -package org.team340.lib.util; +package org.team340.lib.logging; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; @@ -17,8 +17,8 @@ * A simple pseudo call-graph profiler that publishes timings to NetworkTables. * *

Data is published to {@code /Profiling}, with CPU time utilized by the garbage - * collector published at {@code /Profiling/GC}, and time spent publishing values to - * NT at {@code /Profiling/Overhead}. All timings are recorded in milliseconds, + * collector published at {@code /Profiling/gc}, and time spent publishing values to + * NT at {@code /Profiling/overhead}. All timings are recorded in milliseconds, * as the duration of execution. The profiler records the time to execute blocks * of code, with support for nesting execution times which are reflected in an * expanding NT tree. @@ -26,15 +26,15 @@ *

All invocations of {@link Profiler#start(String)} are expected to be * followed by a closing {@link Profiler#end()}. Additionally, it is expected * that a single {@link Profiler#start(String)}/{@link Profiler#end()} pair is - * found at the highest level of the robot's code (i.e. {@code robotPeriodic}), - * with no other "root" pairs. If either of these limitations are left unsatisfied, - * an error will be printed to the Driver Station. Profiling across threads is also - * not supported. + * found at the highest level of the robot's code, with no other "root" pairs + * (this is already done if utilizing {@link LoggedRobot}. If either of these + * limitations are left unsatisfied, an error will be printed to the Driver Station + * console. Profiling across threads is also not supported. */ public final class Profiler { private Profiler() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Profiling"); @@ -47,7 +47,7 @@ private Profiler() { private static final DoublePublisher gcPub = nt.getDoubleTopic("gc").publish(); private static String root = ""; - private static long lastGCTime = 0; + private static long lastGCTime = 0L; /** * Determines if the profiler is currently running. @@ -123,7 +123,7 @@ public static void start(String name) { /** * Ends profiling a section of code. This method should be invoked * after the user code to be profiled. Trailing calls to this method - * will result in an error printed to the Driver Station. + * will result in an error printed to the Driver Station console. */ public static void end() { CallData call = callGraph.get(String.join("/", stack)); @@ -145,10 +145,10 @@ public static void end() { entryCall.pubAndReset(start); } - long gcSum = 0; + long gcSum = 0L; for (var gc : gcList) { long gcTime = gc.getCollectionTime(); - if (gcTime != -1) gcSum += gcTime; + if (gcTime != -1L) gcSum += gcTime; } gcPub.set(gcSum - lastGCTime); lastGCTime = gcSum; @@ -166,7 +166,7 @@ public static void end() { private static final class CallData implements AutoCloseable { public final String fullName; - public long time = -1; + public long time = -1L; public boolean done = false; public DoublePublisher pub; @@ -196,7 +196,7 @@ public void onEnd() { public void pubAndReset(long timestamp) { if (pub == null) pub = nt.getDoubleTopic(fullName).publish(); pub.set(time / 1000.0, timestamp); - time = -1; + time = -1L; done = false; } diff --git a/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java b/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java deleted file mode 100644 index 1e14a01..0000000 --- a/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.apriltag; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(AprilTagFieldLayout.class) -public class AprilTagFieldLayoutLogger extends ClassSpecificLogger { - - public AprilTagFieldLayoutLogger() { - super(AprilTagFieldLayout.class); - } - - @Override - public void update(EpilogueBackend backend, AprilTagFieldLayout fieldLayout) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java deleted file mode 100644 index 0243526..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix; - -import com.ctre.phoenix6.CANBus; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(CANBus.class) -public class CANBusLogger extends ClassSpecificLogger { - - public CANBusLogger() { - super(CANBus.class); - } - - @Override - public void update(EpilogueBackend backend, CANBus canBus) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java index b5458cc..f3167f8 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java @@ -1,14 +1,10 @@ package org.team340.lib.logging.phoenix; -import static edu.wpi.first.util.struct.StructGenerator.genEnum; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.MagnetHealthValue; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.Map; import java.util.function.Consumer; @@ -17,27 +13,22 @@ public class CANcoderLogger extends ClassSpecificLogger { private static final Map> cache = new HashMap<>(); - private static final Struct magnetHealthStruct = genEnum(MagnetHealthValue.class); public CANcoderLogger() { super(CANcoder.class); } @Override - public void update(EpilogueBackend backend, CANcoder canCoder) { + public void update(EpilogueBackend backend, CANcoder cancoder) { cache - .computeIfAbsent(canCoder, key -> { - var absolutePosition = canCoder.getAbsolutePosition(false); - var magnetHealth = canCoder.getMagnetHealth(false); - var position = canCoder.getPosition(false); - var velocity = canCoder.getVelocity(false); + .computeIfAbsent(cancoder, key -> { + var position = cancoder.getPosition(false); + var velocity = cancoder.getVelocity(false); - BaseStatusSignal[] signals = { absolutePosition, magnetHealth, position, velocity }; + BaseStatusSignal[] signals = { position, velocity }; return b -> { BaseStatusSignal.refreshAll(signals); - b.log("absolutePosition", absolutePosition.getValueAsDouble()); - b.log("magnetHealth", magnetHealth.getValue(), magnetHealthStruct); b.log("position", position.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java new file mode 100644 index 0000000..36ffeaf --- /dev/null +++ b/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging.phoenix; + +import com.ctre.phoenix6.hardware.CANdle; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; + +@CustomLoggerFor(CANdle.class) +public class CANdleLogger extends ClassSpecificLogger { + + public CANdleLogger() { + super(CANdle.class); + } + + @Override + public void update(EpilogueBackend backend, CANdle candle) { + // No-op + } +} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java index ed21047..cc00672 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java @@ -19,12 +19,12 @@ public CANrangeLogger() { } @Override - public void update(EpilogueBackend backend, CANrange canRange) { + public void update(EpilogueBackend backend, CANrange canrange) { cache - .computeIfAbsent(canRange, key -> { - var ambientSignal = canRange.getAmbientSignal(false); - var distance = canRange.getDistance(false); - var isDetected = canRange.getIsDetected(false); + .computeIfAbsent(canrange, key -> { + var ambientSignal = canrange.getAmbientSignal(false); + var distance = canrange.getDistance(false); + var isDetected = canrange.getIsDetected(false); BaseStatusSignal[] signals = { ambientSignal, distance, isDetected }; diff --git a/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java b/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java deleted file mode 100644 index 84a12ef..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.team340.lib.logging.phoenix; - -import com.ctre.phoenix6.StatusSignal; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(StatusSignal.class) -public class StatusSignalLogger extends ClassSpecificLogger> { - - @SuppressWarnings({ "unchecked", "rawtypes" }) - public StatusSignalLogger() { - super((Class) StatusSignal.class); - } - - @Override - public void update(EpilogueBackend backend, StatusSignal signal) {} -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java b/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java index b9e744a..5ae7c48 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java @@ -27,6 +27,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { var motorVoltage = talonFX.getMotorVoltage(false); var position = talonFX.getPosition(false); var statorCurrent = talonFX.getStatorCurrent(false); + var supplyCurrent = talonFX.getSupplyCurrent(false); var velocity = talonFX.getVelocity(false); BaseStatusSignal[] signals = { @@ -35,6 +36,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { motorVoltage, position, statorCurrent, + supplyCurrent, velocity }; @@ -45,6 +47,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { b.log("motorVoltage", motorVoltage.getValueAsDouble()); b.log("position", position.getValueAsDouble()); b.log("statorCurrent", statorCurrent.getValueAsDouble()); + b.log("supplyCurrent", supplyCurrent.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; }) diff --git a/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java b/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java index b6d3a1f..c7897e0 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java @@ -27,6 +27,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { var motorVoltage = talonFXS.getMotorVoltage(false); var position = talonFXS.getPosition(false); var statorCurrent = talonFXS.getStatorCurrent(false); + var supplyCurrent = talonFXS.getSupplyCurrent(false); var velocity = talonFXS.getVelocity(false); BaseStatusSignal[] signals = { @@ -35,6 +36,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { motorVoltage, position, statorCurrent, + supplyCurrent, velocity }; @@ -45,6 +47,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { b.log("motorVoltage", motorVoltage.getValueAsDouble()); b.log("position", position.getValueAsDouble()); b.log("statorCurrent", statorCurrent.getValueAsDouble()); + b.log("supplyCurrent", supplyCurrent.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; }) diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java deleted file mode 100644 index 1953e57..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.CoastOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(CoastOut.class) -public class CoastOutLogger extends ClassSpecificLogger { - - public CoastOutLogger() { - super(CoastOut.class); - } - - @Override - public void update(EpilogueBackend backend, CoastOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java deleted file mode 100644 index eda3331..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.DutyCycleOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(DutyCycleOut.class) -public class DutyCycleOutLogger extends ClassSpecificLogger { - - public DutyCycleOutLogger() { - super(DutyCycleOut.class); - } - - @Override - public void update(EpilogueBackend backend, DutyCycleOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java deleted file mode 100644 index 375e056..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(DynamicMotionMagicVoltage.class) -public class DynamicMotionMagicVoltageLogger extends ClassSpecificLogger { - - public DynamicMotionMagicVoltageLogger() { - super(DynamicMotionMagicVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, DynamicMotionMagicVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java deleted file mode 100644 index 01700ac..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.Follower; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(Follower.class) -public class FollowerLogger extends ClassSpecificLogger { - - public FollowerLogger() { - super(Follower.class); - } - - @Override - public void update(EpilogueBackend backend, Follower control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java deleted file mode 100644 index 25077bd..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(MotionMagicVoltage.class) -public class MotionMagicVoltageLogger extends ClassSpecificLogger { - - public MotionMagicVoltageLogger() { - super(MotionMagicVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, MotionMagicVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java deleted file mode 100644 index 02da2e5..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.PositionVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(PositionVoltage.class) -public class PositionVoltageLogger extends ClassSpecificLogger { - - public PositionVoltageLogger() { - super(PositionVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, PositionVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java deleted file mode 100644 index 23a8120..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.StrictFollower; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(StrictFollower.class) -public class StrictFollowerLogger extends ClassSpecificLogger { - - public StrictFollowerLogger() { - super(StrictFollower.class); - } - - @Override - public void update(EpilogueBackend backend, StrictFollower control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java deleted file mode 100644 index c7ee95d..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(TorqueCurrentFOC.class) -public class TorqueCurrentFOCLogger extends ClassSpecificLogger { - - public TorqueCurrentFOCLogger() { - super(TorqueCurrentFOC.class); - } - - @Override - public void update(EpilogueBackend backend, TorqueCurrentFOC control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java deleted file mode 100644 index ce3bb6e..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VelocityTorqueCurrentFOC.class) -public class VelocityTorqueCurrentFOCLogger extends ClassSpecificLogger { - - public VelocityTorqueCurrentFOCLogger() { - super(VelocityTorqueCurrentFOC.class); - } - - @Override - public void update(EpilogueBackend backend, VelocityTorqueCurrentFOC control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java deleted file mode 100644 index f28f974..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VelocityVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VelocityVoltage.class) -public class VelocityVoltageLogger extends ClassSpecificLogger { - - public VelocityVoltageLogger() { - super(VelocityVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, VelocityVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java deleted file mode 100644 index d79a619..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VoltageOut.class) -public class VoltageOutLogger extends ClassSpecificLogger { - - public VoltageOutLogger() { - super(VoltageOut.class); - } - - @Override - public void update(EpilogueBackend backend, VoltageOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java b/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java deleted file mode 100644 index 99ddfb7..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.EstimatedRobotPose; - -@CustomLoggerFor(EstimatedRobotPose.class) -public class EstimatedRobotPoseLogger extends ClassSpecificLogger { - - public EstimatedRobotPoseLogger() { - super(EstimatedRobotPose.class); - } - - @Override - public void update(EpilogueBackend backend, EstimatedRobotPose estimatedRobotPose) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java b/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java deleted file mode 100644 index 151cac6..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.PhotonCamera; - -@CustomLoggerFor(PhotonCamera.class) -public class PhotonCameraLogger extends ClassSpecificLogger { - - public PhotonCameraLogger() { - super(PhotonCamera.class); - } - - @Override - public void update(EpilogueBackend backend, PhotonCamera photonCamera) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java b/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java deleted file mode 100644 index 602e4e8..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.PhotonPoseEstimator; - -@CustomLoggerFor(PhotonPoseEstimator.class) -public class PhotonPoseEstimatorLogger extends ClassSpecificLogger { - - public PhotonPoseEstimatorLogger() { - super(PhotonPoseEstimator.class); - } - - @Override - public void update(EpilogueBackend backend, PhotonPoseEstimator photonPoseEstimator) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java deleted file mode 100644 index 3512e89..0000000 --- a/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.revlib; - -import com.revrobotics.spark.SparkClosedLoopController; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(SparkClosedLoopController.class) -public class SparkClosedLoopControllerLogger extends ClassSpecificLogger { - - public SparkClosedLoopControllerLogger() { - super(SparkClosedLoopController.class); - } - - @Override - public void update(EpilogueBackend backend, SparkClosedLoopController pidController) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java b/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java deleted file mode 100644 index e854ea1..0000000 --- a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpilibj; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -@CustomLoggerFor(AddressableLEDBuffer.class) -public class AddressableLEDBufferLogger extends ClassSpecificLogger { - - public AddressableLEDBufferLogger() { - super(AddressableLEDBuffer.class); - } - - @Override - public void update(EpilogueBackend backend, AddressableLEDBuffer buffer) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java b/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java deleted file mode 100644 index ab294dd..0000000 --- a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpilibj; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.wpilibj.AddressableLED; - -@CustomLoggerFor(AddressableLED.class) -public class AddressableLEDLogger extends ClassSpecificLogger { - - public AddressableLEDLogger() { - super(AddressableLED.class); - } - - @Override - public void update(EpilogueBackend backend, AddressableLED addressableLED) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java b/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java deleted file mode 100644 index 5383818..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.filter.Debouncer; - -@CustomLoggerFor(Debouncer.class) -public class DebouncerLogger extends ClassSpecificLogger { - - public DebouncerLogger() { - super(Debouncer.class); - } - - @Override - public void update(EpilogueBackend backend, Debouncer debouncer) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java b/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java deleted file mode 100644 index d60f648..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.ExponentialProfile; - -@CustomLoggerFor(ExponentialProfile.Constraints.class) -public class ExponentialProfileConstraintsLogger extends ClassSpecificLogger { - - public ExponentialProfileConstraintsLogger() { - super(ExponentialProfile.Constraints.class); - } - - @Override - public void update(EpilogueBackend backend, ExponentialProfile.Constraints constraints) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java b/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java deleted file mode 100644 index ffddec7..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.ExponentialProfile; - -@CustomLoggerFor(ExponentialProfile.class) -public class ExponentialProfileLogger extends ClassSpecificLogger { - - public ExponentialProfileLogger() { - super(ExponentialProfile.class); - } - - @Override - public void update(EpilogueBackend backend, ExponentialProfile profile) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java b/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java deleted file mode 100644 index 98fb578..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -@CustomLoggerFor(TrapezoidProfile.Constraints.class) -public class TrapezoidProfileConstraintsLogger extends ClassSpecificLogger { - - public TrapezoidProfileConstraintsLogger() { - super(TrapezoidProfile.Constraints.class); - } - - @Override - public void update(EpilogueBackend backend, TrapezoidProfile.Constraints constraints) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java b/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java deleted file mode 100644 index de4789a..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -@CustomLoggerFor(TrapezoidProfile.class) -public class TrapezoidProfileLogger extends ClassSpecificLogger { - - public TrapezoidProfileLogger() { - super(TrapezoidProfile.class); - } - - @Override - public void update(EpilogueBackend backend, TrapezoidProfile profile) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/swerve/Perspective.java b/src/main/java/org/team340/lib/swerve/Perspective.java index 0f3d281..ad855ba 100644 --- a/src/main/java/org/team340/lib/swerve/Perspective.java +++ b/src/main/java/org/team340/lib/swerve/Perspective.java @@ -13,20 +13,20 @@ public enum Perspective { * is on the blue alliance, X+ drives towards the red alliance. If the * robot is on the red alliance, X+ drives towards the blue alliance. */ - kOperator { + OPERATOR { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - return (Alliance.isBlue() ? kBlue : kRed).toRobotSpeeds(speeds, robotAngle); + return (Alliance.isBlue() ? BLUE : RED).toRobotSpeeds(speeds, robotAngle); } @Override public ChassisSpeeds toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - return (Alliance.isBlue() ? kBlue : kRed).toPerspectiveSpeeds(speeds, robotAngle); + return (Alliance.isBlue() ? BLUE : RED).toPerspectiveSpeeds(speeds, robotAngle); } @Override public Rotation2d getTareRotation() { - return (Alliance.isBlue() ? kBlue : kRed).getTareRotation(); + return (Alliance.isBlue() ? BLUE : RED).getTareRotation(); } }, @@ -34,7 +34,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the blue alliance perspective. * X+ drives towards the red alliance. */ - kBlue { + BLUE { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle); @@ -55,7 +55,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the red alliance perspective. * X+ drives towards the blue alliance. */ - kRed { + RED { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle.rotateBy(Rotation2d.kPi)); @@ -76,7 +76,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the robot's perspective. * X+ drives forwards relative to the chassis. */ - kRobot { + ROBOT { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return speeds; diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index 7268d4b..03d3a03 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -1,6 +1,7 @@ package org.team340.lib.swerve; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; @@ -18,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; @@ -67,6 +69,8 @@ public class SwerveAPI implements AutoCloseable { public SwerveAPI(SwerveConfig config) { this.config = config; + if (RobotBase.isSimulation()) config.phoenixCanBus = new CANBus(); + moduleCount = config.modules.length; modules = new SwerveModule[moduleCount]; moduleLocations = new Translation2d[moduleCount]; @@ -120,6 +124,7 @@ public void refresh() { } state.pose = poseEstimator.getEstimatedPosition(); + state.odometryPose = odometry.getPoseMeters(); state.poseHistory.clear(); state.poseHistory.addAll(odometryThread.poseHistory); @@ -186,8 +191,8 @@ public void resetPose(Pose2d pose) { /** * Tares the rotation of the robot. Useful for fixing an out of sync or drifting - * IMU. For most cases, a perspective of {@link Perspective#kOperator} is - * desirable. {@link Perspective#kRobot} will no-op. + * IMU. For most cases, a perspective of {@link Perspective#OPERATOR} is + * desirable. {@link Perspective#ROBOT} will no-op. * @param perspective The perspective to tare the rotation to. */ public void tareRotation(Perspective perspective) { @@ -327,57 +332,45 @@ public void applySpeeds(ChassisSpeeds speeds, Perspective perspective, boolean d if (ratelimit) { double now = Timer.getFPGATimestamp(); - if (now - lastRatelimit < config.period * 2.0) { - ChassisSpeeds lastSpeeds = perspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle); - - double vx_l = lastSpeeds.vxMetersPerSecond; - double vy_l = lastSpeeds.vyMetersPerSecond; - double v_l = Math.hypot(vx_l, vy_l); - double w_l = lastSpeeds.omegaRadiansPerSecond; - - double dx = speeds.vxMetersPerSecond - vx_l; - double dy = speeds.vyMetersPerSecond - vy_l; - double a_slip = config.slipAccel * config.period; - if (dx * dx + dy * dy > a_slip * a_slip) { - double k = a_slip / Math.hypot(dx, dy); - speeds.vxMetersPerSecond = vx_l + (k * dx); - speeds.vyMetersPerSecond = vy_l + (k * dy); - } - - double v = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - double a_torque = (config.torqueAccel * config.period) * (1.0 - (v_l / config.velocity)); - if (v - v_l > a_torque) { - double k = (v_l + a_torque) / v; - speeds.vxMetersPerSecond *= k; - speeds.vyMetersPerSecond *= k; - } + ChassisSpeeds lastSpeeds = now - lastRatelimit < config.period * 4.0 + ? perspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle) + : perspective.toPerspectiveSpeeds(state.speeds, state.rotation); + + double vx_l = lastSpeeds.vxMetersPerSecond; + double vy_l = lastSpeeds.vyMetersPerSecond; + double v_l = Math.hypot(vx_l, vy_l); + double w_l = lastSpeeds.omegaRadiansPerSecond; + + double dx = speeds.vxMetersPerSecond - vx_l; + double dy = speeds.vyMetersPerSecond - vy_l; + double a_slip = config.slipAccel * config.period; + if (dx * dx + dy * dy > a_slip * a_slip) { + double k = a_slip / Math.hypot(dx, dy); + speeds.vxMetersPerSecond = vx_l + (k * dx); + speeds.vyMetersPerSecond = vy_l + (k * dy); + } - double dw = speeds.omegaRadiansPerSecond - w_l; - double a_angular = config.angularAccel * config.period; - speeds.omegaRadiansPerSecond = w_l + (Math.min(a_angular / Math.abs(dw), 1.0) * dw); + double v = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + double a_torque = (config.torqueAccel * config.period) * (1.0 - (v_l / config.velocity)); + if (v - v_l > a_torque && v > 1e-6) { + double k = (v_l + a_torque) / v; + speeds.vxMetersPerSecond *= k; + speeds.vyMetersPerSecond *= k; } + double dw = speeds.omegaRadiansPerSecond - w_l; + double a_angular = config.angularAccel * config.period; + speeds.omegaRadiansPerSecond = w_l + (Math.min(a_angular / Math.abs(dw), 1.0) * dw); + lastRatelimit = now; } if (discretize) { - double dtheta = speeds.omegaRadiansPerSecond * config.discretizationPeriod; - - double sin = -dtheta / 2.0; - double cos = Math2.epsilonEquals(Math.cos(dtheta) - 1.0, 0.0) - ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) - : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); - - double dt = config.period; - double dx = speeds.vxMetersPerSecond * dt; - double dy = speeds.vyMetersPerSecond * dt; - - speeds.vxMetersPerSecond = ((dx * cos) - (dy * sin)) / dt; - speeds.vyMetersPerSecond = ((dx * sin) + (dy * cos)) / dt; + Math2.discretize(speeds, config.discretizationPeriod); } - lastRobotAngle = state.pose.getRotation(); - Math2.copyInto(perspective.toRobotSpeeds(speeds, state.pose.getRotation()), state.targetSpeeds); + lastRobotAngle = state.rotation; + Math2.copyInto(perspective.toRobotSpeeds(speeds, state.rotation), state.targetSpeeds); applyStates(kinematics.toSwerveModuleStates(state.targetSpeeds)); } @@ -403,7 +396,10 @@ public void applyStates(SwerveModuleState[] states) { */ public void applyStop(boolean lock) { lastRatelimit = Timer.getFPGATimestamp(); - Math2.zero(state.targetSpeeds); + + state.targetSpeeds.vxMetersPerSecond = 0.0; + state.targetSpeeds.vyMetersPerSecond = 0.0; + state.targetSpeeds.omegaRadiansPerSecond = 0.0; for (int i = 0; i < moduleCount; i++) { SwerveModuleState state; diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java index c827123..e5f1a90 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java @@ -1,7 +1,6 @@ package org.team340.lib.swerve; import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; import edu.wpi.first.math.geometry.Pose2d; @@ -9,10 +8,16 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import org.team340.lib.logging.EpilogueProxy; @CustomLoggerFor(SwerveAPI.class) public class SwerveAPILogger extends ClassSpecificLogger { + private static final Map> cache = new HashMap<>(); + public SwerveAPILogger() { super(SwerveAPI.class); } @@ -21,16 +26,31 @@ public SwerveAPILogger() { public void update(EpilogueBackend backend, SwerveAPI api) { logState(backend.getNested("state"), api.state); - var hardware = backend.getNested("hardware"); - var errorHandler = Epilogue.getConfig().errorHandler; + cache + .computeIfAbsent(api, key -> { + var imu = EpilogueProxy.getLogger(api.imu.getAPI()); + + Map> modules = new HashMap<>(); + for (SwerveModule module : api.modules) { + var move = EpilogueProxy.getLogger(module.moveMotor.getAPI()); + var turn = EpilogueProxy.getLogger(module.turnMotor.getAPI()); + var encoder = EpilogueProxy.getLogger(module.encoder.getAPI()); + + modules.put(module.getName(), b -> { + move.accept(b.getNested("moveMotor")); + turn.accept(b.getNested("turnMotor")); + encoder.accept(b.getNested("encoder")); + }); + } - api.imu.log(hardware.getNested("imu"), errorHandler); - for (var m : api.modules) { - var module = hardware.getNested(m.getName()); - m.moveMotor.log(module.getNested("moveMotor"), errorHandler); - m.turnMotor.log(module.getNested("turnMotor"), errorHandler); - m.encoder.log(module.getNested("encoder"), errorHandler); - } + return b -> { + imu.accept(b.getNested("imu")); + for (var module : modules.entrySet()) { + module.getValue().accept(b.getNested(module.getKey())); + } + }; + }) + .accept(backend.getNested("hardware")); } private void logState(EpilogueBackend backend, SwerveState state) { @@ -39,6 +59,7 @@ private void logState(EpilogueBackend backend, SwerveState state) { backend.log("pose", state.pose, Pose2d.struct); backend.log("pitch", state.pitch, Rotation2d.struct); backend.log("roll", state.roll, Rotation2d.struct); + backend.log("odometryPose", state.odometryPose, Pose2d.struct); var modules = backend.getNested("modules"); modules.log("positions", state.modules.positions, SwerveModulePosition.struct); diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index e6b5e2b..7717323 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -75,7 +75,7 @@ public List getSignals() { /** * Refreshes the cached position and state of the module. - * @return {@code true} on success, {@code false} if a read error ocurred. + * @return {@code true} on success, {@code false} if a read error occurred. */ public boolean refresh() { double turnPosition = turnMotor.getPosition(); @@ -139,7 +139,7 @@ public SwerveModuleState getNextTarget() { * @param state The state to apply to the module. */ public void applyState(SwerveModuleState state) { - if (state.speedMetersPerSecond < config.velDeadband) { + if (Math.abs(state.speedMetersPerSecond) < config.velDeadband) { state.speedMetersPerSecond = 0.0; state.angle = nextTarget.angle; } @@ -155,7 +155,7 @@ public void applyState(SwerveModuleState state) { } boolean flipped = false; - if (Math.abs(angleDelta.getRadians()) > Math2.kHalfPi) { + if (Math.abs(angleDelta.getRadians()) > Math2.HALF_PI) { state.speedMetersPerSecond *= -1.0; state.angle = state.angle.rotateBy(Rotation2d.kPi); flipped = true; diff --git a/src/main/java/org/team340/lib/swerve/SwerveState.java b/src/main/java/org/team340/lib/swerve/SwerveState.java index 5ca9cbf..0311198 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveState.java +++ b/src/main/java/org/team340/lib/swerve/SwerveState.java @@ -85,6 +85,8 @@ private OdometryThread() {} public Rotation2d pitch; /** The robot's roll. */ public Rotation2d roll; + /** The uncorrected blue origin relative odometry pose of the robot. */ + public Pose2d odometryPose; /** The timestamp of the swerve state in seconds (FPGA time). */ public double timestamp; @@ -99,5 +101,6 @@ private OdometryThread() {} rotation = Rotation2d.kZero; pitch = Rotation2d.kZero; roll = Rotation2d.kZero; + odometryPose = Pose2d.kZero; } } diff --git a/src/main/java/org/team340/lib/swerve/SwerveTunables.java b/src/main/java/org/team340/lib/swerve/SwerveTunables.java index 2d33988..4311322 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveTunables.java +++ b/src/main/java/org/team340/lib/swerve/SwerveTunables.java @@ -9,7 +9,7 @@ final class SwerveTunables { private SwerveTunables() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -21,68 +21,64 @@ public static void initialize(String name, SwerveAPI api) { SwerveConfig config = api.config; // setTimings() - Tunable.doubleValue(name + "/timings/discretization", config.discretizationPeriod, v -> + Tunable.value(name + "/timings/discretization", config.discretizationPeriod, v -> config.discretizationPeriod = v ); // setMovePID() - Tunable.doubleValue(name + "/movePID/kP", config.movePID[0], v -> { + Tunable.value(name + "/movePID/kP", config.movePID[0], v -> { config.movePID[0] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/movePID/kI", config.movePID[1], v -> { + Tunable.value(name + "/movePID/kI", config.movePID[1], v -> { config.movePID[1] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/movePID/kD", config.movePID[2], v -> { + Tunable.value(name + "/movePID/kD", config.movePID[2], v -> { config.movePID[2] = v; reapplyGains(true, api); }); // setMoveFF() - Tunable.doubleValue(name + "/moveFF/kS", config.moveFF[0], v -> { + Tunable.value(name + "/moveFF/kS", config.moveFF[0], v -> { config.moveFF[0] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/moveFF/kV", config.moveFF[1], v -> { + Tunable.value(name + "/moveFF/kV", config.moveFF[1], v -> { config.moveFF[1] = v; reapplyGains(true, api); }); // setTurnPID() - Tunable.doubleValue(name + "/turnPID/kP", config.turnPID[0], v -> { + Tunable.value(name + "/turnPID/kP", config.turnPID[0], v -> { config.turnPID[0] = v; reapplyGains(false, api); }); - Tunable.doubleValue(name + "/turnPID/kI", config.turnPID[1], v -> { + Tunable.value(name + "/turnPID/kI", config.turnPID[1], v -> { config.turnPID[1] = v; reapplyGains(false, api); }); - Tunable.doubleValue(name + "/turnPID/kD", config.turnPID[2], v -> { + Tunable.value(name + "/turnPID/kD", config.turnPID[2], v -> { config.turnPID[2] = v; reapplyGains(false, api); }); // setLimits() - Tunable.doubleValue(name + "/limits/velocity", config.velocity, v -> config.velocity = v); - Tunable.doubleValue(name + "/limits/velDeadband", config.velDeadband, v -> config.velDeadband = v); - Tunable.doubleValue(name + "/limits/slipAccel", config.slipAccel, v -> config.slipAccel = v); - Tunable.doubleValue(name + "/limits/torqueAccel", config.torqueAccel, v -> config.torqueAccel = v); - Tunable.doubleValue(name + "/limits/angularAccel", config.angularAccel, v -> config.angularAccel = v); + Tunable.value(name + "/limits/velocity", config.velocity, v -> config.velocity = v); + Tunable.value(name + "/limits/velDeadband", config.velDeadband, v -> config.velDeadband = v); + Tunable.value(name + "/limits/slipAccel", config.slipAccel, v -> config.slipAccel = v); + Tunable.value(name + "/limits/torqueAccel", config.torqueAccel, v -> config.torqueAccel = v); + Tunable.value(name + "/limits/angularAccel", config.angularAccel, v -> config.angularAccel = v); // setDriverProfile() - Tunable.doubleValue(name + "/driverProfile/vel", config.driverVel, v -> config.driverVel = v); - Tunable.doubleValue(name + "/driverProfile/velExp", config.driverVelExp, v -> config.driverVelExp = v); - Tunable.doubleValue(name + "/driverProfile/velDeadband", config.driverVelDeadband, v -> - config.driverVelDeadband = v - ); - Tunable.doubleValue(name + "/driverProfile/angularVel", config.driverAngularVel, v -> - config.driverAngularVel = v - ); - Tunable.doubleValue(name + "/driverProfile/angularVelExp", config.driverAngularVelExp, v -> + Tunable.value(name + "/driverProfile/vel", config.driverVel, v -> config.driverVel = v); + Tunable.value(name + "/driverProfile/velExp", config.driverVelExp, v -> config.driverVelExp = v); + Tunable.value(name + "/driverProfile/velDeadband", config.driverVelDeadband, v -> config.driverVelDeadband = v); + Tunable.value(name + "/driverProfile/angularVel", config.driverAngularVel, v -> config.driverAngularVel = v); + Tunable.value(name + "/driverProfile/angularVelExp", config.driverAngularVelExp, v -> config.driverAngularVelExp = v ); - Tunable.doubleValue(name + "/driverProfile/angularVelDeadband", config.driverAngularVelDeadband, v -> + Tunable.value(name + "/driverProfile/angularVelDeadband", config.driverAngularVelDeadband, v -> config.driverAngularVelDeadband = v ); } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java index 9b27413..35c4c98 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java @@ -297,6 +297,16 @@ public SwerveConfig setModules(SwerveModuleConfig... modules) { * Throws an error if an issue is found. */ public void verify() { + var missing = missing(); + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveConfig missing values: " + String.join(", ", missing)); + } + } + + /** + * Collects a list of missing elements. + */ + private List missing() { List missing = new ArrayList<>(); if (period == -1.0) missing.add("Period"); if (odometryPeriod == -1.0) missing.add("Odometry Period"); @@ -327,14 +337,16 @@ public void verify() { if (wheelDiameter == -1.0) missing.add("Wheel Diameter"); if (odometryStdDevs == null) missing.add("Odometry Standard Deviations"); if (imu == null) missing.add("IMU"); - if (modules == null) missing.add("Modules"); - if (!missing.isEmpty()) { - throw new IllegalArgumentException("SwerveConfig missing values: " + String.join(", ", missing)); + if (modules == null || modules.length == 0) { + missing.add("Modules"); + } else { + for (int i = 0; i < modules.length; i++) { + String name = modules[i].name == null ? "Module " + i : modules[i].name; + missing.addAll(modules[i].missing().stream().map(s -> name + ": " + s).toList()); + } } - for (SwerveModuleConfig module : modules) { - module.verify(); - } + return missing; } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java index e63c6e9..f7fe80b 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java @@ -75,6 +75,16 @@ public SwerveModuleConfig setEncoder(SwerveEncoder.Ctor encoder) { * Verifies the config. */ public void verify() { + var missing = missing(); + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveModuleConfig missing values: " + String.join(", ", missing)); + } + } + + /** + * Collects a list of missing elements. + */ + List missing() { List missing = new ArrayList<>(); if (name == null) missing.add("Name"); if (location == null) missing.add("Position"); @@ -82,8 +92,6 @@ public void verify() { if (turnMotor == null) missing.add("Turn Motor"); if (encoder == null) missing.add("Encoder"); - if (!missing.isEmpty()) { - throw new IllegalArgumentException("SwerveModuleConfig missing values: " + String.join(", ", missing)); - } + return missing; } } diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java index 866ab97..ac46a99 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java @@ -1,8 +1,6 @@ package org.team340.lib.swerve.hardware; import com.ctre.phoenix6.BaseStatusSignal; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import java.util.List; import org.team340.lib.swerve.SwerveAPI; @@ -12,13 +10,6 @@ interface SwerveBaseHardware extends AutoCloseable { */ public abstract Object getAPI(); - /** - * Logs the device via Epilogue. - * @param logger The logger to log data to. - * @param errorHandler The handler to use if logging raised an exception. - */ - public abstract void log(EpilogueBackend backend, ErrorHandler errorHandler); - /** * Returns all Phoenix status signals in use by the hardware. Phoenix * hardware should not invoke {@code .refresh()} on their status diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java index 7a7c812..0befdbe 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java @@ -17,16 +17,11 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; import java.util.List; import java.util.function.BiFunction; -import org.team340.lib.logging.phoenix.CANcoderLogger; -import org.team340.lib.logging.reduxlib.CanandmagLogger; -import org.team340.lib.logging.revlib.SparkAbsoluteEncoderLogger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder.HookStatus; @@ -41,7 +36,7 @@ public final class SwerveEncoders { private SwerveEncoders() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -101,7 +96,6 @@ public static SwerveEncoder.Ctor sparkMaxEncoder(double offset, boolean inverted throw new UnsupportedOperationException("Turn motor is not a Spark Max"); } - var deviceLogger = new SparkAbsoluteEncoderLogger(); SparkAbsoluteEncoder encoder = sparkMax.getAbsoluteEncoder(); HookStatus hookStatus = new HookStatus(false, true); @@ -142,11 +136,6 @@ public Object getAPI() { return encoder; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, encoder, errorHandler); - } - @Override public void close() {} }; @@ -164,7 +153,6 @@ public static SwerveEncoder.Ctor sparkFlexEncoder(double offset, boolean inverte throw new UnsupportedOperationException("Turn motor is not a Spark Flex"); } - var deviceLogger = new SparkAbsoluteEncoderLogger(); SparkAbsoluteEncoder encoder = sparkFlex.getAbsoluteEncoder(); HookStatus hookStatus = new HookStatus(false, true); @@ -205,11 +193,6 @@ public Object getAPI() { return encoder; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, encoder, errorHandler); - } - @Override public void close() {} }; @@ -224,7 +207,6 @@ public void close() {} */ public static SwerveEncoder.Ctor canandmag(int id, double offset, boolean inverted) { return (config, turnMotor) -> { - var deviceLogger = new CanandmagLogger(); Canandmag canandmag = new Canandmag(id); var settings = new CanandmagSettings() @@ -249,11 +231,6 @@ public Object getAPI() { return canandmag; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canandmag, errorHandler); - } - @Override public void close() { canandmag.close(); @@ -268,28 +245,27 @@ public void close() { * @param offset Offset of the magnet in rotations. * @param inverted If the encoder is inverted. */ - public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverted) { + public static SwerveEncoder.Ctor cancoder(int id, double offset, boolean inverted) { return (config, turnMotor) -> { - var deviceLogger = new CANcoderLogger(); - CANcoder canCoder = new CANcoder(id, config.phoenixCanBus); + CANcoder cancoder = new CANcoder(id, config.phoenixCanBus); HookStatus tempHookStatus = new HookStatus(false, false); - StatusSignal position = canCoder.getPosition().clone(); - StatusSignal velocity = canCoder.getVelocity().clone(); + StatusSignal position = cancoder.getPosition().clone(); + StatusSignal velocity = cancoder.getVelocity().clone(); - var canCoderConfig = new CANcoderConfiguration(); - canCoderConfig.MagnetSensor.MagnetOffset = offset; - canCoderConfig.MagnetSensor.SensorDirection = inverted + var cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = offset; + cancoderConfig.MagnetSensor.SensorDirection = inverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - PhoenixUtil.run("Clear Sticky Faults", () -> canCoder.clearStickyFaults()); - PhoenixUtil.run("Apply CANcoderConfiguration", () -> canCoder.getConfigurator().apply(canCoderConfig)); + PhoenixUtil.run("Clear Sticky Faults", () -> cancoder.clearStickyFaults()); + PhoenixUtil.run("Apply CANcoderConfiguration", () -> cancoder.getConfigurator().apply(cancoderConfig)); PhoenixUtil.run("Set Update Frequency", () -> BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity) ); PhoenixUtil.run("Optimize Bus Utilization", () -> - canCoder.optimizeBusUtilization(1.0 / config.defaultFramePeriod, 0.05) + cancoder.optimizeBusUtilization(1.0 / config.defaultFramePeriod, 0.05) ); if (turnMotor.getAPI() instanceof TalonFX talonFX) { @@ -329,12 +305,7 @@ public HookStatus hookStatus() { @Override public Object getAPI() { - return canCoder; - } - - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canCoder, errorHandler); + return cancoder; } @Override @@ -344,7 +315,7 @@ public List getSignals() { @Override public void close() { - canCoder.close(); + cancoder.close(); } }; }; @@ -373,13 +344,6 @@ public Object getAPI() { return encoder.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - encoder.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("position", getPosition()); - } - @Override public List getSignals() { return encoder.getSignals(); diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java index d05da7e..1a16e49 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java @@ -5,8 +5,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.reduxrobotics.sensors.canandgyro.Canandgyro; import com.reduxrobotics.sensors.canandgyro.CanandgyroSettings; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; @@ -19,9 +17,6 @@ import java.util.List; import java.util.function.Consumer; import java.util.function.Function; -import org.team340.lib.logging.phoenix.Pigeon2Logger; -import org.team340.lib.logging.reduxlib.CanandgyroLogger; -import org.team340.lib.logging.wpilibj.ADIS16470Logger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU.IMUSimHook; @@ -35,7 +30,7 @@ public final class SwerveIMUs { private SwerveIMUs() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -100,7 +95,6 @@ public static SwerveIMU.Ctor adis16470( CalibrationTime calibrationTime ) { return config -> { - var deviceLogger = new ADIS16470Logger(); ADIS16470_IMU adis16470 = new ADIS16470_IMU(yawAxis, pitchAxis, rollAxis, port, calibrationTime); return new SwerveIMU() { @@ -124,11 +118,6 @@ public Object getAPI() { return adis16470; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, adis16470, errorHandler); - } - @Override public void close() { adis16470.close(); @@ -143,7 +132,6 @@ public void close() { */ public static SwerveIMU.Ctor canandgyro(int id) { return config -> { - var deviceLogger = new CanandgyroLogger(); Canandgyro canandgyro = new Canandgyro(id); var settings = new CanandgyroSettings() @@ -189,11 +177,6 @@ public Object getAPI() { return canandgyro; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canandgyro, errorHandler); - } - @Override public void close() { canandgyro.close(); @@ -208,7 +191,6 @@ public void close() { */ public static SwerveIMU.Ctor pigeon2(int id) { return config -> { - var deviceLogger = new Pigeon2Logger(); Pigeon2 pigeon2 = new Pigeon2(id, config.phoenixCanBus); StatusSignal yaw = pigeon2.getYaw().clone(); @@ -261,11 +243,6 @@ public Object getAPI() { return pigeon2; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, pigeon2, errorHandler); - } - @Override public List getSignals() { return List.of(yaw, yawVelocity); @@ -312,15 +289,6 @@ public Object getAPI() { return imu.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - imu.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("yaw", getYaw(), Rotation2d.struct); - sim.log("pitch", getPitch(), Rotation2d.struct); - sim.log("roll", getRoll(), Rotation2d.struct); - } - @Override public List getSignals() { return imu.getSignals(); diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java index 585f6b5..8f21834 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java @@ -22,16 +22,11 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; import java.util.List; import java.util.function.BiFunction; -import org.team340.lib.logging.phoenix.TalonFXLogger; -import org.team340.lib.logging.revlib.SparkFlexLogger; -import org.team340.lib.logging.revlib.SparkMaxLogger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.util.vendors.PhoenixUtil; @@ -43,7 +38,7 @@ public final class SwerveMotors { private SwerveMotors() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -112,7 +107,6 @@ public static SwerveMotor construct(Ctor ctor, SwerveConfig config, boolean isMo */ public static SwerveMotor.Ctor sparkMax(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new SparkMaxLogger(); SparkMax sparkMax = new SparkMax(id, MotorType.kBrushless); RelativeEncoder relativeEncoder = sparkMax.getEncoder(); SparkClosedLoopController pid = sparkMax.getClosedLoopController(); @@ -202,11 +196,6 @@ public Object getAPI() { return sparkMax; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, sparkMax, errorHandler); - } - @Override public boolean readError() { return !sparkMax.getLastError().equals(REVLibError.kOk); @@ -227,7 +216,6 @@ public void close() { */ public static SwerveMotor.Ctor sparkFlex(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new SparkFlexLogger(); SparkFlex sparkFlex = new SparkFlex(id, MotorType.kBrushless); RelativeEncoder relativeEncoder = sparkFlex.getEncoder(); SparkClosedLoopController pid = sparkFlex.getClosedLoopController(); @@ -321,11 +309,6 @@ public Object getAPI() { return sparkFlex; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, sparkFlex, errorHandler); - } - @Override public boolean readError() { return !sparkFlex.getLastError().equals(REVLibError.kOk); @@ -346,7 +329,6 @@ public void close() { */ public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new TalonFXLogger(); TalonFX talonFX = new TalonFX(id, config.phoenixCanBus); StatusSignal position = talonFX.getPosition().clone(); @@ -448,11 +430,6 @@ public Object getAPI() { return talonFX; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, talonFX, errorHandler); - } - @Override public List getSignals() { return List.of(position, velocity); @@ -519,14 +496,6 @@ public Object getAPI() { return motor.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - motor.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("position", getPosition()); - sim.log("velocity", getVelocity()); - } - @Override public List getSignals() { return motor.getSignals(); diff --git a/src/main/java/org/team340/lib/util/Alliance.java b/src/main/java/org/team340/lib/util/Alliance.java index 4a44229..3329def 100644 --- a/src/main/java/org/team340/lib/util/Alliance.java +++ b/src/main/java/org/team340/lib/util/Alliance.java @@ -9,7 +9,7 @@ public final class Alliance { private Alliance() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/DisableWatchdog.java b/src/main/java/org/team340/lib/util/DisableWatchdog.java index 9ae11ec..77b48f1 100644 --- a/src/main/java/org/team340/lib/util/DisableWatchdog.java +++ b/src/main/java/org/team340/lib/util/DisableWatchdog.java @@ -10,7 +10,7 @@ public final class DisableWatchdog { private DisableWatchdog() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/FieldFlip.java b/src/main/java/org/team340/lib/util/FieldFlip.java new file mode 100644 index 0000000..0e35784 --- /dev/null +++ b/src/main/java/org/team340/lib/util/FieldFlip.java @@ -0,0 +1,135 @@ +package org.team340.lib.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.function.Supplier; + +/** + * Utility class for flipping field locations across lines of symmetry. + * By default, {@link FieldInfo} is utilized to retrieve field dimensions. + */ +public final class FieldFlip { + + private FieldFlip() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static double length = FieldInfo.length(); + private static double width = FieldInfo.width(); + + /** + * Overrides the default field dimensions. This method + * should be called before all other user code. + * @param length The field's length, in meters. + * @param width The field's width, in meters. + */ + public static void setFieldDimensions(double length, double width) { + FieldFlip.length = length; + FieldFlip.width = width; + } + + /** + * Flips the provided {@link Translation2d} over the + * length of the field (i.e. hamburger style). + * @param translation The translation to flip. + */ + public static Translation2d overLength(Translation2d translation) { + return new Translation2d(length - translation.getX(), translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the + * length of the field (i.e. hamburger style). + * @param rotation The rotation to flip. + */ + public static Rotation2d overLength(Rotation2d rotation) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } + + /** + * Flips the provided {@link Pose2d} over the + * length of the field (i.e. hamburger style). + * @param pose The pose to flip. + */ + public static Pose2d overLength(Pose2d pose) { + return new Pose2d(overLength(pose.getTranslation()), overLength(pose.getRotation())); + } + + /** + * Flips the provided {@link Translation2d} over the + * width of the field (i.e. hotdog style). + * @param translation The translation to flip. + */ + public static Translation2d overWidth(Translation2d translation) { + return new Translation2d(translation.getX(), width - translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the + * width of the field (i.e. hotdog style). + * @param rotation The rotation to flip. + */ + public static Rotation2d overWidth(Rotation2d rotation) { + return rotation.unaryMinus(); + } + + /** + * Flips the provided {@link Pose2d} over the + * width of the field (i.e. hotdog style). + * @param pose The pose to flip. + */ + public static Pose2d overWidth(Pose2d pose) { + return new Pose2d(overWidth(pose.getTranslation()), overWidth(pose.getRotation())); + } + + /** + * Flips the provided {@link Translation2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param translation The translation to flip. + */ + public static Translation2d overDiagonal(Translation2d translation) { + return new Translation2d(length - translation.getX(), width - translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param rotation The rotation to flip. + */ + public static Rotation2d overDiagonal(Rotation2d rotation) { + return new Rotation2d(-rotation.getCos(), -rotation.getSin()); + } + + /** + * Flips the provided {@link Pose2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param pose The pose to flip. + */ + public static Pose2d overDiagonal(Pose2d pose) { + return new Pose2d(overDiagonal(pose.getTranslation()), overDiagonal(pose.getRotation())); + } + + /** + * Creates a supplier that returns the provided blue origin relative + * {@link Pose2d}, optionally flipped over the length of the field + * (i.e. hamburger style) if the robot is on the red alliance. + * @param pose The blue origin relative pose. + */ + public static Supplier allianceLength(Pose2d pose) { + Pose2d flipped = overLength(pose); + return () -> Alliance.isBlue() ? pose : flipped; + } + + /** + * Creates a supplier that returns the provided blue origin relative + * {@link Pose2d}, optionally flipped over the field's diagonal + * (i.e. rotated 180deg around the field's center) if the robot + * is on the red alliance. + * @param pose The blue origin relative pose. + */ + public static Supplier allianceDiagonal(Pose2d pose) { + Pose2d flipped = overDiagonal(pose); + return () -> Alliance.isBlue() ? pose : flipped; + } +} diff --git a/src/main/java/org/team340/lib/util/FieldInfo.java b/src/main/java/org/team340/lib/util/FieldInfo.java new file mode 100644 index 0000000..3cf47f0 --- /dev/null +++ b/src/main/java/org/team340/lib/util/FieldInfo.java @@ -0,0 +1,64 @@ +package org.team340.lib.util; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Retrieves information about the playing field from an AprilTag field + * layout. By default, {@link AprilTagFields#kDefaultField} is used. + */ +public final class FieldInfo { + + private FieldInfo() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + /** + * Loads a new AprilTag layout to retrieve field information from. + * This method should be called before all other user code. + * @param field The loadable AprilTag field layout. + */ + public static void setLayout(AprilTagFields field) { + layout = AprilTagFieldLayout.loadField(field); + } + + /** + * Loads a new AprilTag layout to retrieve field information from. + * This method should be called before all other user code. + * @param resourcePath The absolute path of the resource. + */ + public static void setLayout(String resourcePath) { + try { + layout = AprilTagFieldLayout.loadFromResource(resourcePath); + } catch (Exception e) { + DriverStation.reportError( + "[AprilTags] Unable to load layout from resource \"" + resourcePath + "\": " + e.getMessage(), + true + ); + } + } + + /** + * Returns the length of the field in meters. + */ + public static double length() { + return layout.getFieldLength(); + } + + /** + * Returns thr width of the field in meters. + */ + public static double width() { + return layout.getFieldWidth(); + } + + /** + * Returns the current field's AprilTag layout. + */ + public static AprilTagFieldLayout aprilTags() { + return layout; + } +} diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index 0feec29..b46a9d0 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -1,10 +1,12 @@ package org.team340.lib.util; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import java.util.Optional; /** * Math utilities. @@ -13,93 +15,89 @@ public final class Math2 { private Math2() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } - /** Shared maximum accuracy floating point. */ - public static final double kEpsilon = 1e-6; /** {@code PI/6} (30deg) */ - public static final double kSixthPi = Math.PI / 6.0; + public static final double SIXTH_PI = 0.5235987755982989; /** {@code PI/4} (45deg) */ - public static final double kQuarterPi = Math.PI / 4.0; + public static final double QUARTER_PI = 0.7853981633974483; /** {@code PI/3} (60deg) */ - public static final double kThirdPi = Math.PI / 3.0; + public static final double THIRD_PI = 1.0471975511965979; /** {@code PI/2} (90deg) */ - public static final double kHalfPi = Math.PI / 2.0; + public static final double HALF_PI = 1.5707963267948966; /** {@code 2PI/3} (120deg) */ - public static final double kTwoThirdsPi = (2.0 * Math.PI) / 3.0; + public static final double TWO_THIRDS_PI = 2.0943951023931957; /** {@code 3PI/4} (135deg) */ - public static final double kThreeQuartersPi = (3.0 * Math.PI) / 4.0; + public static final double THREE_QUARTERS_PI = 2.356194490192345; /** {@code 5PI/6} (150deg) */ - public static final double kFiveSixthsPi = (5.0 * Math.PI) / 6.0; + public static final double FIVE_SIXTHS_PI = 2.6179938779914944; /** {@code PI*2} (360deg) */ - public static final double kTwoPi = Math.PI * 2.0; + public static final double TWO_PI = 6.283185307179586; /** - * Returns a random double from {@code 0.0} to {@code max}. - * @param max The maximum value to return. + * Discretizes a continuous-time chassis speed in place. + * @param speeds The continuous speeds. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + * @return The provided speeds object. + * @see {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} */ - public static double random(double max) { - return Math.random() * max; - } + public static ChassisSpeeds discretize(ChassisSpeeds speeds, double dtSeconds) { + double dtheta = speeds.omegaRadiansPerSecond * dtSeconds; - /** - * Returns a random double from {@code min} to {@code max}. - * @param min The minimum value to return. - * @param max The maximum value to return. - */ - public static double random(double min, double max) { - return (Math.random() * (max - min)) + min; + double sin = -dtheta / 2.0; + double cos = Math.abs(Math.cos(dtheta) - 1.0) < 1e-6 + ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) + : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); + + double dt = dtSeconds; + double dx = speeds.vxMetersPerSecond * dt; + double dy = speeds.vyMetersPerSecond * dt; + + speeds.vxMetersPerSecond = ((dx * cos) - (dy * sin)) / dt; + speeds.vyMetersPerSecond = ((dx * sin) + (dy * cos)) / dt; + + return speeds; } /** - * Check if two optional values are equal within the accuracy of the default epsilon. - * If one option is empty and the other is present, or both options are present and - * unequal, {@code false} is returned. If both options empty, or both options are - * present and equal, {@code true} is returned. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. + * Checks if the distance between two {@link Translation2d}s is within a specified tolerance. + * @param expected The expected {@link Translation2d}. + * @param actual The actual {@link Translation2d}. + * @param tolerance The allowed distance between the actual and the expected translations, in meters. */ - public static boolean epsilonEquals(Optional a, Optional b) { - return epsilonEquals(a, b, kEpsilon); + public static boolean isNear(Translation2d expected, Translation2d actual, double tolerance) { + double dx = expected.getX() - actual.getX(); + double dy = expected.getY() - actual.getY(); + return dx * dx + dy * dy < tolerance * tolerance; } /** - * Check if two optional values are equal within the accuracy of the default epsilon. - * If one option is empty and the other is present, or both options are present and - * unequal, {@code false} is returned. If both options empty, or both options are - * present and equal, {@code true} is returned. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. + * Checks if the angle between two {@link Rotation2d}s is within a specified tolerance. + * @param expected The expected {@link Rotation2d}. + * @param actual The actual {@link Rotation2d}. + * @param tolerance The allowed difference between the actual and the expected rotations, in radians. */ - public static boolean epsilonEquals(Optional a, Optional b, double epsilon) { - if (a == b) return true; - if (a.isEmpty() || b.isEmpty()) return a.isEmpty() && b.isEmpty(); - return epsilonEquals(a.get(), b.get(), epsilon); + public static boolean isNear(Rotation2d expected, Rotation2d actual, double tolerance) { + double dot = expected.getCos() * actual.getCos() + expected.getSin() * actual.getSin(); + return Math.acos(MathUtil.clamp(dot, -1.0, 1.0)) < tolerance; } /** - * Check if two values are equal within the accuracy of the default epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. + * Returns a random double from {@code 0.0} to {@code max}. + * @param max The maximum value to return. */ - public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, kEpsilon); + public static double random(double max) { + return Math.random() * max; } /** - * Check if two values are equal within the accuracy of a provided epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. + * Returns a random double from {@code min} to {@code max}. + * @param min The minimum value to return. + * @param max The maximum value to return. */ - public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); + public static double random(double min, double max) { + return (Math.random() * (max - min)) + min; } /** @@ -151,28 +149,4 @@ public static SwerveModuleState copyInto(SwerveModuleState source, SwerveModuleS output.angle = source.angle; return output; } - - /** - * Zeroes a {@link ChassisSpeeds} object in place. - * @param speeds The speeds to zero. - * @return The provided speeds object. - */ - public static ChassisSpeeds zero(ChassisSpeeds speeds) { - speeds.vxMetersPerSecond = 0.0; - speeds.vyMetersPerSecond = 0.0; - speeds.omegaRadiansPerSecond = 0.0; - return speeds; - } - - /** - * Zeroes a {@link Twist2d} object in place. - * @param twist The twist to zero. - * @return The provided twist object. - */ - public static Twist2d zero(Twist2d twist) { - twist.dx = 0.0; - twist.dy = 0.0; - twist.dtheta = 0.0; - return twist; - } } diff --git a/src/main/java/org/team340/lib/util/PAPFController.java b/src/main/java/org/team340/lib/util/PAPFController.java new file mode 100644 index 0000000..c4f8592 --- /dev/null +++ b/src/main/java/org/team340/lib/util/PAPFController.java @@ -0,0 +1,482 @@ +package org.team340.lib.util; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.epilogue.NotLogged; +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 java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * Implements a predictive artificial potential field (P-APF), + * capable of real-time motion planning with obstacle avoidance. + * + *

This implementation improves upon traditional APF algorithms by predicting the + * future movement of the robot to determine intermediate setpoints, with the goal of + * achieving smoother motion and preventing oscillation caused by suboptimal interactions + * with potentials generated by obstacles. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class PAPFController { + + private final Obstacle[] obstacles; + private double resolution; + private double horizon; + private double tolerance; + private boolean savePrediction; + + // spotless:off + + // Setpoint/goal are Pose2d instead of Translation2d + // due to visualization options in AdvantageScope. + @Logged private final List prediction; + @Logged private Pose2d setpoint = Pose2d.kZero; + @Logged private Pose2d goal = Pose2d.kZero; + + // spotless:on + + /** + * Creates a new P-APF controller. + * @param horizon The horizon at which to predict the robot's motion, in meters. + * Larger horizons increase computational cost, with the benefit + * of generally smoother motion. + * @param resolution The resolution at which to sample the horizon in meters. Higher + * resolutions (i.e. smaller values) will increase accuracy of + * predicted motion, however are more computationally expensive. + * @param tolerance The distance from the goal in meters at which to return a velocity output of 0. + * @param savePrediction If the robot's predicted motion should be saved. Useful + * for debugging, however does increase GC pressure. + * @param obstacles The field's obstacles. + */ + public PAPFController( + double horizon, + double resolution, + double tolerance, + boolean savePrediction, + Obstacle[] obstacles + ) { + this.horizon = horizon; + this.resolution = resolution; + this.tolerance = tolerance; + this.savePrediction = savePrediction; + this.obstacles = obstacles; + + prediction = new ArrayList<>((int) Math.ceil(horizon / resolution)); + } + + /** + * Returns the current setpoint of the controller from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + */ + @NotLogged + public Translation2d getSetpoint() { + return setpoint.getTranslation(); + } + + /** + * Returns the current goal of the controller from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + */ + @NotLogged + public Translation2d getGoal() { + return goal.getTranslation(); + } + + /** + * Returns the current predicted motion of the robot from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + * Note that this list will be empty if {@code savePrediction} is {@code false}. + */ + @NotLogged + public List getPrediction() { + return Collections.unmodifiableList(prediction); + } + + /** + * Returns the next velocity output of the potential field. Note + * that while deceleration is accounted for, acceleration from + * rest is not. If acceleration limiting is desired, it must be + * implemented downstream. + * @param currentPose The current robot pose. + * @param goal The desired position of the robot. + * @param maxVelocity The desired cruise velocity of the robot, in m/s. + * @param maxDeceleration The desired deceleration rate of the robot, in m/s/s. + * + */ + public ChassisSpeeds calculate(Pose2d currentPose, Translation2d goal, double maxVelocity, double maxDeceleration) { + this.goal = new Pose2d(goal, currentPose.getRotation()); + prediction.clear(); + + // To future readers, apologies for the notation crimes. + + double e_x = goal.getX() - currentPose.getX(); + double e_y = goal.getY() - currentPose.getY(); + double error = Math.hypot(e_x, e_y); + + if (error <= tolerance) { + setpoint = this.goal; + return new ChassisSpeeds(); + } + + double rk_x = goal.getX(); + double rk_y = goal.getY(); + + if (horizon > 0.0 && resolution <= error) { + double accum_x = currentPose.getX(); + double accum_y = currentPose.getY(); + NetForce netForce = new NetForce(); + + double d_max = 0.0; + double seg_c = rk_x * currentPose.getY() - rk_y * currentPose.getX(); + for (int i = 0; i < Math.ceil(horizon / resolution); i++) { + netForce.reset(); + forceAt(accum_x, accum_y, goal, netForce); + double norm = Math.hypot(netForce.x, netForce.y); + if (norm < 1e-6) break; + + double alpha = resolution / norm; + accum_x += netForce.x * alpha; + accum_y += netForce.y * alpha; + + if (savePrediction) { + prediction.add(new Translation2d(accum_x, accum_y)); + } + + double d = Math.abs(e_y * accum_x - e_x * accum_y + seg_c) / error; + if (d > resolution && d >= d_max) { + rk_x = accum_x; + rk_y = accum_y; + d_max = d; + } + + double ex_k_n = goal.getX() - accum_x; + double ey_k_n = goal.getY() - accum_y; + if (ex_k_n * ex_k_n + ey_k_n * ey_k_n <= resolution * resolution) { + break; + } + } + } + + setpoint = new Pose2d(rk_x, rk_y, currentPose.getRotation()); + + NetForce force = new NetForce(); + forceAt(currentPose.getX(), currentPose.getY(), setpoint.getTranslation(), force); + + if (error < (maxVelocity * maxVelocity) / (2.0 * maxDeceleration)) { + maxVelocity = Math.sqrt(2.0 * maxDeceleration * error); + } + + double norm = Math.hypot(force.x, force.y); + return norm > 1e-6 + ? new ChassisSpeeds(maxVelocity * (force.x / norm), maxVelocity * (force.y / norm), 0.0) + : new ChassisSpeeds(); + } + + /** + * Applies the the force generated by all obstacles as well as the + * attraction potential of the goal to the provided accumulator, + * based on the robot's current position and goal. + * @param x The robot's current X position on the field. + * @param y The robot's current Y position on the field. + * @param goal The current target field location. + * @param netForce The force accumulator to apply the force to. + */ + private void forceAt(double x, double y, Translation2d goal, NetForce netForce) { + double e_x = goal.getX() - x; + double e_y = goal.getY() - y; + double error = Math.hypot(e_x, e_y); + + if (error < 1e-6) return; + + double targetForce = 1.0 + (2.0 / error); + netForce.x += targetForce * (e_x / error); + netForce.y += targetForce * (e_y / error); + + for (Obstacle obstacle : obstacles) { + obstacle.applyForce(x, y, goal, netForce); + } + } + + /** + * Enables publishing tunables for adjustment of the controller's parameters. + * @param name The parent name for the tunables in NetworkTables. + */ + public void enableTunables(String name) { + Tunable.value(name + "/horizon", horizon, v -> horizon = v); + Tunable.value(name + "/resolution", resolution, v -> resolution = v); + Tunable.value(name + "/tolerance", tolerance, v -> tolerance = v); + Tunable.value(name + "/savePrediction", savePrediction, v -> savePrediction = v); + } + + /** + * Returns a list of samples from the potential field for visualization in + * AdvantageScope. Represented as {@link Pose2d}s, the translation of the + * pose is the sample location, and the rotation is the direction of force + * at the sample. Useful for debugging purposes, but is not recommended for + * regular use due to performance implications. + * @param rows The number of rows (X+ direction) to sample. Columns are sampled + * relative to the ratio of {@code fieldWidth / fieldLength}. + * @param padding The distance past the field perimeter to sample in meters. + * @param fieldLength The length of the field in meters. + * @param fieldWidth The width of the field in meters. + */ + public List visualizeField(int rows, double padding, double fieldLength, double fieldWidth) { + fieldLength += (padding * 2.0); + fieldWidth += (padding * 2.0); + + List field = new ArrayList<>(); + NetForce sample = new NetForce(); + int columns = (int) Math.ceil(rows * (fieldWidth / fieldLength)); + for (double x = -padding; x < fieldLength + 1e-3; x += fieldLength / rows) { + for (double y = -padding; y < fieldWidth + 1e-3; y += fieldWidth / columns) { + sample.reset(); + forceAt(x, y, setpoint.getTranslation(), sample); + if (sample.x * sample.x + sample.y * sample.y > 1e-6) { + field.add(new Pose2d(x, y, new Rotation2d(sample.x, sample.y))); + } + } + } + + return field; + } + + /** + * Represents a sum of forces. + */ + private static final class NetForce { + + // TODO (2027 MRC) + // NetForce exists as an optimization to reduce GC pressure, over + // the alternative of returning immutable container objects. With + // the release of the 2027 MRC, this will not be necessary. + + public double x = 0.0; + public double y = 0.0; + + public void reset() { + x = 0.0; + y = 0.0; + } + } + + /** + * A generic obstacle. + */ + public abstract static class Obstacle { + + private final double strength; + private final boolean repulsive; + + /** + * Creates an obstacle. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public Obstacle(double strength, boolean repulsive) { + this.strength = strength; + this.repulsive = repulsive; + } + + /** + * Applies the force generated by the obstacle to the provided + * accumulator, based on the robot's current position and goal. + * @param x The robot's current X position on the field. + * @param y The robot's current Y position on the field. + * @param goal The current target field location. + * @param netForce The force accumulator to apply the force to. + */ + public abstract void applyForce(double x, double y, Translation2d goal, NetForce netForce); + + /** + * Converts a distance from the obstacle to the strength of the force. + * @param dist The distance from the obstacle. + */ + protected final double getForceMagnitude(double dist) { + double dist_2 = Math.max(1e-6, dist * dist); + return (strength / dist_2) * (repulsive ? 1.0 : -1.0); + } + } + + /** + * A line segment that pushes perpendicular from its length. + */ + public static final class LineObstacle extends Obstacle { + + private final Translation2d start; + private final Translation2d end; + + private final double length; + private final Rotation2d inverse; + private final Rotation2d perpendicular; + + /** + * Creates a line obstacle. + * @param start The start of the line segment. + * @param end The end of the line segment. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LineObstacle(Translation2d start, Translation2d end, double strength, boolean repulsive) { + super(strength, repulsive); + this.start = start; + this.end = end; + + Translation2d difference = end.minus(start); + length = difference.getNorm(); + inverse = difference.getAngle().unaryMinus(); + perpendicular = difference.getAngle().rotateBy(Rotation2d.kCCW_Pi_2); + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double startDist_x = x - start.getX(); + double startDist_y = y - start.getY(); + + double proj_x = startDist_x * inverse.getCos() - startDist_y * inverse.getSin(); + double proj_y = startDist_x * inverse.getSin() + startDist_y * inverse.getCos(); + + if (proj_x > 0.0 && proj_x < length) { + double magnitude = Math.copySign(getForceMagnitude(proj_y), proj_y); + netForce.x += magnitude * perpendicular.getCos(); + netForce.y += magnitude * perpendicular.getSin(); + } else { + Translation2d closest = proj_x <= 0.0 ? start : end; + double d_x = x - closest.getX(); + double d_y = y - closest.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double magnitude = getForceMagnitude(d_norm); + + netForce.x += magnitude * (d_x / d_norm); + netForce.y += magnitude * (d_y / d_norm); + } + } + } + + /** + * A simple point obstacle. Applies a force from the specified field location. + */ + public static final class PointObstacle extends Obstacle { + + private final Translation2d location; + + /** + * Creates a point obstacle. + * @param location The point's location on the field. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public PointObstacle(Translation2d location, double strength, boolean repulsive) { + super(strength, repulsive); + this.location = location; + } + + @Override + public void applyForce(double x, double y, Translation2d goal, NetForce netForce) { + double d_x = x - location.getX(); + double d_y = y - location.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double force = getForceMagnitude(d_norm); + + netForce.x += force * (d_x / d_norm); + netForce.y += force * (d_y / d_norm); + } + } + + /** + * A circle with a specified radius. + */ + public static final class CircleObstacle extends Obstacle { + + private final Translation2d location; + private final double radius; + + /** + * Creates a circle obstacle. + * @param location The location of the circle's center. + * @param radius The radius of the circle. + * @param strength The strength of the obstacle's force. + */ + public CircleObstacle(Translation2d location, double radius, double strength) { + super(strength, true); + this.location = location; + this.radius = radius; + } + + @Override + public void applyForce(double x, double y, Translation2d goal, NetForce netForce) { + double d_x = x - location.getX(); + double d_y = y - location.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double force = getForceMagnitude(Math.max(0.0, d_norm - radius)); + + netForce.x += force * (d_x / d_norm); + netForce.y += force * (d_y / d_norm); + } + } + + /** + * An infinite line that pushes parallel to the X axis, to + * constrain forwards/backwards field-relative movement. + */ + public static final class LongitudinalObstacle extends Obstacle { + + private final double x; + + /** + * Creates a longitudinal obstacle. + * @param x The X coordinate of the line. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LongitudinalObstacle(double x, double strength, boolean repulsive) { + super(strength, repulsive); + this.x = x; + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double difference = x - this.x; + netForce.x += Math.copySign(getForceMagnitude(difference), difference); + } + } + + /** + * An infinite line that pushes parallel to the Y axis, to + * constrain left/right field-relative movement. + */ + public static final class LateralObstacle extends Obstacle { + + private final double y; + + /** + * Creates a lateral obstacle. + * @param y The Y coordinate of the line. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LateralObstacle(double y, double strength, boolean repulsive) { + super(strength, repulsive); + this.y = y; + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double difference = y - this.y; + netForce.y += Math.copySign(getForceMagnitude(difference), difference); + } + } +} diff --git a/src/main/java/org/team340/lib/util/Sleep.java b/src/main/java/org/team340/lib/util/Sleep.java index b0b0227..8b2289d 100644 --- a/src/main/java/org/team340/lib/util/Sleep.java +++ b/src/main/java/org/team340/lib/util/Sleep.java @@ -12,7 +12,7 @@ public final class Sleep { private Sleep() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/Tunable.java b/src/main/java/org/team340/lib/util/Tunable.java index a860576..0a678ce 100644 --- a/src/main/java/org/team340/lib/util/Tunable.java +++ b/src/main/java/org/team340/lib/util/Tunable.java @@ -28,6 +28,7 @@ import java.util.function.Consumer; import java.util.function.DoubleConsumer; import java.util.function.IntConsumer; +import org.team340.lib.logging.LoggedRobot; import org.team340.lib.util.vendors.RevUtil; /** @@ -37,15 +38,15 @@ public final class Tunable { private Tunable() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } - private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Tunables"); + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("/Tunables"); private static final EventLoop pollChanges = new EventLoop(); /** - * Polls changes from NetworkTables. Must be called - * periodically in order for this class to function. + * Polls changes from NetworkTables. Must be called periodically in order for + * this class to function (this is already done if utilizing {@link LoggedRobot}). */ public static void update() { pollChanges.poll(); @@ -56,8 +57,8 @@ public static void update() { * @param name The name for the tunable. Must be unique. * @param defaultValue The default value of the tunable (e.g. a programmed constant). */ - public static TunableBoolean booleanValue(String name, boolean defaultValue) { - return booleanValue(name, defaultValue, null); + public static TunableBoolean value(String name, boolean defaultValue) { + return value(name, defaultValue, null); } /** @@ -66,7 +67,7 @@ public static TunableBoolean booleanValue(String name, boolean defaultValue) { * @param defaultValue The default value of the tunable (e.g. a programmed constant). * @param onChange A consumer that is invoked when the value of the tunable is modified. */ - public static TunableBoolean booleanValue(String name, boolean defaultValue, BooleanConsumer onChange) { + public static TunableBoolean value(String name, boolean defaultValue, BooleanConsumer onChange) { return new TunableBoolean(name, defaultValue, onChange); } @@ -96,7 +97,7 @@ private TunableBoolean(String name, boolean defaultValue, BooleanConsumer onChan /** * Returns the value of the tunable. */ - public boolean value() { + public boolean get() { return value; } @@ -120,8 +121,8 @@ public void close() { * @param name The name for the tunable. Must be unique. * @param defaultValue The default value of the tunable (e.g. a programmed constant). */ - public static TunableInteger intValue(String name, int defaultValue) { - return intValue(name, defaultValue, null); + public static TunableInteger value(String name, int defaultValue) { + return value(name, defaultValue, (IntConsumer) null); } /** @@ -130,7 +131,7 @@ public static TunableInteger intValue(String name, int defaultValue) { * @param defaultValue The default value of the tunable (e.g. a programmed constant). * @param onChange A consumer that is invoked when the value of the tunable is modified. */ - public static TunableInteger intValue(String name, int defaultValue, IntConsumer onChange) { + public static TunableInteger value(String name, int defaultValue, IntConsumer onChange) { return new TunableInteger(name, defaultValue, onChange); } @@ -160,7 +161,7 @@ private TunableInteger(String name, int defaultValue, IntConsumer onChange) { /** * Returns the value of the tunable. */ - public int value() { + public int get() { return value; } @@ -184,8 +185,8 @@ public void close() { * @param name The name for the tunable. Must be unique. * @param defaultValue The default value of the tunable (e.g. a programmed constant). */ - public static TunableFloat floatValue(String name, float defaultValue) { - return floatValue(name, defaultValue, null); + public static TunableFloat value(String name, float defaultValue) { + return value(name, defaultValue, (FloatConsumer) null); } /** @@ -194,7 +195,7 @@ public static TunableFloat floatValue(String name, float defaultValue) { * @param defaultValue The default value of the tunable (e.g. a programmed constant). * @param onChange A consumer that is invoked when the value of the tunable is modified. */ - public static TunableFloat floatValue(String name, float defaultValue, FloatConsumer onChange) { + public static TunableFloat value(String name, float defaultValue, FloatConsumer onChange) { return new TunableFloat(name, defaultValue, onChange); } @@ -224,7 +225,7 @@ private TunableFloat(String name, float defaultValue, FloatConsumer onChange) { /** * Returns the value of the tunable. */ - public float value() { + public float get() { return value; } @@ -248,8 +249,8 @@ public void close() { * @param name The name for the tunable. Must be unique. * @param defaultValue The default value of the tunable (e.g. a programmed constant). */ - public static TunableDouble doubleValue(String name, double defaultValue) { - return doubleValue(name, defaultValue, null); + public static TunableDouble value(String name, double defaultValue) { + return value(name, defaultValue, (DoubleConsumer) null); } /** @@ -258,7 +259,7 @@ public static TunableDouble doubleValue(String name, double defaultValue) { * @param defaultValue The default value of the tunable (e.g. a programmed constant). * @param onChange A consumer that is invoked when the value of the tunable is modified. */ - public static TunableDouble doubleValue(String name, double defaultValue, DoubleConsumer onChange) { + public static TunableDouble value(String name, double defaultValue, DoubleConsumer onChange) { return new TunableDouble(name, defaultValue, onChange); } @@ -288,7 +289,7 @@ private TunableDouble(String name, double defaultValue, DoubleConsumer onChange) /** * Returns the value of the tunable. */ - public double value() { + public double get() { return value; } @@ -312,8 +313,8 @@ public void close() { * @param name The name for the tunable. Must be unique. * @param defaultValue The default value of the tunable (e.g. a programmed constant). */ - public static TunableString stringValue(String name, String defaultValue) { - return stringValue(name, defaultValue, null); + public static TunableString value(String name, String defaultValue) { + return value(name, defaultValue, null); } /** @@ -322,7 +323,7 @@ public static TunableString stringValue(String name, String defaultValue) { * @param defaultValue The default value of the tunable (e.g. a programmed constant). * @param onChange A consumer that is invoked when the value of the tunable is modified. */ - public static TunableString stringValue(String name, String defaultValue, Consumer onChange) { + public static TunableString value(String name, String defaultValue, Consumer onChange) { return new TunableString(name, defaultValue, onChange); } @@ -352,7 +353,7 @@ private TunableString(String name, String defaultValue, Consumer onChang /** * Returns the value of the tunable. */ - public String value() { + public String get() { return value; } @@ -377,10 +378,10 @@ public void close() { * @param controller The PID controller. */ public static void pidController(String name, PIDController controller) { - doubleValue(name + "/kP", controller.getP(), controller::setP); - doubleValue(name + "/kI", controller.getI(), controller::setI); - doubleValue(name + "/kD", controller.getD(), controller::setD); - doubleValue(name + "/iZone", controller.getIZone(), controller::setIZone); + value(name + "/kP", controller.getP(), controller::setP); + value(name + "/kI", controller.getI(), controller::setI); + value(name + "/kD", controller.getD(), controller::setD); + value(name + "/iZone", controller.getIZone(), controller::setIZone); } /** @@ -389,14 +390,14 @@ public static void pidController(String name, PIDController controller) { * @param controller The PID controller. */ public static void pidController(String name, ProfiledPIDController controller) { - doubleValue(name + "/kP", controller.getP(), controller::setP); - doubleValue(name + "/kI", controller.getI(), controller::setI); - doubleValue(name + "/kD", controller.getD(), controller::setD); - doubleValue(name + "/iZone", controller.getIZone(), controller::setIZone); - doubleValue(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> + value(name + "/kP", controller.getP(), controller::setP); + value(name + "/kI", controller.getI(), controller::setI); + value(name + "/kD", controller.getD(), controller::setD); + value(name + "/iZone", controller.getIZone(), controller::setIZone); + value(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> controller.setConstraints(new TrapezoidProfile.Constraints(v, controller.getConstraints().maxAcceleration)) ); - doubleValue(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> + value(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> controller.setConstraints(new TrapezoidProfile.Constraints(controller.getConstraints().maxVelocity, v)) ); } @@ -419,42 +420,42 @@ public static void pidController(String name, SparkMax spark) { public static void pidController(String name, SparkMax spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop; - doubleValue(name + "/kP", config.getP(slot), v -> { + value(name + "/kP", config.getP(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.p(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kI", config.getI(slot), v -> { + value(name + "/kI", config.getI(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.i(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kD", config.getD(slot), v -> { + value(name + "/kD", config.getD(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.d(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kV", config.getFF(slot), v -> { + value(name + "/kV", config.getFF(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.velocityFF(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/iZone", config.getIZone(slot), v -> { + value(name + "/iZone", config.getIZone(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.iZone(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/dFilter", config.getDFilter(slot), v -> { + value(name + "/dFilter", config.getDFilter(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.dFilter(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> { + value(name + "/minOutput", config.getMinOutput(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.minOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> { + value(name + "/maxOutput", config.getMaxOutput(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -479,42 +480,42 @@ public static void pidController(String name, SparkFlex spark) { public static void pidController(String name, SparkFlex spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop; - doubleValue(name + "/kP", config.getP(slot), v -> { + value(name + "/kP", config.getP(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.p(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kI", config.getI(slot), v -> { + value(name + "/kI", config.getI(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.i(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kD", config.getD(slot), v -> { + value(name + "/kD", config.getD(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.d(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kV", config.getFF(slot), v -> { + value(name + "/kV", config.getFF(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.velocityFF(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/iZone", config.getIZone(slot), v -> { + value(name + "/iZone", config.getIZone(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.iZone(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/dFilter", config.getDFilter(slot), v -> { + value(name + "/dFilter", config.getDFilter(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.dFilter(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> { + value(name + "/minOutput", config.getMinOutput(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.minOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> { + value(name + "/maxOutput", config.getMaxOutput(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -531,37 +532,37 @@ public static void pidController(String name, TalonFX talonFX) { Slot0Configs config = new Slot0Configs(); talonFX.getConfigurator().refresh(config); - doubleValue(name + "/kP", config.kP, v -> { + value(name + "/kP", config.kP, v -> { talonFX.getConfigurator().refresh(config); config.kP = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kI", config.kI, v -> { + value(name + "/kI", config.kI, v -> { talonFX.getConfigurator().refresh(config); config.kI = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kD", config.kD, v -> { + value(name + "/kD", config.kD, v -> { talonFX.getConfigurator().refresh(config); config.kD = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kS", config.kS, v -> { + value(name + "/kS", config.kS, v -> { talonFX.getConfigurator().refresh(config); config.kS = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kV", config.kV, v -> { + value(name + "/kV", config.kV, v -> { talonFX.getConfigurator().refresh(config); config.kV = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kA", config.kA, v -> { + value(name + "/kA", config.kA, v -> { talonFX.getConfigurator().refresh(config); config.kA = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kG", config.kG, v -> { + value(name + "/kG", config.kG, v -> { talonFX.getConfigurator().refresh(config); config.kG = v; talonFX.getConfigurator().apply(config); @@ -586,17 +587,17 @@ public static void motionProfile(String name, SparkMax spark) { public static void motionProfile(String name, SparkMax spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop.maxMotion; - doubleValue(name + "/velocity", config.getMaxVelocity(slot), v -> { + value(name + "/velocity", config.getMaxVelocity(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.maxVelocity(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/acceleration", config.getMaxAcceleration(slot), v -> { + value(name + "/acceleration", config.getMaxAcceleration(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.maxAcceleration(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { + value(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.allowedClosedLoopError(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -621,17 +622,17 @@ public static void motionProfile(String name, SparkFlex spark) { public static void motionProfile(String name, SparkFlex spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop.maxMotion; - doubleValue(name + "/velocity", config.getMaxVelocity(slot), v -> { + value(name + "/velocity", config.getMaxVelocity(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.maxVelocity(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/acceleration", config.getMaxAcceleration(slot), v -> { + value(name + "/acceleration", config.getMaxAcceleration(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.maxAcceleration(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { + value(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.allowedClosedLoopError(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -647,27 +648,27 @@ public static void motionProfile(String name, TalonFX talonFX) { MotionMagicConfigs config = new MotionMagicConfigs(); talonFX.getConfigurator().refresh(config); - doubleValue(name + "/velocity", config.MotionMagicCruiseVelocity, v -> { + value(name + "/velocity", config.MotionMagicCruiseVelocity, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicCruiseVelocity = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/acceleration", config.MotionMagicAcceleration, v -> { + value(name + "/acceleration", config.MotionMagicAcceleration, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicAcceleration = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/jerk", config.MotionMagicJerk, v -> { + value(name + "/jerk", config.MotionMagicJerk, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicJerk = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/expoKv", config.MotionMagicExpo_kV, v -> { + value(name + "/expoKv", config.MotionMagicExpo_kV, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicExpo_kV = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/expoKa", config.MotionMagicExpo_kA, v -> { + value(name + "/expoKa", config.MotionMagicExpo_kA, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicExpo_kA = v; talonFX.getConfigurator().apply(config); @@ -680,6 +681,6 @@ public static void motionProfile(String name, TalonFX talonFX) { * @param debouncer The debouncer to tune. */ public static void debounce(String name, Debouncer debouncer) { - doubleValue(name, debouncer.getDebounceTime(), debouncer::setDebounceTime); + value(name, debouncer.getDebounceTime(), debouncer::setDebounceTime); } } diff --git a/src/main/java/org/team340/lib/util/command/AutoChooser.java b/src/main/java/org/team340/lib/util/command/AutoChooser.java new file mode 100644 index 0000000..c438050 --- /dev/null +++ b/src/main/java/org/team340/lib/util/command/AutoChooser.java @@ -0,0 +1,121 @@ +package org.team340.lib.util.command; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.HashMap; +import java.util.Map; + +/** + * Similar to {@link SendableChooser}, this class implements a selector over + * NetworkTables for choosing a command to run during the autonomous period. + * + *

The AutoChooser binds itself to the default {@link CommandScheduler} + * instance, and will automatically schedule the selected command at the + * beginning of autonomous. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class AutoChooser { + + private static final String DEFAULT = "Do Nothing"; + + private final StringArrayPublisher optionsPub; + private final StringPublisher activePub; + private final StringSubscriber selectedSub; + + private final Map options = new HashMap<>(); + private final boolean renameCommands; + + private String activeName = DEFAULT; + private Command activeCommand = Commands.none(); + private boolean newSelection = false; + + /** + * Creates an auto chooser. + */ + public AutoChooser() { + this("Autos", true); + } + + /** + * Creates an auto chooser. + * @param name The name for the chooser in NetworkTables. + * @param renameCommands If the specified names of autonomous options should + * be applied to their corresponding commands when + * adding them to the chooser. + */ + public AutoChooser(String name, boolean renameCommands) { + this.renameCommands = renameCommands; + + NetworkTable nt = NetworkTableInstance.getDefault().getTable(name); + + nt.getStringTopic(".type").publish().set("String Chooser"); + nt.getBooleanTopic(".controllable").publish().set(true); + nt.getStringTopic("default").publish().set(DEFAULT); + + optionsPub = nt.getStringArrayTopic("options").publish(); + activePub = nt.getStringTopic("active").publish(); + selectedSub = nt.getStringTopic("selected").subscribe(DEFAULT); + + add(activeName, activeCommand); + activePub.set(activeName); + + CommandScheduler.getInstance().getDefaultButtonLoop().bind(this::update); + RobotModeTriggers.autonomous().whileTrue(Commands.deferredProxy(() -> activeCommand)); + } + + /** + * Returns the command of the currently selected auto. + */ + public Command getSelected() { + return activeCommand; + } + + /** + * Returns a {@link Trigger} that rises {@code true} + * for one cycle after an auto is selected. + */ + public Trigger newSelection() { + return new Trigger(() -> newSelection); + } + + /** + * Add an option to the chooser. + * @param name The name of the option. Must be unique. + * @param command The option's command. + * @return A {@link Trigger} that is {@code true} when the option is selected. + */ + public Trigger add(String name, Command command) { + if (renameCommands) command.setName(name); + + options.put(name, command); + optionsPub.set(options.keySet().toArray(String[]::new)); + return new Trigger(() -> activeName.equals(name)); + } + + /** + * Updates the auto chooser. You do not need to call this method + * when using {@link AutoChooser#bind(CommandScheduler)}. + */ + private void update() { + String selected = selectedSub.get(); + if (!selected.equals(activeName)) { + activeName = options.containsKey(selected) ? selected : DEFAULT; + activeCommand = options.get(activeName); + activePub.set(activeName); + newSelection = true; + } else { + newSelection = false; + } + } +} diff --git a/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java b/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java index 3e490c8..3407fa9 100644 --- a/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java @@ -10,7 +10,7 @@ public final class PhoenixUtil { private PhoenixUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java index 503e877..62ae6cc 100644 --- a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java @@ -10,6 +10,7 @@ import com.reduxrobotics.sensors.canandmag.CanandmagSettings; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import java.util.Map; @@ -19,7 +20,7 @@ public final class ReduxUtil { private ReduxUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -41,10 +42,11 @@ public static boolean applySettings(Canandmag canandmag, CanandmagSettings setti * @return {@code true} if success, {@code false} otherwise. */ public static boolean applySettings(Canandmag canandmag, CanandmagSettings settings, int maxTries) { + if (RobotBase.isSimulation()) return true; + String results = ""; for (int i = 0; i < maxTries; i++) { - boolean result = canandmag.setSettings(settings, 0.05); - if (!result) { + if (!canandmag.setSettings(settings, 0.05)) { results += (results.isEmpty() ? "" : ", ") + "setSettings() Failure"; continue; } @@ -88,10 +90,11 @@ public static boolean applySettings(Canandgyro canandgyro, CanandgyroSettings se * @return {@code true} if success, {@code false} otherwise. */ public static boolean applySettings(Canandgyro canandgyro, CanandgyroSettings settings, int maxTries) { + if (RobotBase.isSimulation()) return true; + String results = ""; for (int i = 0; i < maxTries; i++) { - boolean result = canandgyro.setSettings(settings, 0.05); - if (!result) { + if (!canandgyro.setSettings(settings, 0.05)) { results += (results.isEmpty() ? "" : ", ") + "setSettings() Failure"; continue; } diff --git a/src/main/java/org/team340/lib/util/vendors/RevUtil.java b/src/main/java/org/team340/lib/util/vendors/RevUtil.java index 37902a9..a460de5 100644 --- a/src/main/java/org/team340/lib/util/vendors/RevUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/RevUtil.java @@ -17,7 +17,7 @@ public final class RevUtil { private RevUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 98001b4..742cbd3 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -7,10 +7,11 @@ */ public final class Constants { - public static final double kVoltage = 12.0; + public static final double VOLTAGE = 12.0; - public static final int kDriver = 0; - public static final int kCoDriver = 1; + // Controller ports + public static final int DRIVER = 0; + public static final int CO_DRIVER = 1; /** * The RobotMap class defines CAN IDs, CAN bus names, DIO/PWM/PH/PCM channel @@ -18,22 +19,22 @@ public final class Constants { */ public static final class RobotMap { - public static final String kLowerCANBus = "LowerCAN"; + public static final String LOWER_CAN = "LowerCAN"; - public static final int kFlMove = 2; - public static final int kFlTurn = 3; - public static final int kFrMove = 4; - public static final int kFrTurn = 5; - public static final int kBlMove = 6; - public static final int kBlTurn = 7; - public static final int kBrMove = 8; - public static final int kBrTurn = 9; + public static final int FL_MOVE = 2; + public static final int FL_TURN = 3; + public static final int FR_MOVE = 4; + public static final int FR_TURN = 5; + public static final int BL_MOVE = 6; + public static final int BL_TURN = 7; + public static final int BR_MOVE = 8; + public static final int BR_TURN = 9; - public static final int kFlEncoder = 10; - public static final int kFrEncoder = 11; - public static final int kBlEncoder = 12; - public static final int kBrEncoder = 13; + public static final int FL_ENCODER = 10; + public static final int FR_ENCODER = 11; + public static final int BL_ENCODER = 12; + public static final int BR_ENCODER = 13; - public static final int kCanandgyro = 14; + public static final int CANANDGYRO = 14; } } diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index 8d18d93..f3a3a83 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -2,28 +2,22 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import com.ctre.phoenix6.SignalLogger; -import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; -import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import org.team340.lib.logging.LoggedRobot; +import org.team340.lib.logging.Profiler; import org.team340.lib.util.DisableWatchdog; -import org.team340.lib.util.Profiler; -import org.team340.lib.util.Tunable; import org.team340.robot.commands.Autos; import org.team340.robot.commands.Routines; import org.team340.robot.subsystems.Swerve; @Logged -public final class Robot extends TimedRobot { +public final class Robot extends LoggedRobot { - private final CommandScheduler scheduler = CommandScheduler.getInstance(); + public final CommandScheduler scheduler = CommandScheduler.getInstance(); public final Swerve swerve; @@ -34,16 +28,6 @@ public final class Robot extends TimedRobot { private final CommandXboxController coDriver; public Robot() { - DriverStation.silenceJoystickConnectionWarning(true); - DisableWatchdog.in(scheduler, "m_watchdog"); - DisableWatchdog.in(this, "m_watchdog"); - - // Configure logging - DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog()); - SignalLogger.enableAutoLogging(false); - Epilogue.getConfig().root = "/Telemetry"; - // Initialize subsystems swerve = new Swerve(); @@ -52,23 +36,22 @@ public Robot() { autos = new Autos(this); // Initialize controllers - driver = new CommandXboxController(Constants.kDriver); - coDriver = new CommandXboxController(Constants.kCoDriver); + driver = new CommandXboxController(Constants.DRIVER); + coDriver = new CommandXboxController(Constants.CO_DRIVER); - // Create triggers - RobotModeTriggers.autonomous().whileTrue(autos.runSelectedAuto()); + // Set default commands + swerve.setDefaultCommand(swerve.drive(this::driverX, this::driverY, this::driverAngular)); // Driver bindings + driver.a().onTrue(none()); driver.povLeft().onTrue(swerve.tareRotation()); // Co-driver bindings coDriver.a().onTrue(none()); - // Set thread priority - waitSeconds(10.0) - .until(DriverStation::isEnabled) - .andThen(() -> Threads.setCurrentThreadPriority(true, 10)) - .schedule(); + // Disable loop overrun warnings from the command + // scheduler, since we already log loop timings + DisableWatchdog.in(scheduler, "m_watchdog"); } /** @@ -95,25 +78,6 @@ public double driverAngular() { @Override public void robotPeriodic() { - Profiler.start("robotPeriodic"); Profiler.run("scheduler", scheduler::run); - Profiler.run("epilogue", () -> Epilogue.update(this)); - Profiler.run("tunables", Tunable::update); - Profiler.end(); } - - @Override - public void simulationPeriodic() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopPeriodic() {} - - @Override - public void testPeriodic() {} } diff --git a/src/main/java/org/team340/robot/commands/Autos.java b/src/main/java/org/team340/robot/commands/Autos.java index 755f677..483fd28 100644 --- a/src/main/java/org/team340/robot/commands/Autos.java +++ b/src/main/java/org/team340/robot/commands/Autos.java @@ -2,15 +2,15 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import choreo.auto.AutoChooser; -import choreo.auto.AutoChooser; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import org.team340.lib.util.FieldFlip; +import org.team340.lib.util.Tunable; +import org.team340.lib.util.Tunable.TunableDouble; +import org.team340.lib.util.command.AutoChooser; import org.team340.robot.Robot; import org.team340.robot.subsystems.Swerve; @@ -22,38 +22,35 @@ @Logged(strategy = Strategy.OPT_IN) public final class Autos { + private static final TunableDouble deceleration = Tunable.value("autos/deceleration", 6.0); + private static final TunableDouble tolerance = Tunable.value("autos/tolerance", 0.05); + private final Swerve swerve; private final Routines routines; - private final AutoFactory factory; private final AutoChooser chooser; public Autos(Robot robot) { swerve = robot.swerve; routines = robot.routines; - // Create the auto factory - factory = new AutoFactory(swerve::getPose, swerve::resetPose, swerve::followTrajectory, true, swerve); + // Create the auto chooser chooser = new AutoChooser(); // Add autonomous modes to the dashboard - chooser.addRoutine("Example", this::example); - SmartDashboard.putData("autos", chooser); - } - - /** - * Returns a command that when scheduled will run the currently selected auto. - */ - public Command runSelectedAuto() { - return chooser.selectedCommandScheduler(); + chooser.add("Example", example()); } - private AutoRoutine example() { - AutoRoutine routine = factory.newRoutine("Example"); - AutoTrajectory exampleTraj = routine.trajectory("ExampleTrajectory"); - - routine.active().onTrue(sequence(exampleTraj.resetOdometry(), exampleTraj.spawnCmd())); - - return routine; + private Command example() { + Pose2d start = new Pose2d(1.0, 1.0, Rotation2d.kZero); + Pose2d firstGoal = new Pose2d(2.0, 5.0, Rotation2d.k180deg); + Pose2d secondGoal = new Pose2d(6.0, 2.5, Rotation2d.kCW_90deg); + + return sequence( + swerve.resetPose(FieldFlip.allianceDiagonal(start)), + swerve.apfDrive(FieldFlip.allianceDiagonal(firstGoal), deceleration::get, tolerance::get), + swerve.apfDrive(FieldFlip.allianceDiagonal(secondGoal), deceleration::get, tolerance::get), + swerve.stop(false) + ); } } diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java index 7eea19a..f97f641 100644 --- a/src/main/java/org/team340/robot/commands/Routines.java +++ b/src/main/java/org/team340/robot/commands/Routines.java @@ -9,8 +9,8 @@ import org.team340.robot.subsystems.Swerve; /** - * The Routines class contains command compositions, such as sequences - * or parallel command groups, that require multiple subsystems. + * The Routines class contains command compositions that require + * multiple subsystems, such as sequences or parallel command groups. */ @Logged(strategy = Strategy.OPT_IN) public final class Routines { diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index 4bc821d..4905224 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -1,17 +1,15 @@ package org.team340.robot.subsystems; -import choreo.auto.AutoFactory; -import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.CANBus; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; -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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.team340.lib.swerve.Perspective; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.SwerveState; @@ -20,6 +18,9 @@ import org.team340.lib.swerve.hardware.SwerveEncoders; import org.team340.lib.swerve.hardware.SwerveIMUs; import org.team340.lib.swerve.hardware.SwerveMotors; +import org.team340.lib.util.Math2; +import org.team340.lib.util.PAPFController; +import org.team340.lib.util.PAPFController.Obstacle; import org.team340.lib.util.Tunable; import org.team340.lib.util.command.GRRSubsystem; import org.team340.robot.Constants; @@ -31,39 +32,37 @@ @Logged public final class Swerve extends GRRSubsystem { - private static final double kMoveRatio = (54.0 / 10.0) * (18.0 / 38.0) * (45.0 / 15.0); - private static final double kTurnRatio = (22.0 / 10.0) * (88.0 / 16.0); - private static final double kModuleOffset = Units.inchesToMeters(12.5); + private static final double OFFSET = Units.inchesToMeters(12.5); - private static final SwerveModuleConfig kFrontLeft = new SwerveModuleConfig() + private final SwerveModuleConfig frontLeft = new SwerveModuleConfig() .setName("frontLeft") - .setLocation(kModuleOffset, kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFlMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFlTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kFlEncoder, 0.0, false)); + .setLocation(OFFSET, OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.FL_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.FL_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.FL_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kFrontRight = new SwerveModuleConfig() + private final SwerveModuleConfig frontRight = new SwerveModuleConfig() .setName("frontRight") - .setLocation(kModuleOffset, -kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFrMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFrTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kFrEncoder, 0.0, false)); + .setLocation(OFFSET, -OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.FR_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.FR_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.FR_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kBackLeft = new SwerveModuleConfig() + private final SwerveModuleConfig backLeft = new SwerveModuleConfig() .setName("backLeft") - .setLocation(-kModuleOffset, kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBlMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBlTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kBlEncoder, 0.0, false)); + .setLocation(-OFFSET, OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.BL_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.BL_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.BL_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kBackRight = new SwerveModuleConfig() + private final SwerveModuleConfig backRight = new SwerveModuleConfig() .setName("backRight") - .setLocation(-kModuleOffset, -kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBrMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBrTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kBrEncoder, 0.0, false)); + .setLocation(-OFFSET, -OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.BR_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.BR_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.BR_ENCODER, 0.0, false)); - private static final SwerveConfig kConfig = new SwerveConfig() + private final SwerveConfig config = new SwerveConfig() .setTimings(TimedRobot.kDefaultPeriod, 0.004, 0.02, 0.01) .setMovePID(0.25, 0.0, 0.0) .setMoveFF(0.0, 0.125) @@ -71,38 +70,30 @@ public final class Swerve extends GRRSubsystem { .setBrakeMode(false, true) .setLimits(4.5, 0.05, 17.5, 14.0, 30.0) .setDriverProfile(4.0, 1.5, 0.15, 4.75, 2.0, 0.05) - .setPowerProperties(Constants.kVoltage, 100.0, 80.0, 60.0, 60.0) - .setMechanicalProperties(kMoveRatio, kTurnRatio, 0.0, Units.inchesToMeters(4.0)) + .setPowerProperties(Constants.VOLTAGE, 100.0, 80.0, 60.0, 60.0) + .setMechanicalProperties(729.0 / 95.0, 12.1, 0.0, Units.inchesToMeters(4.0)) .setOdometryStd(0.1, 0.1, 0.1) - .setIMU(SwerveIMUs.canandgyro(RobotMap.kCanandgyro)) - .setPhoenixFeatures(new CANBus(RobotMap.kLowerCANBus), true, true, true) - .setModules(kFrontLeft, kFrontRight, kBackLeft, kBackRight); + .setIMU(SwerveIMUs.canandgyro(RobotMap.CANANDGYRO)) + .setPhoenixFeatures(new CANBus(RobotMap.LOWER_CAN), true, true, true) + .setModules(frontLeft, frontRight, backLeft, backRight); private final SwerveAPI api; private final SwerveState state; - private final PIDController autoPIDx; - private final PIDController autoPIDy; - private final PIDController autoPIDangular; - - @SuppressWarnings("unused") - private Pose2d autoLast = null; - - private Pose2d autoNext = null; + private final PAPFController apf; + private final ProfiledPIDController angularPID; public Swerve() { - api = new SwerveAPI(kConfig); + api = new SwerveAPI(config); state = api.state; - autoPIDx = new PIDController(10.0, 0.0, 0.0); - autoPIDy = new PIDController(10.0, 0.0, 0.0); - autoPIDangular = new PIDController(10.0, 0.0, 0.0); - autoPIDangular.enableContinuousInput(-Math.PI, Math.PI); + apf = new PAPFController(4.0, 0.5, 0.01, true, new Obstacle[0]); + angularPID = new ProfiledPIDController(10.0, 0.0, 0.0, new Constraints(10.0, 25.0)); + angularPID.enableContinuousInput(-Math.PI, Math.PI); api.enableTunables("swerve/api"); - Tunable.pidController("swerve/autoPID", autoPIDx); - Tunable.pidController("swerve/autoPID", autoPIDy); - Tunable.pidController("swerve/autoPIDangular", autoPIDangular); + apf.enableTunables("swerve/apf"); + Tunable.pidController("swerve/angularPID", angularPID); } @Override @@ -110,21 +101,24 @@ public void periodic() { api.refresh(); } - /** - * Returns the current blue origin relative pose of the robot. - */ - @NotLogged - public Pose2d getPose() { - return state.pose; - } - /** * Tares the rotation of the robot. Useful for * fixing an out of sync or drifting IMU. */ public Command tareRotation() { return commandBuilder("Swerve.tareRotation()") - .onInitialize(() -> api.tareRotation(Perspective.kOperator)) + .onInitialize(() -> api.tareRotation(Perspective.OPERATOR)) + .isFinished(true) + .ignoringDisable(true); + } + + /** + * Resets the pose of the robot, inherently seeding field-relative movement. + * @param pose A supplier that returns the new blue origin relative pose to apply to the pose estimator. + */ + public Command resetPose(Supplier pose) { + return commandBuilder("Swerve.resetPose()") + .onInitialize(() -> api.resetPose(pose.get())) .isFinished(true) .ignoringDisable(true); } @@ -141,7 +135,7 @@ public Command drive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier angular) x.getAsDouble(), y.getAsDouble(), angular.getAsDouble(), - Perspective.kOperator, + Perspective.OPERATOR, true, true ) @@ -149,45 +143,49 @@ public Command drive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier angular) } /** - * Drives the modules to stop the robot from moving. - * @param lock If the wheels should be driven to an X formation to stop the robot from being pushed. + * Drives the robot to a target position using the P-APF, until the + * robot is positioned within a specified tolerance of the target. + * @param goal A supplier that returns the target blue-origin relative field location. + * @param maxDeceleration A supplier that returns the desired deceleration rate of the robot, in m/s/s. + * @param endTolerance The tolerance in meters at which to end the command. */ - public Command stop(boolean lock) { - return commandBuilder("Swerve.stop(" + lock + ")").onExecute(() -> api.applyStop(lock)); + public Command apfDrive(Supplier goal, DoubleSupplier maxDeceleration, DoubleSupplier endTolerance) { + return apfDrive(goal, maxDeceleration) + .until(() -> Math2.isNear(goal.get().getTranslation(), state.translation, endTolerance.getAsDouble())) + .withName("Swerve.apfDrive()"); } /** - * Resets the pose of the robot, inherently seeding field-relative movement. This - * method is not intended for use outside of creating an {@link AutoFactory}. - * @param pose The new blue origin relative pose to apply to the pose estimator. + * Drives the robot to a target position using the P-APF. This command does not end. + * @param goal A supplier that returns the target blue-origin relative field location. + * @param maxDeceleration A supplier that returns the desired deceleration rate of the robot, in m/s/s. */ - public void resetPose(Pose2d pose) { - api.resetPose(pose); - - autoPIDx.reset(); - autoPIDy.reset(); - autoPIDangular.reset(); + public Command apfDrive(Supplier goal, DoubleSupplier maxDeceleration) { + return commandBuilder("Swerve.apfDrive()") + .onInitialize(() -> angularPID.reset(state.rotation.getRadians(), state.speeds.omegaRadiansPerSecond)) + .onExecute(() -> { + Pose2d next = goal.get(); + var speeds = apf.calculate( + state.pose, + next.getTranslation(), + config.velocity, + maxDeceleration.getAsDouble() + ); + + speeds.omegaRadiansPerSecond = angularPID.calculate( + state.rotation.getRadians(), + next.getRotation().getRadians() + ); + + api.applySpeeds(speeds, Perspective.BLUE, true, true); + }); } /** - * Follows a Choreo trajectory by moving towards the next sample. This method - * is not intended for use outside of creating an {@link AutoFactory}. - * @param sample The next trajectory sample. + * Drives the modules to stop the robot from moving. + * @param lock If the wheels should be driven to an X formation to stop the robot from being pushed. */ - public void followTrajectory(SwerveSample sample) { - autoLast = autoNext; - autoNext = sample.getPose(); - - Pose2d pose = state.pose; - api.applySpeeds( - new ChassisSpeeds( - sample.vx + autoPIDx.calculate(pose.getX(), sample.x), - sample.vy + autoPIDy.calculate(pose.getY(), sample.y), - sample.omega + autoPIDangular.calculate(pose.getRotation().getRadians(), sample.heading) - ), - Perspective.kBlue, - true, - false - ); + public Command stop(boolean lock) { + return commandBuilder("Swerve.stop(" + lock + ")").onExecute(() -> api.applyStop(lock)); } } diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json deleted file mode 100644 index 5a8cd54..0000000 --- a/vendordeps/ChoreoLib-2025.0.3.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "fileName": "ChoreoLib-2025.0.3.json", - "name": "ChoreoLib", - "version": "2025.0.3", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://lib.choreo.autos/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.3" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.3", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index d7bea57..6f40c84 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +446,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -444,6 +458,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 2d7b1d8..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "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": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.3.1" - } - ] -} \ No newline at end of file