diff --git a/.gitattributes b/.gitattributes index eef2add..94f480d 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,2 +1 @@ -*.java text eol=lf -*.gradle text eol=lf \ No newline at end of file +* text=auto eol=lf \ No newline at end of file diff --git a/.gitignore b/.gitignore index 0fcb4ad..eb0a18d 100644 --- a/.gitignore +++ b/.gitignore @@ -161,5 +161,30 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file +networktables.json +simgui.json *-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 6956b19..18860c0 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "currentLanguage": "java", "enableCppIntellisense": false, - "projectYear": "2024", + "currentLanguage": "java", + "projectYear": "2025", "teamNumber": 2508 } \ No newline at end of file diff --git a/LICENSE.txt b/LICENSE.txt index f80ae6d..9aaae25 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,6 +1,6 @@ MIT License -Copyright (c) [2024] [Armada 2508] +Copyright (c) [2024-2025] [Armada 2508] Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/build.gradle b/build.gradle index 9ef4ecc..6aecb85 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.2.1" id 'com.diffplug.spotless' version '6.25.0' } @@ -14,6 +14,7 @@ repositories { } dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -23,6 +24,7 @@ dependencies { test { useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' ignoreFailures = true // Don't fail builds or deploys because of failing tests } @@ -53,3 +55,8 @@ spotless { endWithNewline() } } + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 7f93135..a4b76b9 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67..34bd9ce 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a4..f5feea6 100644 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59..9d21a21 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/src/main/java/frc/robot/lib/logging/LogUtil.java b/src/main/java/frc/robot/lib/logging/LogUtil.java index 896c4ae..2b89971 100644 --- a/src/main/java/frc/robot/lib/logging/LogUtil.java +++ b/src/main/java/frc/robot/lib/logging/LogUtil.java @@ -1,12 +1,8 @@ package frc.robot.lib.logging; -import java.lang.reflect.Field; import java.text.SimpleDateFormat; -import java.util.ArrayList; import java.util.Date; -import java.util.List; import java.util.Locale; -import java.util.Map; import java.util.Optional; import edu.wpi.first.networktables.BooleanSubscriber; @@ -14,12 +10,13 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.DoubleTopic; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.TimedRobot; 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.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WrapperCommand; public class LogUtil { @@ -84,44 +81,43 @@ public static BooleanSubscriber getTunableBoolean(String name, boolean defaultVa return topic.subscribe(defaultValue); } - static Command getSequentialCommandCurrentCommand(SequentialCommandGroup command) { - try { - final Field fieldIndex = SequentialCommandGroup.class.getDeclaredField("m_currentCommandIndex"); - fieldIndex.setAccessible(true); - final Field fieldCommands = SequentialCommandGroup.class.getDeclaredField("m_commands"); - fieldCommands.setAccessible(true); - @SuppressWarnings("unchecked") - List list = (List) fieldCommands.get(command); - return list.get(fieldIndex.getInt(command)); - } catch (NoSuchFieldException | SecurityException | IllegalArgumentException | IllegalAccessException e) { - return Commands.none(); - } - } - - static List getParallelCommandCurrentCommands(ParallelCommandGroup command) { - try { - List list = new ArrayList<>(); - final Field fieldCommands = ParallelCommandGroup.class.getDeclaredField("m_commands"); - fieldCommands.setAccessible(true); - @SuppressWarnings("unchecked") - Map map = (Map) fieldCommands.get(command); - map.forEach((cmd, running) -> { - if (running) list.add(cmd); - }); - return list; - } catch (NoSuchFieldException | SecurityException | IllegalArgumentException | IllegalAccessException e) { - return List.of(Commands.none()); - } + /** + * Logs Driver Station data to NetworkTables. Adds a periodic callback to the given robot. + * @param robot The robot to add the callback to + * Should try to get some of these into DriverStation's FMSInfo or DS datalog + */ + public static void logDriverStation(TimedRobot robot) { + robot.addPeriodic(() -> { + String mode = "Unknown"; + if (DriverStation.isTeleop()) { + mode = "Teleop"; + } + else if (DriverStation.isAutonomous()) { + mode = "Autonomous"; + } + else if (DriverStation.isTest()) { + mode = "Test"; + } + var table = NetworkTableInstance.getDefault().getTable("Driver Station"); + table.getEntry("DS Mode").setString(mode); + table.getEntry("Robot Enabled").setBoolean(DriverStation.isEnabled()); + table.getEntry("Match Time").setDouble(DriverStation.getMatchTime()); + table.getEntry("is FMS Attached").setBoolean(DriverStation.isFMSAttached()); + }, TimedRobot.kDefaultPeriod); } - static Command getWrapperCommandInner(WrapperCommand command) { - try { - final Field cmd = WrapperCommand.class.getDeclaredField("m_command"); - cmd.setAccessible(true); - return (Command) cmd.get(command); - } catch (NoSuchFieldException | SecurityException | IllegalArgumentException | IllegalAccessException e) { - return Commands.none(); - } + /** + * Logs command interrupts to NetworkTables and DataLog. Does not have to be called periodically. + */ + public static void logCommandInterrupts(DataLog log) { + CommandScheduler.getInstance().onCommandInterrupt((interruptedCommand, interrupter) -> { + Command interruptingCommand = interrupter.orElseGet(Commands::none); + var commandInterrupt = new StringLogEntry(log, "/Command Scheduler"); + var table = NetworkTableInstance.getDefault().getTable("Command Scheduler"); + commandInterrupt.append("Command: " + interruptedCommand.getName() + " was interrupted by " + interruptingCommand.getName() + "."); + table.getEntry("Last Interrupted Command").setString(interruptedCommand.getName()); + table.getEntry("Last Interrupting Command").setString(interruptingCommand.getName()); + }); } } diff --git a/src/main/java/frc/robot/lib/logging/Loggable.java b/src/main/java/frc/robot/lib/logging/Loggable.java deleted file mode 100644 index 3bbb042..0000000 --- a/src/main/java/frc/robot/lib/logging/Loggable.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.lib.logging; - -import java.util.Map; - -public interface Loggable { - - Map log(Map map); - -} diff --git a/src/main/java/frc/robot/lib/logging/NTLogger.java b/src/main/java/frc/robot/lib/logging/NTLogger.java deleted file mode 100644 index a23bf56..0000000 --- a/src/main/java/frc/robot/lib/logging/NTLogger.java +++ /dev/null @@ -1,318 +0,0 @@ -package frc.robot.lib.logging; - -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Optional; -import java.util.stream.Collectors; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.geometry.Twist3d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.ProtobufPublisher; -import edu.wpi.first.networktables.ProtobufTopic; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.networktables.StructTopic; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.DriverStation; -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.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.WrapperCommand; -import us.hebi.quickbuf.ProtoMessage; - -/** - * Used to log fields to network tables and data log for viewing during testing and after a match.

