Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup and fix shooter tuner

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-25 12:47:28 -06:00
parent a4f7282ecc
commit e0e3abea48
1 changed files with 113 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ShooterTuner.java
+113 -73
View File
@@ -3,15 +3,13 @@ package frc4388.robot.commands;
import java.io.File; import java.io.File;
import java.io.IOException; import java.io.IOException;
import java.io.OutputStream; import java.io.OutputStream;
import java.nio.file.FileSystem;
import java.nio.file.FileSystems; import java.nio.file.FileSystems;
import java.nio.file.Files; import java.nio.file.Files;
import java.nio.file.Path; import java.nio.file.Path;
import java.nio.file.StandardOpenOption; import java.nio.file.StandardOpenOption;
import java.nio.file.attribute.FileOwnerAttributeView; import java.nio.file.attribute.FileOwnerAttributeView;
import java.nio.file.attribute.UserPrincipal;
import java.nio.file.attribute.UserPrincipalLookupService;
import java.util.Arrays; import java.util.Arrays;
import java.util.logging.Logger;
import com.diffplug.common.base.Errors; import com.diffplug.common.base.Errors;
@@ -22,66 +20,86 @@ import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.CommandBase;
import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
import frc4388.robot.Robot;
import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
public class ShooterTuner extends CommandBase { public class ShooterTuner extends CommandBase {
private static final Logger LOGGER = Logger.getLogger(ShooterTuner.class.getSimpleName());
private static final Path PATH = new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath();
private final BoomBoom m_boomBoom; private final BoomBoom m_boomBoom;
private final Sendable m_shotEditor; private final ShotEditor m_shotEditor;
private final Sendable m_shotCsvAppender; private final CSVAppender m_shotCsvAppender;
private final Sendable m_shooterTableView; private final DistanceReader m_distanceReader;
// private final Sendable m_shooterTableUpdater; private final ShooterTableEditor m_shooterTableEditor;
private OutputStream csvOutputStream; private final DisabledInstantCommand m_shooterTableUpdater;
private final DisabledInstantCommand m_printCsvFile;
private final ShooterTableEntry tableOverrideEntry;
public ShooterTuner(BoomBoom boomBoom) { public ShooterTuner(BoomBoom boomBoom) {
m_boomBoom = boomBoom; m_boomBoom = boomBoom;
addRequirements(boomBoom);
setName("Enable");
m_shotEditor = new ShotEditor(); m_shotEditor = new ShotEditor();
m_shotCsvAppender = new PersistentInstantCommand(this::appendCsv).withName("Append"); m_shotCsvAppender = new CSVAppender();
m_shooterTableView = new ShooterTableEditor(); m_distanceReader = new DistanceReader();
// m_shooterTableUpdater = new m_shooterTableEditor = new ShooterTableEditor();
// PersistentInstantCommand(m_boomBoom::loadShooterTable).withName("Reload"); 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();
setName("Shooter Data Mode");
} }
private OutputStream getCsvOutputStream() { @Override
if (csvOutputStream == null) { public void initialize() {
Path path = new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(); var tab = Shuffleboard.getTab("Shooter Tuner");
if (RobotBase.isReal()) if (tab.getComponents().isEmpty()) {
Errors.log().run(() -> Files.getFileAttributeView(path, FileOwnerAttributeView.class).setOwner(FileSystems.getDefault().getUserPrincipalLookupService().lookupPrincipalByName("admin"))); var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 3);
csvOutputStream = Errors.rethrow().get(() -> Files.newOutputStream(path, StandardOpenOption.WRITE, StandardOpenOption.APPEND)); manual.add("Distance Reader", m_distanceReader);
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);
} }
return csvOutputStream; tableOverrideEntry.distance = 0.0;
tableOverrideEntry.hoodExt = 0.0;
tableOverrideEntry.drumVelocity = 0.0;
m_boomBoom.m_shooterTable = new ShooterTableEntry[] { tableOverrideEntry };
Shuffleboard.selectTab("Shooter Tuner");
} }
private class ShotEditor implements Sendable { @Override
@Override public final boolean isFinished() {
public void initSendable(SendableBuilder builder) { return true;
builder.setSmartDashboardType("RobotPreferences");
builder.addBooleanProperty("[Enabled]", ShooterTuner.this::isScheduled, b -> {
if (!b) cancel();
});
builder.addDoubleProperty("Drum Velocity", () -> m_boomBoom.m_shooterTable[0].drumVelocity, d -> m_boomBoom.m_shooterTable[0].drumVelocity = d);
builder.addDoubleProperty("Hood Extension", () -> m_boomBoom.m_shooterTable[0].hoodExt, d -> m_boomBoom.m_shooterTable[0].hoodExt = d);
builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println);
}
} }
private class ShooterTableEditor implements Sendable { @Override
@Override public String getName() {
public void initSendable(SendableBuilder builder) { return isOverridden() ? "Tuner Override" : "CSV File";
Arrays.stream(m_boomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] { e.hoodExt, e.drumVelocity }, a -> {
}));
}
} }
private class PersistentInstantCommand extends InstantCommand { private boolean isOverridden() {
public PersistentInstantCommand(Runnable toRun, Subsystem... requirements) { return m_boomBoom.m_shooterTable.length == 1 && m_boomBoom.m_shooterTable[0].equals(tableOverrideEntry);
}
@Override
public boolean runsWhenDisabled() {
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); super(toRun, requirements);
setName(name);
} }
@Override @Override
@@ -90,43 +108,65 @@ public class ShooterTuner extends CommandBase {
} }
} }
private void appendCsv() { private class DistanceReader extends CommandBase {
String s = String.format("%s,%s,%s%n", m_boomBoom.m_shooterTable[0].distance, m_boomBoom.m_shooterTable[0].hoodExt, m_boomBoom.m_shooterTable[0].drumVelocity); @Override
byte[] b = s.getBytes(); public void execute() {
Errors.log().run(() -> getCsvOutputStream().write(b)); tableOverrideEntry.distance = SmartDashboard.getNumber("Distance to Target", -1);
}
@Override
public String getName() {
return isScheduled() ? "Reading" : "Enable";
}
@Override
public boolean runsWhenDisabled() {
return true;
}
} }
@Override private class ShotEditor implements Sendable {
public void initialize() { @Override
setName("Disable"); public void initSendable(SendableBuilder builder) {
var tab = Shuffleboard.getTab("Shooter Tuner"); builder.setSmartDashboardType("RobotPreferences");
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 3); builder.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d);
manual.add("Manual Shooter Data", m_shotEditor); builder.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d);
manual.add("Shooter Table Appender", m_shotCsvAppender); builder.addDoubleProperty("Measured Distance", () -> tableOverrideEntry.distance, ShooterTuner::setReadOnlyProperty);
var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2, 0).withSize(4, 3); }
csv.add("Initial Shooter Data", m_shooterTableView);
// csv.add("Reload Data", m_shooterTableUpdater);
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;
dummyEntry.hoodExt = 0.0;
dummyEntry.drumVelocity = 0.0;
m_boomBoom.m_shooterTable = new ShooterTableEntry[] { dummyEntry };
Shuffleboard.selectTab("Shooter Tuner");
} }
@Override private class CSVAppender extends CommandBase {
public void execute() { @Override
m_boomBoom.m_shooterTable[0].distance = SmartDashboard.getNumber("Distance to Target", -1); public void initialize() {
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);
}
}
@Override
public String getName() {
return isScheduled() ? "Appending" : "Append";
}
@Override
public final boolean isFinished() {
return true;
}
@Override
public boolean runsWhenDisabled() {
return true;
}
} }
@Override private class ShooterTableEditor implements Sendable {
public void end(boolean interrupted) { @Override
Errors.log().run(getCsvOutputStream()::close); public void initSendable(SendableBuilder builder) {
m_boomBoom.loadShooterTable(); 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));
}
@Override
public boolean runsWhenDisabled() {
return true;
} }
} }