Simplify usage and improve shooter tuner interface

src/main/java/frc4388/robot/commands/ShooterTuner.java

@@ -8,7 +8,7 @@ import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.StandardOpenOption;
 import java.nio.file.attribute.FileOwnerAttributeView;
-import java.util.Arrays;
+import java.util.logging.Level;
 import java.util.logging.Logger;
 
 import com.diffplug.common.base.Errors;
@@ -20,13 +20,10 @@ import edu.wpi.first.wpilibj.RobotBase;
 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.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
+import frc4388.utility.SendableTable;
 
 public class ShooterTuner extends CommandBase {
   private static final Logger LOGGER = Logger.getLogger(ShooterTuner.class.getSimpleName());
@@ -34,21 +31,16 @@ public class ShooterTuner extends CommandBase {
   private final BoomBoom m_boomBoom;
   private final ShotEditor m_shotEditor;
   private final CSVAppender m_shotCsvAppender;
-  private final DistanceReader m_distanceReader;
-  private final ShooterTableEditor m_shooterTableEditor;
-  private final DisabledInstantCommand m_shooterTableUpdater;
-  private final DisabledInstantCommand m_printCsvFile;
   private final ShooterTableEntry tableOverrideEntry;
+  private final SendableTable m_tableEditor;
+  private boolean measureDistance = false;
 
   public ShooterTuner(BoomBoom boomBoom) {
     m_boomBoom = boomBoom;
     m_shotEditor = new ShotEditor();
     m_shotCsvAppender = new CSVAppender();
-    m_distanceReader = new DistanceReader();
-    m_shooterTableEditor = new ShooterTableEditor();
-    m_printCsvFile = new DisabledInstantCommand(() -> LOGGER.info(Errors.log().wrapWithDefault(() -> Files.readString(PATH), "Failed to read CSV")), "Print");
-    m_shooterTableUpdater = new DisabledInstantCommand(m_boomBoom::loadShooterTable, "Load CSV");
     tableOverrideEntry = new ShooterTableEntry();
+    m_tableEditor = new SendableTable(() -> m_boomBoom.m_shooterTable);
     setName("Shooter Data Mode");
   }
 
@@ -56,35 +48,38 @@ public class ShooterTuner extends CommandBase {
   public void initialize() {
     var tab = Shuffleboard.getTab("Shooter Tuner");
     if (tab.getComponents().isEmpty()) {
-      var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 3);
+      var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5);
-      manual.add("Distance Reader", m_distanceReader);
       manual.add("Manual Shooter Data", m_shotEditor);
-      manual.add("Shooter Table Appender", m_shotCsvAppender);
+      manual.add("Manual Data Appender", m_shotCsvAppender);
-      var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2, 0).withSize(4, 3);
+      var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5);
-      csv.addBoolean("Is Shooter Data Overridden", this::isOverridden);
+      csv.add("Shooter Table", m_tableEditor);
-      csv.add("Shooter Data (Broken)", m_shooterTableEditor);
+      csv.add("Shooter Tuner State", this);
-      csv.add("Shooter CSV Loader", m_shooterTableUpdater);
-      csv.add("Print CSV File", m_printCsvFile);
     }
     tableOverrideEntry.distance = 0.0;
     tableOverrideEntry.hoodExt = 0.0;
     tableOverrideEntry.drumVelocity = 0.0;
     m_boomBoom.m_shooterTable = new ShooterTableEntry[] { tableOverrideEntry };
     Shuffleboard.selectTab("Shooter Tuner");
+    SmartDashboard.putData("TABLE", m_tableEditor);
+  }
+  @Override
+  public void execute() {
+    if (measureDistance)
+      tableOverrideEntry.distance = SmartDashboard.getNumber("SmartDashboard/Distance to Target", -1);
+  }
+  @Override
+  public void end(boolean interrupted) {
+    m_boomBoom.loadShooterTable();
+    LOGGER.info(Errors.log().wrapWithDefault(() -> Files.readString(PATH), "Failed to read CSV"));
   }