- * You must register your object using {@link NTLogger#register(Loggable obj)} for its values to be - * put onto network tables when running {@link NTLogger#log()}.

- * {@link NTLogger#log()} should be called in Robot Periodic. - * @author WispySparks - * - */ -public final class NTLogger { - - private static NetworkTable mainTable = NetworkTableInstance.getDefault().getTable("Logging"); - private static NetworkTable schedulerTable = mainTable.getSubTable("Command Scheduler"); - private static Map indexedLoggables = new HashMap<>(); - private static Map loggingMap = new HashMap<>(); - private static List> structPublishers = new ArrayList<>(); - private static List> protobufPublishers = new ArrayList<>(); - - /** - * Prevent this class from being instantiated. - */ - private NTLogger() {} - - /** - * Convenience method to start the data log manager, log driver station, joystick data and command interrupts. - */ - public static void initDataLogger() { - DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog()); - CommandScheduler.getInstance().onCommandInterrupt((interruptedCommand, interrupter) -> { - Command interruptingCommand = interrupter.orElseGet(Commands::none); - DataLogManager.log("Command: " + interruptedCommand.getName() + " was interrupted by " + interruptingCommand.getName() + "."); - schedulerTable.getEntry("Last Interrupted Command").setString(interruptedCommand.getName()); - schedulerTable.getEntry("Last Interrupting Command").setString(interruptedCommand.getName()); - }); - } - - /** - * Registers an object to the logger who's 'log' method will be called. - * @param obj to register - */ - public static void register(Loggable obj) { - int index = indexedLoggables.values() - .stream() - .filter((value) -> value.getClass().equals(obj.getClass())) - .collect(Collectors.toList()) - .size(); - indexedLoggables.put(obj, index); - } - - /** - * Call this in robot periodic to log all registered objects, extra driver station data - * and command interrupts to network tables. - */ - public static void logEverything() { - logDriverStation(); - indexedLoggables.forEach((loggable, index) -> { - NetworkTable table = getLoggablesTable(loggable, index); - loggingMap.clear(); - loggable.log(loggingMap).forEach((name, val) -> logValue(table, name, val)); - }); - } - - /** - * Logs an invidual value to the loggable's network table. Useful for logging one off values inside of methods. - * @param loggable used to find the right network table - * @param name for value - * @param val to log - */ - public static void log(Loggable loggable, String name, Object val) { - int index = indexedLoggables.get(loggable); - NetworkTable table = getLoggablesTable(loggable, index); - logValue(table, name, val); - } - - /** - * Gets a loggable's network table to log to - * @param loggable to get network table for - * @param index of loggable, counts up for every instance - * @return loggable's network table - */ - private static NetworkTable getLoggablesTable(Loggable loggable, int index) { - return (index == 0) ? mainTable.getSubTable(loggable.getClass().getSimpleName()) : - mainTable.getSubTable(loggable.getClass().getSimpleName() + "-" + index); - } - - /** - * Logs a value into a network table, correctly logs structs and protobufs. - * If it's not a supported type it just calls {@link Object#toString()}. - * @param table to log to - * @param name for value - * @param val to log - */ - private static void logValue(NetworkTable table, String name, Object val) { - if (name == null || val == null) return; - Optional> struct = getStruct(val); - Optional>> protobuf = getProtobuf(val); - if (struct.isPresent()) { - logStruct(table, name, val, struct.get()); - return; - } - if (protobuf.isPresent()) { - logProtobuf(table, name, val, protobuf.get()); - return; - } - NetworkTableEntry entry = table.getEntry(name); - try { - entry.setValue(val); - } catch (IllegalArgumentException e) { - entry.setString(val.toString()); - } - } - - /** - * Fills your map with TalonFX StatusSignals. - * @param talon to log - * @param name of talon for logging - * @param map to fill - * @return the map passed in for method chaining - */ - public static Map putTalonLog(TalonFX talon, String name, Map map) { - map.put(name + ": Device ID", talon.getDeviceID()); - map.put(name + ": Control Mode", talon.getControlMode().getValue().toString()); - map.put(name + ": Rotor Polarity", talon.getAppliedRotorPolarity().getValue().name()); - map.put(name + ": Fwd Limit Switch", talon.getForwardLimit().getValue().toString()); - map.put(name + ": Rev Limit Switch", talon.getReverseLimit().getValue().toString()); - map.put(name + ": Position (Rots)", talon.getPosition().getValueAsDouble()); - map.put(name + ": Velocity (Rots\\s)", talon.getVelocity().getValueAsDouble()); - map.put(name + ": Acceleration (Rots\\s^2)", talon.getAcceleration().getValueAsDouble()); - map.put(name + ": Closed Loop Target", talon.getClosedLoopReference().getValueAsDouble()); - map.put(name + ": Closed Loop Slot", talon.getClosedLoopSlot().getValue().intValue()); - map.put(name + ": Supply Voltage (V)", talon.getSupplyVoltage().getValueAsDouble()); - map.put(name + ": Motor Voltage (V)", talon.getMotorVoltage().getValueAsDouble()); - map.put(name + ": Supply Current (A)", talon.getSupplyCurrent().getValueAsDouble()); - map.put(name + ": Torque Current (A)", talon.getTorqueCurrent().getValueAsDouble()); - map.put(name + ": Device Temperature (C)", talon.getDeviceTemp().getValueAsDouble()); - map.put(name + ": Has Reset Occurred", talon.hasResetOccurred()); - return map; - } - - /** - * Fills your map with TalonFX StatusSignals, name defaults to the TalonFX's device ID. - * @param talon to log - * @param map to fill - * @return the map passed in for method chaining - */ - public static Map putTalonLog(TalonFX talon, Map map) { - int ID = talon.getDeviceID(); - return putTalonLog(talon, "TalonFX " + ID, map); - } - - /** - * Fills your map with values to log on a subsystem. - * @param subsystem to log - * @param map to fill - * @return the map passed in for method chaining - */ - public static Map putSubsystemLog(Subsystem subsystem, Map map) { - Command currentCommand = subsystem.getCurrentCommand(); - Command innerCommand = Commands.none(); - String commandGroupCurrentCommand = "None"; - if (currentCommand instanceof WrapperCommand cmd) { - innerCommand = LogUtil.getWrapperCommandInner(cmd); - } - if (innerCommand instanceof SequentialCommandGroup group) { - commandGroupCurrentCommand = LogUtil.getSequentialCommandCurrentCommand(group).getName(); - } - if (innerCommand instanceof ParallelCommandGroup group) { - for (Command c : LogUtil.getParallelCommandCurrentCommands(group)) { - commandGroupCurrentCommand += c.getName() + " "; - } - } - map.put("Subsystem: _Name", subsystem.getName()); - map.put("Subsystem: Default Command", subsystem.getDefaultCommand() == null ? "None" : subsystem.getDefaultCommand().getName()); - map.put("Subsystem: Current Command", currentCommand == null ? "None" : currentCommand.getName()); - map.put("Subsystem: Command Group Current Command", commandGroupCurrentCommand); - return map; - } - - /** - * Logs a value that uses structs to network tables. - * @param table network table to log value to - * @param name for value - * @param objToLog object that will be logged as a struct - * @param struct implementation that will be used for network tables to log object - */ - @SuppressWarnings("unchecked") - private static void logStruct(NetworkTable table, String name, T objToLog, Struct struct) { - StructTopic topic = table.getStructTopic(name, struct); - for (StructPublisher publisher : structPublishers) { - if (publisher.getTopic().equals(topic)) { - ((StructPublisher) publisher).set(objToLog); - return; - } - } - StructPublisher publisher = topic.publish(); - publisher.set(objToLog); - structPublishers.add(publisher); - } - - /** - * If an object has both struct and protobuf, prefer struct as it's faster. - * Gets the corresponding struct implementation for an object e.g. Pose2d.struct for a Pose2d. - * @param obj object to get struct for - * @return An object's struct implementation or empty if there is none - */ - @SuppressWarnings("unchecked") - private static Optional> getStruct(T obj) { - Struct struct = null; - if (obj instanceof Pose2d) struct = Pose2d.struct; - if (obj instanceof Pose3d) struct = Pose3d.struct; - if (obj instanceof Rotation2d) struct = Rotation2d.struct; - if (obj instanceof Rotation3d) struct = Rotation3d.struct; - if (obj instanceof Translation2d) struct = Translation2d.struct; - if (obj instanceof Translation3d) struct = Translation3d.struct; - if (obj instanceof Transform2d) struct = Transform2d.struct; - if (obj instanceof Transform3d) struct = Transform3d.struct; - if (obj instanceof Twist2d) struct = Twist2d.struct; - if (obj instanceof Twist3d) struct = Twist3d.struct; - return Optional.ofNullable((Struct) struct); - } - - /** - * Logs a value that uses protobufs to network tables. - * @param table network table to log value to - * @param name for value - * @param objToLog object that will be logged as a protobuf - * @param protobuf implementation that will be used for network tables to log object - */ - @SuppressWarnings("unchecked") - private static > void logProtobuf(NetworkTable table, String name, T objToLog, Protobuf protobuf) { - ProtobufTopic topic = table.getProtobufTopic(name, protobuf); - for (ProtobufPublisher publisher : protobufPublishers) { - if (publisher.getTopic().equals(topic)) { - ((ProtobufPublisher) publisher).set(objToLog); - return; - } - } - ProtobufPublisher publisher = topic.publish(); - publisher.set(objToLog); - protobufPublishers.add(publisher); - } - - /** - * If an object has both struct and protobuf, prefer struct as it's faster. - * Gets the corresponding protobuf implementation for an object e.g. Pose2d.proto for a Pose2d. - * @param obj object to get struct for - * @return An object's protobuf implementation or empty if there is none - */ - @SuppressWarnings("unchecked") - private static > Optional> getProtobuf(T obj) { - Protobuf protobuf = null; - if (obj instanceof Trajectory) protobuf = Trajectory.proto; - return Optional.ofNullable((Protobuf) protobuf); - } - - /** - * Logs common Driver Station data like robot mode and match time etc. - */ - private static void logDriverStation() { - String mode = "Unknown"; - if (DriverStation.isTeleop()) { - mode = "Teleop"; - } - else if (DriverStation.isAutonomous()) { - mode = "Autonomous"; - } - else if (DriverStation.isTest()) { - mode = "Test"; - } - mainTable.getEntry("_DS Mode").setString(mode); - mainTable.getEntry("_Robot Enabled").setBoolean(DriverStation.isEnabled()); - mainTable.getEntry("_Match Time").setDouble(DriverStation.getMatchTime()); - mainTable.getEntry("_is FMS Attached").setBoolean(DriverStation.isFMSAttached()); - } - -} diff --git a/src/main/java/frc/robot/lib/logging/TalonFXLogger.java b/src/main/java/frc/robot/lib/logging/TalonFXLogger.java new file mode 100644 index 0000000..4d53d98 --- /dev/null +++ b/src/main/java/frc/robot/lib/logging/TalonFXLogger.java @@ -0,0 +1,132 @@ +package frc.robot.lib.logging; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AppliedRotorPolarityValue; +import com.ctre.phoenix6.signals.ControlModeValue; +import com.ctre.phoenix6.signals.ForwardLimitValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.TimedRobot; + +@CustomLoggerFor(TalonFX.class) +public class TalonFXLogger extends ClassSpecificLogger { + + private static Map talonFXSignals = new HashMap<>(); + + public TalonFXLogger() { + super(TalonFX.class); + } + + @Override + protected void update(EpilogueBackend dataLogger, TalonFX talon) { + var signals = talonFXSignals.computeIfAbsent(talon, TalonFXSignals::new); + dataLogger.log("Device ID", talon.getDeviceID()); + dataLogger.log("Has Reset Occurred", talon.hasResetOccurred()); + dataLogger.log("Control Mode", signals.controlMode.getValue()); + dataLogger.log("Rotor Polarity", signals.appliedRotorPolarity.getValue()); + dataLogger.log("Fwd Limit Switch", signals.forwardLimit.getValue()); + dataLogger.log("Rev Limit Switch", signals.reverseLimit.getValue()); + dataLogger.log("Position (Rots)", signals.position.getValueAsDouble()); + dataLogger.log("Velocity (Rots\\s)", signals.velocity.getValueAsDouble()); + dataLogger.log("Acceleration (Rots\\s^2)", signals.acceleration.getValueAsDouble()); + dataLogger.log("Closed Loop Reference", signals.closedLoopReference.getValueAsDouble()); + dataLogger.log("Closed Loop Slot", signals.closedLoopSlot.getValue()); + dataLogger.log("Supply Voltage (V)", signals.supplyVoltage.getValueAsDouble()); + dataLogger.log("Motor Voltage (V)", signals.motorVoltage.getValueAsDouble()); + dataLogger.log("Supply Current (A)", signals.supplyCurrent.getValueAsDouble()); + dataLogger.log("Torque Current (A)", signals.torqueCurrent.getValueAsDouble()); + dataLogger.log("Device Temperature (C)", signals.deviceTemp.getValueAsDouble()); + dataLogger.log("Firmware Version", signals.version.getValue()); + } + + /** + * This must be called for your TalonFXs to be logged properly. + * Refreshes all of the TalonFX at once, periodically determined by the period and offset provided. + * @param robot The robot to add the callback to + * @param period The rate at which the TalonFXs should be refreshed + * @param offset The offset from the main loop at which this refresh should occur + */ + public static void refreshAllLoggedTalonFX(TimedRobot robot, Time period, Time offset) { + robot.addPeriodic(() -> { + List signals = new ArrayList<>(); // cache this + talonFXSignals.values().forEach(s -> signals.addAll(s.allSignals)); + if (signals.size() > 0) { + BaseStatusSignal.refreshAll(signals.toArray(new BaseStatusSignal[signals.size()])); + } + }, period, offset); + } + +} + +class TalonFXSignals { + + public final StatusSignal controlMode; + public final StatusSignal appliedRotorPolarity; + public final StatusSignal forwardLimit; + public final StatusSignal reverseLimit; + public final StatusSignal position; + public final StatusSignal velocity; + public final StatusSignal acceleration; + public final StatusSignal closedLoopReference; + public final StatusSignal closedLoopSlot; + public final StatusSignal supplyVoltage; + public final StatusSignal motorVoltage; + public final StatusSignal supplyCurrent; + public final StatusSignal torqueCurrent; + public final StatusSignal deviceTemp; + public final StatusSignal version; + public final List allSignals; + + public TalonFXSignals(TalonFX talon) { + this.controlMode = talon.getControlMode(); + this.appliedRotorPolarity = talon.getAppliedRotorPolarity(); + this.forwardLimit = talon.getForwardLimit(); + this.reverseLimit = talon.getReverseLimit(); + this.position = talon.getPosition(); + this.velocity = talon.getVelocity(); + this.acceleration = talon.getAcceleration(); + this.closedLoopReference = talon.getClosedLoopReference(); + this.closedLoopSlot = talon.getClosedLoopSlot(); + this.supplyVoltage = talon.getSupplyVoltage(); + this.motorVoltage = talon.getMotorVoltage(); + this.supplyCurrent = talon.getSupplyCurrent(); + this.torqueCurrent = talon.getTorqueCurrent(); + this.deviceTemp = talon.getDeviceTemp(); + this.version = talon.getVersion(); + allSignals = List.of( + controlMode, + appliedRotorPolarity, + forwardLimit, + reverseLimit, + position, + velocity, + acceleration, + closedLoopReference, + closedLoopSlot, + supplyVoltage, + motorVoltage, + supplyCurrent, + torqueCurrent, + deviceTemp, + version + ); + } + +} diff --git a/src/main/java/frc/robot/lib/motion/FollowTrajectory.java b/src/main/java/frc/robot/lib/motion/FollowTrajectory.java index 8a54bd5..7ce16aa 100644 --- a/src/main/java/frc/robot/lib/motion/FollowTrajectory.java +++ b/src/main/java/frc/robot/lib/motion/FollowTrajectory.java @@ -4,18 +4,13 @@ import java.util.function.Supplier; import edu.wpi.first.math.controller.LTVUnicycleController; -import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -29,41 +24,6 @@ public class FollowTrajectory { */ private FollowTrajectory() {} - private static RamseteController ramseteController; - private static DifferentialDriveKinematics diffKinematics; - - /** - * Configure FollowTrajectory for using its static methods, must be called. - * @param b The B constant(RAMSETE) - * @param zeta The Zeta constant(RAMSETE) - * @param trackWidth The width of the drivetrain(Kinematics) - */ - public static void config(double b, double zeta, Measure trackWidth) { - ramseteController = new RamseteController(b, zeta); - diffKinematics = new DifferentialDriveKinematics(trackWidth); - } - - /** - * Returns a RamseteCommand that follows the specified trajectory and uses the PID loop on the Talons - * @param trajectory The Trajectory to follow - * @param zeroPose The position to start relative to - * @param pose The supplier of the robot pose - * @param velocity The consumer of ramsete's left and right wheel velocities in meters/second - * @param driveSubsystem The subsystem to require during the command - * @return Returns a RamseteCommand that will follow the specified trajectory with the specified driveSubsystem - */ - public static Command ramseteControllerCommand(Trajectory trajectory, Pose2d zeroPose, Supplier pose, BiConsumer velocity, Subsystem driveSubsystem) { - trajectory = trajectory.transformBy(new Transform2d(new Pose2d(), zeroPose)); - return new RamseteCommand( - trajectory, - pose, - ramseteController, - diffKinematics, - velocity, - driveSubsystem - ); - } - /** * Creates a command to follow a given trajectory using a LTVUnicycleController * @param trajectory The Trajectory to follow @@ -72,7 +32,8 @@ public static Command ramseteControllerCommand(Trajectory trajectory, Pose2d zer * @param driveSubsystem The subsystem to require during the command * @return A command to follow a given trajectory using a LTVUnicycleController */ - public static Command LTVControllerCommand(Trajectory trajectory, Supplier pose, BiConsumer velocity, Subsystem driveSubsystem) { + public static Command LTVControllerCommand(Trajectory trajectory, Supplier pose, BiConsumer velocity, + DifferentialDriveKinematics diffKinematics, Subsystem driveSubsystem) { LTVUnicycleController controller = new LTVUnicycleController(TimedRobot.kDefaultPeriod); Timer timer = new Timer(); return driveSubsystem.runOnce(timer::restart) diff --git a/src/main/java/frc/robot/lib/pneumatics/CurrentLimitedCompressor.java b/src/main/java/frc/robot/lib/pneumatics/CurrentLimitedCompressor.java deleted file mode 100644 index 189de6b..0000000 --- a/src/main/java/frc/robot/lib/pneumatics/CurrentLimitedCompressor.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.lib.pneumatics; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Seconds; - -import edu.wpi.first.units.Current; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Time; -import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.PneumaticsModuleType; - -public class CurrentLimitedCompressor extends Compressor { - - private double currentTime = 0; - private final double maxAmps; - private final double timeToTrip; - private static int numberInstances = 0; - - public CurrentLimitedCompressor(int module, PneumaticsModuleType moduleType, double maxAmps, double timeToTrip) { - super(module, moduleType); - this.maxAmps = maxAmps; - this.timeToTrip = timeToTrip; - numberInstances++; - } - - public CurrentLimitedCompressor(int module, PneumaticsModuleType moduleType, Measure maxAmps, Measure