diff --git a/.gitignore b/.gitignore index 0fcb4ad..da9d1c5 100644 --- a/.gitignore +++ b/.gitignore @@ -161,5 +161,27 @@ 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 diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 6956b19..5211e04 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "currentLanguage": "java", "enableCppIntellisense": false, - "projectYear": "2024", + "currentLanguage": "java", + "projectYear": "2025beta", "teamNumber": 2508 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 9ef4ecc..a996cfb 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.1.1-beta-2" 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..57d8931 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; @@ -15,11 +11,6 @@ import edu.wpi.first.networktables.DoubleTopic; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -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 +75,30 @@ 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(); - } + public 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()); } - 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()); - } - } - - 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(); - } + public static void logCommandInterrupts() { + // 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(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..84dd1c7 --- /dev/null +++ b/src/main/java/frc/robot/lib/logging/TalonFXLogger.java @@ -0,0 +1,37 @@ +package frc.robot.lib.logging; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(TalonFX.class) +public class TalonFXLogger extends ClassSpecificLogger { + + public TalonFXLogger() { + super(TalonFX.class); + } + + @Override + protected void update(DataLogger dataLogger, TalonFX talon) { + dataLogger.log("Device ID", talon.getDeviceID()); + dataLogger.log("Control Mode", talon.getControlMode().getValue()); + dataLogger.log("Rotor Polarity", talon.getAppliedRotorPolarity().getValue()); + dataLogger.log("Fwd Limit Switch", talon.getForwardLimit().getValue()); + dataLogger.log("Rev Limit Switch", talon.getReverseLimit().getValue()); + dataLogger.log("Position (Rots)", talon.getPosition().getValueAsDouble()); + dataLogger.log("Velocity (Rots\\s)", talon.getVelocity().getValueAsDouble()); + dataLogger.log("Acceleration (Rots\\s^2)", talon.getAcceleration().getValueAsDouble()); + dataLogger.log("Closed Loop Reference", talon.getClosedLoopReference().getValueAsDouble()); + dataLogger.log("Closed Loop Slot", talon.getClosedLoopSlot().getValue()); + dataLogger.log("Supply Voltage (V)", talon.getSupplyVoltage().getValueAsDouble()); + dataLogger.log("Motor Voltage (V)", talon.getMotorVoltage().getValueAsDouble()); + dataLogger.log("Supply Current (A)", talon.getSupplyCurrent().getValueAsDouble()); + dataLogger.log("Torque Current (A)", talon.getTorqueCurrent().getValueAsDouble()); + dataLogger.log("Device Temperature (C)", talon.getDeviceTemp().getValueAsDouble()); + dataLogger.log("Firmware Version", talon.getVersion().getValue()); + dataLogger.log("Has Reset Occurred", talon.hasResetOccurred()); + } + +} 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 index 189de6b..3e1940c 100644 --- a/src/main/java/frc/robot/lib/pneumatics/CurrentLimitedCompressor.java +++ b/src/main/java/frc/robot/lib/pneumatics/CurrentLimitedCompressor.java @@ -3,9 +3,8 @@ 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.units.measure.Current; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.PneumaticsModuleType; @@ -23,7 +22,7 @@ public CurrentLimitedCompressor(int module, PneumaticsModuleType moduleType, dou numberInstances++; } - public CurrentLimitedCompressor(int module, PneumaticsModuleType moduleType, Measure maxAmps, Measure