diff --git a/src/main/java/frc4388/robot/commands/ShooterTuner.java b/src/main/java/frc4388/robot/commands/ShooterTuner.java index 51d1e73..bc8c8d6 100644 --- a/src/main/java/frc4388/robot/commands/ShooterTuner.java +++ b/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); - manual.add("Distance Reader", m_distanceReader); + var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5); manual.add("Manual Shooter Data", m_shotEditor); - manual.add("Shooter Table Appender", m_shotCsvAppender); - var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2, 0).withSize(4, 3); - csv.addBoolean("Is Shooter Data Overridden", this::isOverridden); - csv.add("Shooter Data (Broken)", m_shooterTableEditor); - csv.add("Shooter CSV Loader", m_shooterTableUpdater); - csv.add("Print CSV File", m_printCsvFile); + 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); } 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"; - } - - private boolean isOverridden() { - return m_boomBoom.m_shooterTable.length == 1 && m_boomBoom.m_shooterTable[0].equals(tableOverrideEntry); + return isScheduled() ? "Enabled" : "Disabled"; } @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"; - } - - @Override - public final boolean isFinished() { - return true; + return isScheduled() ? "Appending" : "Append to File"; } @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)); - } - } } diff --git a/src/main/java/frc4388/utility/SendableTable.java b/src/main/java/frc4388/utility/SendableTable.java new file mode 100644 index 0000000..871546c --- /dev/null +++ b/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 m_table; + public SendableTable(Supplier 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(); + } + } +}