diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 038a5ee..dd05ec5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,9 +101,10 @@ public class RobotContainer { private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); - private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH:mm:ss.SSS'.path'"); + private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'"); private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault()); private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner"); + // Function that removes the ".path" from the end of a string. private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst("")); /** @@ -224,15 +225,17 @@ public class RobotContainer { return m_operatorXbox; } + /** + * Creates a WatchKey for the path planner directory and registers it with the WatchService. + * Then creates a NotifierCommand that will update the auto chooser with the latest path files. + * Finally, adds the existing path files to the auto chooser + */ private void autoInit() { try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE); // TODO: Store this and other commands as fields so they can be rescheduled. new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { - @Override - public boolean runsWhenDisabled() { - return true; - } + @Override public boolean runsWhenDisabled() { return true; } }.withName("Path Watcher").schedule(); } catch (IOException exception) { LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); @@ -243,6 +246,9 @@ public class RobotContainer { SmartDashboard.putData("Auto Chooser", autoChooser); } + /** + * Creates a button on the SmartDashboard that will record the path of the robot. + */ public void recordInit() { SmartDashboard.putData("Recording", new RunCommand(this::recordPeriodic) { @@ -258,6 +264,12 @@ public class RobotContainer { }.withName("Record Path (Cancel to Save)")); } + /** + * Called when a file is created, modified, or deleted. + * Adds newly created .path files to the SendableChooser. + * Reloads the path if the currently selected file is modified. + * @param watchKey The WatchKey that is being observed. + */ private void updateAutoChooser(WatchKey watchKey) { List> watchEvents = watchKey.pollEvents(); if (!watchEvents.isEmpty()) { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6692940..c02de5e 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -10,6 +10,7 @@ import java.util.Comparator; import java.util.Map; import java.util.Optional; import java.util.function.Function; +import java.util.logging.Level; import java.util.logging.Logger; import java.util.regex.Pattern; import java.util.stream.IntStream; @@ -40,8 +41,7 @@ public class BoomBoom extends SubsystemBase { public Hood m_hoodSubsystem; public Turret m_turretSubsystem; - // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values - // later + // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later public static class ShooterTableEntry { public Double distance, hoodExt, drumVelocity; @@ -58,31 +58,66 @@ public class BoomBoom extends SubsystemBase { m_shooterFalconRight = shooterFalconRight; try { + // This is a helper class that allows us to read a CSV file into a Java array. CSV csv = new CSV<>(ShooterTableEntry::new) { + // This is a regular expression that removes all parentheses from the header of the CSV file. private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + /** + * Removes the parentheses from the CSV header + * + * @param header The header to be sanitized. + * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class. + * The parentheses.matcher(header) is matching the header with the parentheses regular + * expression. The replaceAll method is replacing all of the parentheses with an empty + * string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling + * the parent sanitizer. + */ @Override protected String headerSanitizer(final String header) { return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); } - }; + // This is reading the CSV file into a Java array. m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); - new Thread(() -> LOGGER.fine(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); - } catch (final IOException e) { - e.printStackTrace(); - // throw new RuntimeException(e); + // This is a running a helper method that is logging the contents of the table to the console on a new thread. + new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException exception) { + ShooterTableEntry dummyEntry = new ShooterTableEntry(); + dummyEntry.distance = 0.0; + dummyEntry.hoodExt = 0.0; + dummyEntry.drumVelocity = 0.0; + m_shooterTable = new ShooterTableEntry[] { dummyEntry }; + LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); } } public Double getVelocity(final Double distance) { + // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a + // linear interpolation of the two values (drumVelocity) at the two closest points in the table + // (m_shooterTable) to the given value (distance). return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } public Double getHood(final Double distance) { + // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear + // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable) + // to the given value (distance). return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } + /** + * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the + * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the + * interpolation (y) between the two values (y0 and y1) accquired by applying the target getter + * function to the lower and upper bounds entries. + * + * @param table An array of entries to search through. + * @param lookupValue The value to lookup in the table. + * @param lookupGetter A function that takes an entry from the table and returns . + * @param targetGetter A function that takes an E and returns a Number. + * @return The interpolated value. + */ private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); final E closestRecord = closestEntry.getValue(); @@ -91,11 +126,35 @@ public class BoomBoom extends SubsystemBase { return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); } + /** + * If the value is in the table, return the entry. Otherwise, return the entry with the closest + * value + * + * @param table The array of values to search. + * @param value The value to search for. + * @param valueGetter A function that takes an element of the table and returns a Number to compare + * with the given value. + * @param exactMatch If true, the lookup will only return a match if the value is exactly equal to + * the value of the entry. If false, the lookup will return a match with a value closest to + * the given value. + * @return The entry with the closest value to the given value. + */ private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); } + /** + * Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a + * linear interpolation of the two values y0 and y1 + * + * @param x The value to interpolate. + * @param x0 the x coordinate of the lower bound to interpolate to + * @param y0 The value at x0. + * @param x1 the x-coordinate of the upper bound to interpolate to + * @param y1 The value at x1. + * @return The interpolation between y0 and y1 at x. + */ private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); @@ -104,7 +163,6 @@ public class BoomBoom extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java index 901e2d7..57a1d90 100644 --- a/src/main/java/frc4388/utility/AnsiLogging.java +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -48,6 +48,9 @@ public class AnsiLogging extends ConsoleHandler { } } + /** + * This class is a ConsoleHandler that uses ANSI escape codes to colorize the output + */ public static class AnsiColorConsoleHandler extends ConsoleHandler { @Override public void publish(LogRecord logRecord) { @@ -91,26 +94,41 @@ public class AnsiLogging extends ConsoleHandler { @Override public String format(LogRecord logRecord) { ZonedDateTime time = ZonedDateTime.ofInstant(logRecord.getInstant(), zoneId); + // Get the logger name, source class name, and/or source method name. String source = Optional.ofNullable(logRecord.getLoggerName()).or(() -> Optional.ofNullable(logRecord.getSourceClassName())).map(s -> s + " ").orElse("") + Optional.ofNullable(logRecord.getSourceMethodName()).orElse(""); String message = formatMessage(logRecord); + // Get the stack trace of the exception if it was thrown. String throwable = Optional.ofNullable(logRecord.getThrown()).map(this::makeStackTraceString).orElse(""); + // Select the appropriate format string for the log level. String format = levelColors.getOrDefault(logRecord.getLevel().intValue(), levelColors.get(Level.ALL.intValue())); - return String.format(format, time, source, logRecord.getLevel().getLocalizedName(), message.lines().count() > 1 ? System.lineSeparator() : " ", message.contains("\033") ? "\033[0m" + message : message, throwable); + // Format the log message. + return String.format(format, time, source, logRecord.getLevel().getLocalizedName(), message.lines().count() > 1 ? System.lineSeparator() : " ", message.contains("\033") ? "\033[0m" + message : message, throwable); } }; } + /** + * Create a PrintStream that writes to the given logger at the given level + * + * @param logger The logger to use. + * @param level The level of the log message. + * @return A new PrintStream object. + */ private static PrintStream printStreamLogger(Logger logger, Level level) { return new PrintStream(new OutputStream() { + // This is a buffer that is used to store the characters that are written to the PrintStream. private final StringBuilder stringBuilder = new StringBuilder(); + /** + * If the character is a newline, flush the buffer to the logger, otherwise add the character to the + * buffer. + */ @Override public void write(int i) throws IOException { if (i == '\n') { logger.log(level, stringBuilder::toString); stringBuilder.setLength(0); - } else - stringBuilder.appendCodePoint(i); + } else stringBuilder.appendCodePoint(i); } }); } diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java index 45ac768..de885a7 100644 --- a/src/main/java/frc4388/utility/CSV.java +++ b/src/main/java/frc4388/utility/CSV.java @@ -27,6 +27,9 @@ import java.util.stream.Collectors; import java.util.stream.IntStream; import java.util.stream.Stream; +/** + * Reads and parses a CSV file and returns an array of records. + */ public class CSV { private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]");