-
   @Override
   public final boolean isFinished() {
-    return true;
+    return false;
   }
 
   @Override
   public String getName() {
-    return isOverridden() ? "Tuner Override" : "CSV File";
+    return isScheduled() ? "Enabled" : "Disabled";
-  }
-
-  private boolean isOverridden() {
-    return m_boomBoom.m_shooterTable.length == 1 && m_boomBoom.m_shooterTable[0].equals(tableOverrideEntry);
   }
 
   @Override
@@ -92,68 +87,32 @@ public class ShooterTuner extends CommandBase {
     return true;
   }
 
-  private static void setReadOnlyProperty(Object o) {
-    System.err.println("Unable to set read-only property.");
-  }
-
-  private class DisabledInstantCommand extends InstantCommand {
-    public DisabledInstantCommand(Runnable toRun, String name, Subsystem... requirements) {
-      super(toRun, requirements);
-      setName(name);
-    }
-
-    @Override
-    public boolean runsWhenDisabled() {
-      return true;
-    }
-  }
-
-  private class DistanceReader extends CommandBase {
-    @Override
-    public void execute() {
-      tableOverrideEntry.distance = SmartDashboard.getNumber("Distance to Target", -1);
-    }
-
-    @Override
-    public String getName() {
-      return isScheduled() ? "Reading" : "Enable";
-    }
-
-    @Override
-    public boolean runsWhenDisabled() {
-      return true;
-    }
-  }
-
   private class ShotEditor implements Sendable {
     @Override
     public void initSendable(SendableBuilder builder) {
       builder.setSmartDashboardType("RobotPreferences");
       builder.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d);
       builder.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d);
-      builder.addDoubleProperty("Measured Distance", () -> tableOverrideEntry.distance, ShooterTuner::setReadOnlyProperty);
+      builder.addDoubleProperty("Distance", () -> tableOverrideEntry.distance, d -> tableOverrideEntry.distance = d);
+      builder.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b);
     }
   }
 
   private class CSVAppender extends CommandBase {
     @Override
-    public void initialize() {
+    public void execute() {
       if (RobotBase.isReal()) Errors.log().run(() -> Files.getFileAttributeView(PATH, FileOwnerAttributeView.class).setOwner(FileSystems.getDefault().getUserPrincipalLookupService().lookupPrincipalByName("admin")));
       try (OutputStream csvOutputStream = Files.newOutputStream(PATH, StandardOpenOption.WRITE, StandardOpenOption.APPEND)) {
         csvOutputStream.write(String.format("%s,%s,%s%n", tableOverrideEntry.distance, tableOverrideEntry.hoodExt, tableOverrideEntry.drumVelocity).getBytes());
       } catch (IOException e) {
-        System.out.println(e);
+        LOGGER.log(Level.SEVERE, "Failed to write CSV", e);
       }
+      super.cancel();
     }
 
     @Override
     public String getName() {
-      return isScheduled() ? "Appending" : "Append";
+      return isScheduled() ? "Appending" : "Append to File";
-    }
-
-    @Override
-    public final boolean isFinished() {
-      return true;
     }
 
     @Override
@@ -161,12 +120,4 @@ public class ShooterTuner extends CommandBase {
       return true;
     }
   }
-
-  private class ShooterTableEditor implements Sendable {
-    @Override
-    public void initSendable(SendableBuilder builder) {
-      builder.addStringArrayProperty("distance", () -> new String[] { "hoodExt", "drumVelocity" }, ShooterTuner::setReadOnlyProperty);
-      Arrays.stream(m_boomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] { e.hoodExt, e.drumVelocity }, ShooterTuner::setReadOnlyProperty));
-    }
-  }
 }

src/main/java/frc4388/utility/SendableTable.java

@@ -0,0 +1,41 @@
+package frc4388.utility;
+
+import java.nio.ByteBuffer;
+import java.util.Arrays;
+import java.util.function.Supplier;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
+
+public class SendableTable implements Sendable {
+  private Supplier<ShooterTableEntry[]> m_table;
+  public SendableTable(Supplier<ShooterTableEntry[]> table) {
+    m_table = table;
+  }
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Table");
+    builder.addRawProperty("table", this::getTableAsBytes, null);
+    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];
+  }
+  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();
+    }
+  }
+}