Apply shooter tuner changes stash

Show selected path

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.");

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() {

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,10 +150,8 @@ public class BoomBoom extends SubsystemBase {
     // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
   }
 
-  public void loadShooterTable() {
-    try {
   // This is a helper class that allows us to read a CSV file into a Java array.
-      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
+  private CSV<ShooterTableEntry> 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("\\([^\\)]*+\\)");
 
@@ -162,8 +170,11 @@ public class BoomBoom extends SubsystemBase {
       return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
     }
   };
+
+  public void loadShooterTable() {
+    try {
       // 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;
   }

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) {

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,19 +49,27 @@ public class CSV<R> {
   private final Map<String, BiConsumer<R, String>> 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.
@@ -105,6 +115,17 @@ public class CSV<R> {
     }
   }
 
+  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<Stream<String>> header = Stream.of(Stream.of(originalHeader.split(",")));
+    final Stream<Stream<String>> 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<String, ?> getTypeParser(final Class<?> type) {
     try {
@@ -143,6 +164,14 @@ public class CSV<R> {
     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 <T> String create(final T[] objects, boolean colored) {
       final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new);

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<ShooterTableEntry[]> m_table;
+  private Supplier<ShooterTableEntry[]> m_tableGetter;
-  public SendableTable(Supplier<ShooterTableEntry[]> table) {
+  private Consumer<ShooterTableEntry[]> m_tableSetter;
-    m_table = table;
+  private ShooterTableEntry[] tableCache;
+  private byte[] bytesCache;
+
+  public SendableTable(Supplier<ShooterTableEntry[]> getter, Consumer<ShooterTableEntry[]> setter) {
+    m_tableGetter = getter;
+    m_tableSetter = setter;
   }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Table");
-    builder.addRawProperty("table", this::getTableAsBytes, 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();
+    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);
       });
-    return byteBuffer.hasArray() ? byteBuffer.array() : new byte[0];
+      tableCache = table;
+      bytesCache = byteBuffer.array();
     }
+    return bytesCache;
+  }
+
   private void setTableFromBytes(byte[] bytes) {
+    if (bytes.length > 0 && !Arrays.equals(bytesCache, bytes)) {
       ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
-    ShooterTableEntry[] table = m_table.get();
+      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);
+    }
   }
 }