diff --git a/src/main/java/frc4388/robot/commands/PathRecorder.java b/src/main/java/frc4388/robot/commands/PathRecorder.java index b6f16bb..33d3140 100644 --- a/src/main/java/frc4388/robot/commands/PathRecorder.java +++ b/src/main/java/frc4388/robot/commands/PathRecorder.java @@ -169,6 +169,7 @@ public class PathRecorder extends CommandBase { if (pathName != null) { LOGGER.log(Level.WARNING, "Loading path {0}.", pathName); loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(pathName), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); + m_swerveDrive.m_field.getObject("traj").setTrajectory(loadedPathTrajectory); LOGGER.info("Done loading."); } else { LOGGER.severe("No path to load."); diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java index 0b9aabe..e36c902 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.utility.shuffleboard.SendableTable; @@ -41,7 +42,7 @@ public class ShooterTuner extends CommandBase { m_shotEditor = new ShotEditor(); m_shotCsvAppender = new CSVAppender(); tableOverrideEntry = new ShooterTableEntry(); - m_tableEditor = new SendableTable(() -> m_boomBoom.m_shooterTable); + m_tableEditor = new SendableTable(m_boomBoom::getShooterTable, m_boomBoom::setShooterTable); setName("Shooter Data Mode"); } @@ -54,12 +55,18 @@ public class ShooterTuner extends CommandBase { manual.add("Manual Data Appender", m_shotCsvAppender); var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5); csv.add("Shooter Table", m_tableEditor); - csv.add("Shooter Tuner State", this); + csv.add("Save to CSV File", new InstantCommand(m_boomBoom::saveShooterTable) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }); + csv.add("Shooter Tuner State (Disable to Reload)", this); } tableOverrideEntry.distance = 0.0; tableOverrideEntry.hoodExt = 0.0; tableOverrideEntry.drumVelocity = 0.0; - m_boomBoom.m_shooterTable = new ShooterTableEntry[] { tableOverrideEntry }; + m_boomBoom.setShooterTable(new ShooterTableEntry[] { tableOverrideEntry }); Shuffleboard.selectTab("Shooter Tuner"); SmartDashboard.putData("Shooter Table", m_tableEditor); } @@ -72,7 +79,7 @@ public class ShooterTuner extends CommandBase { public void end(boolean interrupted) { m_boomBoom.loadShooterTable(); LOGGER.info(Errors.log().wrapWithDefault(() -> Files.readString(PATH), "Failed to read CSV")); - ShuffleboardHelper.purgeShuffleboardTab("Shooter Tuner"); + // ShuffleboardHelper.purgeShuffleboardTab("Shooter Tuner"); } @Override public final boolean isFinished() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index e6f1f75..6659b19 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -6,6 +6,8 @@ package frc4388.robot.subsystems; import java.io.File; import java.io.IOException; +import java.io.OutputStream; +import java.nio.file.Files; import java.util.Arrays; import java.util.Comparator; import java.util.Map; @@ -20,6 +22,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 com.diffplug.common.base.StringPrinter; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.Filesystem; @@ -55,7 +58,14 @@ public class BoomBoom extends SubsystemBase { public Double distance, hoodExt, drumVelocity; } - public ShooterTableEntry[] m_shooterTable; + private ShooterTableEntry[] m_shooterTable; + + public ShooterTableEntry[] getShooterTable() { + return m_shooterTable; + } + public void setShooterTable(ShooterTableEntry[] shooterTable) { + m_shooterTable = shooterTable; + } /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -140,30 +150,31 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + // This is a helper class that allows us to read a CSV file into a Java array. + private CSV m_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("")); + } + }; + public void loadShooterTable() { 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. - m_shooterTable = Arrays.stream(csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath())).sorted(Comparator.comparingDouble(e -> e.distance)).toArray(ShooterTableEntry[]::new); + m_shooterTable = Arrays.stream(m_csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath())).sorted(Comparator.comparingDouble(e -> e.distance)).toArray(ShooterTableEntry[]::new); } catch (IOException exception) { ShooterTableEntry dummyEntry = new ShooterTableEntry(); dummyEntry.distance = 0.0; @@ -174,6 +185,14 @@ public class BoomBoom extends SubsystemBase { } } + public void saveShooterTable() { + StringPrinter sp = new StringPrinter(Errors.log().wrap(s -> { + Files.writeString(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), s); + System.err.println(s); + })); + Errors.log().run(() -> m_csv.write(sp.toWriter(), "Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)", m_shooterTable, true)); + } + public void updateOffset(double change) { pidOffset = pidOffset + change; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1ca921f..8a2c3e3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -54,7 +54,7 @@ public class SwerveDrive extends SubsystemBase { public Rotation2d rotTarget = new Rotation2d(); private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - private final Field2d m_field = new Field2d(); + public final Field2d m_field = new Field2d(); public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_Pigeon2 gyro) { diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java index eb27bc8..e1cb508 100644 --- a/src/main/java/frc4388/utility/CSV.java +++ b/src/main/java/frc4388/utility/CSV.java @@ -9,6 +9,7 @@ package frc4388.utility; import java.awt.Color; import java.io.BufferedReader; import java.io.IOException; +import java.io.Writer; import java.lang.invoke.MethodHandleProxies; import java.lang.invoke.MethodHandles; import java.lang.invoke.MethodType; @@ -24,6 +25,7 @@ import java.util.List; import java.util.Map; import java.util.Objects; import java.util.function.BiConsumer; +import java.util.function.BinaryOperator; import java.util.function.Function; import java.util.function.IntFunction; import java.util.function.Predicate; @@ -47,20 +49,28 @@ public class CSV { private final Map> setters; /** - * A binary string operator to be applied to the entire header of the CSV. + * A string operator to be applied to the entire header of the CSV when reading. */ protected String headerSanitizer(final String header) { return SANITIZER.matcher(header).replaceAll(""); } /** - * A binary string operator to be applied to each name in the header of the CSV. + * A string operator to be applied to each name in the header of the CSV when reading. */ protected String nameProcessor(final String name) { return Character.toLowerCase(name.charAt(0)) + name.substring(1); } /** + * A string operator to be applied to each cell value in the body of the CSV when writing. + */ + protected String fieldStringifier(final Object fieldValue) { + final String fieldString = Objects.toString(fieldValue, ""); + return fieldString.contains(",") ? "\"" + fieldString.replace("\"", "\"\"") + "\"" : fieldString; + } + +/** * 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 @@ -105,6 +115,17 @@ public class CSV { } } + public void write(final Writer output, final String originalHeader, final R[] data, final boolean prettyPrint) throws IOException { + final String[] fieldNames = Stream.of(headerSanitizer(originalHeader).split(",")).map(this::nameProcessor).toArray(String[]::new); + final Stream> header = Stream.of(Stream.of(originalHeader.split(","))); + final Stream> body = Stream.of(data).map(element -> Stream.of(fieldNames).map(fieldName -> getFieldAsString(element, fieldName))); + if (prettyPrint) { + final String[][] rows = Stream.concat(header, body).map(s -> s.toArray(String[]::new)).toArray(String[][]::new); + final int[] columnWidths = Stream.of(rows).map(row -> Stream.of(row).mapToInt(String::length).toArray()).reduce(new int[fieldNames.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + output.write(Stream.of(rows).map(row -> IntStream.range(0, row.length).mapToObj(i -> String.format("%-" + columnWidths[i] + "s", row[i])).collect(Collectors.joining(",")).stripTrailing()).collect(Collectors.joining(System.lineSeparator())) + System.lineSeparator()); + } else output.write(Stream.concat(header, body).map(fields -> fields.collect(Collectors.joining(","))).collect(Collectors.joining(System.lineSeparator())) + System.lineSeparator()); + } + @SuppressWarnings("unchecked") private static Function getTypeParser(final Class type) { try { @@ -143,6 +164,14 @@ public class CSV { return count; } + private String getFieldAsString(final R element, final String fieldName) { + try { + return fieldStringifier(element.getClass().getField(fieldName).get(element)); + } catch (IllegalAccessException | NoSuchFieldException e) { + throw new RuntimeException(e); + } + } + public static final class ReflectionTable { public static String create(final T[] objects, boolean colored) { final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); diff --git a/src/main/java/frc4388/utility/shuffleboard/SendableTable.java b/src/main/java/frc4388/utility/shuffleboard/SendableTable.java index 0cdead2..b09208e 100644 --- a/src/main/java/frc4388/utility/shuffleboard/SendableTable.java +++ b/src/main/java/frc4388/utility/shuffleboard/SendableTable.java @@ -2,6 +2,7 @@ package frc4388.utility.shuffleboard; import java.nio.ByteBuffer; import java.util.Arrays; +import java.util.function.Consumer; import java.util.function.Supplier; import edu.wpi.first.util.sendable.Sendable; @@ -9,33 +10,48 @@ import edu.wpi.first.util.sendable.SendableBuilder; import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; public class SendableTable implements Sendable { - private Supplier m_table; - public SendableTable(Supplier table) { - m_table = table; + private Supplier m_tableGetter; + private Consumer m_tableSetter; + private ShooterTableEntry[] tableCache; + private byte[] bytesCache; + + public SendableTable(Supplier getter, Consumer setter) { + m_tableGetter = getter; + m_tableSetter = setter; } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Table"); - builder.addRawProperty("table", this::getTableAsBytes, null); - builder.addStringArrayProperty("header", () -> new String[] {"distance", "hoodExt", "drumVelocity"}, null); + builder.addRawProperty("table", this::getTableAsBytes, this::setTableFromBytes); + builder.addStringArrayProperty("header", () -> new String[] { "distance", "hoodExt", "drumVelocity" }, null); } + private byte[] getTableAsBytes() { - ShooterTableEntry[] table = m_table.get(); - ByteBuffer byteBuffer = ByteBuffer.allocate(Double.BYTES * 3 * table.length); - Arrays.stream(table).forEach(e -> { - byteBuffer.putDouble(e.distance); - byteBuffer.putDouble(e.hoodExt); - byteBuffer.putDouble(e.drumVelocity); - }); - return byteBuffer.hasArray() ? byteBuffer.array() : new byte[0]; + ShooterTableEntry[] table = m_tableGetter.get(); + if (!Arrays.equals(tableCache, table)) { + ByteBuffer byteBuffer = ByteBuffer.allocate(Double.BYTES * 3 * table.length); + Arrays.stream(table).forEach(e -> { + byteBuffer.putDouble(e.distance); + byteBuffer.putDouble(e.hoodExt); + byteBuffer.putDouble(e.drumVelocity); + }); + tableCache = table; + bytesCache = byteBuffer.array(); + } + return bytesCache; } + private void setTableFromBytes(byte[] bytes) { - ByteBuffer byteBuffer = ByteBuffer.wrap(bytes); - ShooterTableEntry[] table = m_table.get(); - for (int i = 0; i < table.length; i++) { - table[i].distance = byteBuffer.getDouble(); - table[i].hoodExt = byteBuffer.getDouble(); - table[i].drumVelocity = byteBuffer.getDouble(); + if (bytes.length > 0 && !Arrays.equals(bytesCache, bytes)) { + ByteBuffer byteBuffer = ByteBuffer.wrap(bytes); + ShooterTableEntry[] table = new ShooterTableEntry[bytes.length / (3 * Double.BYTES)]; + for (int i = 0; i < table.length; i++) { + table[i].distance = byteBuffer.getDouble(); + table[i].hoodExt = byteBuffer.getDouble(); + table[i].drumVelocity = byteBuffer.getDouble(); + } + m_tableSetter.accept(table); } } }