diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 9fbaf8f..0d65df0 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -30,7 +30,6 @@ public final class Main { */ public static void main(String... args) { AnsiLogging.systemInstall(); - DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage)); RobotBase.startRobot(Robot::new); } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 7f87f3e..eec2973 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -8,6 +8,7 @@ import java.io.File; import java.io.IOException; import java.util.Comparator; import java.util.Map; +import java.util.Objects; import java.util.Optional; import java.util.function.Function; import java.util.logging.Level; @@ -17,6 +18,7 @@ import java.util.stream.IntStream; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.diffplug.common.base.Errors; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; @@ -24,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; import frc4388.utility.Gains; +import frc4388.utility.NumericData; public class BoomBoom extends SubsystemBase { private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName()); @@ -56,108 +59,23 @@ public class BoomBoom extends SubsystemBase { public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; 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()); - // 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); - } + m_shooterTable = readShooterTable(); + // Run a helper method that logs the contents of the table on a new thread. + new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); } - public Double getVelocity(final Double distance) { + public Double getVelocity(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(); + return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } - public Double getHood(final Double distance) { + public Double getHood(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(); - final int closestRecordIndex = closestEntry.getKey(); - final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; - 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(); + return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } @Override @@ -199,4 +117,38 @@ public class BoomBoom extends SubsystemBase { // feedforward.calculate(targetVel)); // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + + private static ShooterTableEntry[] readShooterTable() { + 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(String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + }; + // This is reading the CSV file into a Java array. + return csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); + } catch (IOException exception) { + ShooterTableEntry dummyEntry = new ShooterTableEntry(); + dummyEntry.distance = 0.0; + dummyEntry.hoodExt = 0.0; + dummyEntry.drumVelocity = 0.0; + LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); + return new ShooterTableEntry[] { dummyEntry }; + } + } } \ No newline at end of file diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java index ce8df45..2659a5e 100644 --- a/src/main/java/frc4388/utility/AnsiLogging.java +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -19,6 +19,9 @@ import java.util.logging.LogRecord; import java.util.logging.Logger; import java.util.logging.StreamHandler; +import com.diffplug.common.base.DurianPlugins; +import com.diffplug.common.base.Errors; + import org.fusesource.jansi.Ansi.Attribute; import org.fusesource.jansi.Ansi.Color; import org.fusesource.jansi.AnsiConsole; @@ -47,6 +50,8 @@ public class AnsiLogging { System.setOut(printStreamLogger(Logger.getGlobal(), "out", Level.INFO)); // Sends standard error output stream messages through a logger. System.setErr(printStreamLogger(Logger.getGlobal(), "err", Level.SEVERE)); + // This is registering a plugin that will log Durian errors to the console using a logger. + DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage)); } catch (IOException exception) { exception.printStackTrace(AnsiConsole.sysErr()); } diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java index de885a7..eb27bc8 100644 --- a/src/main/java/frc4388/utility/CSV.java +++ b/src/main/java/frc4388/utility/CSV.java @@ -1,3 +1,9 @@ +/* + * Copyright 2021 nathanrsxtn + * + * SPDX-License-Identifier: GPL-3.0-or-later + */ + package frc4388.utility; import java.awt.Color; @@ -29,6 +35,9 @@ import java.util.stream.Stream; /** * Reads and parses a CSV file and returns an array of records. + * + * This copy is abbreviated to exclude functions that are not essential to this project. + * @author nathanrsxtn */ public class CSV { private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); @@ -55,9 +64,9 @@ public class CSV { * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by * the given generator. Private fields and fields of primitive types are not supported. * @param generator a parameterless supplier which produces a new object with any number of fields - * corresponding to header names from a CSV file. The first character of the names - * from the header in the CSV file will be made lowercase and invalid characters - * will be removed to match Java naming conventions. + * corresponding to header names from a CSV file. The first character of the names from the + * header in the CSV file will be made lowercase and invalid characters will be removed to + * match Java naming conventions. * @see #read(Path) */ @SuppressWarnings("unchecked") @@ -69,14 +78,13 @@ public class CSV { this.setters = new HashMap<>(); for (final Field field : clazz.getFields()) { final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); - if (parser != null) - this.setters.put(field.getName(), (final R obj, final String rawValue) -> { - try { - field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); - } catch (final IllegalAccessException e) { - throw new RuntimeException(e); - } - }); + if (parser != null) this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); } } @@ -93,7 +101,7 @@ public class CSV { try (final BufferedReader reader = Files.newBufferedReader(path)) { final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); final Stream lines = reader.lines(); - return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(arrayGenerator); } } @@ -117,12 +125,10 @@ public class CSV { if (countTrailing(field, '"') % 2 == 0) { tryFieldEndFromIndex = tryFieldEndIndex + 1; continue; - } else - field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } else field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); } final BiConsumer setter = fieldParseSetters[i]; - if (setter != null) - setter.accept(object, field); + if (setter != null) setter.accept(object, field); tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; i++; } @@ -144,11 +150,10 @@ public class CSV { rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); - if (colored) - IntStream.range(0, fields.length).forEach(i -> { - final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); - rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); - }); + if (colored) IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); } diff --git a/src/main/java/frc4388/utility/NumericData.java b/src/main/java/frc4388/utility/NumericData.java new file mode 100644 index 0000000..fa5a866 --- /dev/null +++ b/src/main/java/frc4388/utility/NumericData.java @@ -0,0 +1,69 @@ +/* + * Copyright 2021 nathanrsxtn + * + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +package frc4388.utility; + +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.stream.IntStream; + +public final class NumericData { + /** + * 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 a Number. + * @param targetGetter A function that takes an E and returns a Number. + * @return The interpolated value. + */ + public 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(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + 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. + */ + public 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. + */ + public 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(); + } +}