From ba60f44e67414064433f2586c9fdfdc84a930089 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Fri, 25 Mar 2022 08:32:58 -0600 Subject: [PATCH] Rewrite shooter tuner --- src/main/java/frc4388/robot/Robot.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 38 +----- .../frc4388/robot/commands/ShooterTuner.java | 117 ++++++++++++++++++ .../frc4388/robot/subsystems/BoomBoom.java | 12 +- 4 files changed, 130 insertions(+), 43 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShooterTuner.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4dba8bf..ce60173 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -96,9 +96,9 @@ public class Robot extends TimedRobot { Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); - System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString()); - System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString()); - System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString()); + // System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString()); + // System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString()); + // System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString()); // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5758dd9..1e5fa02 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -43,6 +43,7 @@ import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.PathRecorder; import frc4388.robot.commands.RunCommandForTime; +import frc4388.robot.commands.ShooterTuner; import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.TrackTarget; @@ -91,6 +92,8 @@ public class RobotContainer { /* Autonomous */ private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); + + private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom); // Controllers private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -273,40 +276,7 @@ public class RobotContainer { m_robotBoomBoom.setDefaultCommand( new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom) ); - - - if (m_robotBoomBoom.readManualData) { - var tab = Shuffleboard.getTab("Manual Shooter Data"); - var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withSize(2, 3); - manual.add("Manual Shooter Data", new Sendable() { - private double manualHoodExt; - private double manualDrumVel; - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("RobotPreferences"); - builder.addBooleanProperty("Disable CSV", () -> m_robotBoomBoom.readManualData, b -> m_robotBoomBoom.readManualData = b); - builder.addDoubleProperty("Drum Velocity", () -> manualDrumVel, d -> manualDrumVel = d); - builder.addDoubleProperty("Hood Extension", () -> manualHoodExt, d -> manualHoodExt = d); - builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println); - } - }); - manual.add("Shooter Table Appender", new InstantCommand(() -> Errors.log().run(() -> Files.write(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), String.format("%s,%s,%s%n", SmartDashboard.getNumber("Manual Shooter Data/Distance to Target", -1), SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", -1), SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", -1)).getBytes(), StandardOpenOption.WRITE, StandardOpenOption.APPEND))) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Append")); - var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2,0).withSize(4, 3); - csv.add("Initial Shooter Data", builder -> Arrays.stream(m_robotBoomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] {e.hoodExt, e.drumVelocity}, a -> {}))); - csv.add("Reload Data", new InstantCommand(m_robotBoomBoom::updateShooterTable) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Reload")); - } - + SmartDashboard.putData("Shooter Tuner", m_shooterTuner); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterTuner.java b/src/main/java/frc4388/robot/commands/ShooterTuner.java new file mode 100644 index 0000000..e5aab2f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterTuner.java @@ -0,0 +1,117 @@ +package frc4388.robot.commands; + +import java.io.File; +import java.io.OutputStream; +import java.nio.file.Files; +import java.nio.file.StandardOpenOption; +import java.util.Arrays; + +import com.diffplug.common.base.Errors; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Filesystem; +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 edu.wpi.first.wpilibj2.command.Subsystem; +import frc4388.robot.subsystems.BoomBoom; +import static frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; + +public class ShooterTuner extends CommandBase { + private final BoomBoom m_boomBoom; + private final Sendable m_shotEditor; + private final Sendable m_shotCsvAppender; + private final Sendable m_shooterTableView; + // private final Sendable m_shooterTableUpdater; + private OutputStream csvOutputStream; + + public ShooterTuner(BoomBoom boomBoom) { + m_boomBoom = boomBoom; + addRequirements(boomBoom); + setName("Enable"); + m_shotEditor = new ShotEditor(); + m_shotCsvAppender = new PersistentInstantCommand(this::appendCsv).withName("Append"); + m_shooterTableView = new ShooterTableEditor(); + // m_shooterTableUpdater = new PersistentInstantCommand(m_boomBoom::loadShooterTable).withName("Reload"); + + csvOutputStream = Errors.rethrow().get(() -> Files.newOutputStream(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), StandardOpenOption.WRITE, StandardOpenOption.APPEND)); + } + + private class ShotEditor implements Sendable { + @Override + public void initSendable(SendableBuilder builder) { + 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 + public void initSendable(SendableBuilder builder) { + 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 { + public PersistentInstantCommand(Runnable toRun, Subsystem... requirements) { + super(toRun, requirements); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + } + + private void appendCsv() { + 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 + ); + byte[] b = s.getBytes(); + Errors.log().run(() -> csvOutputStream.write(b)); + } + + @Override + public void initialize() { + setName("Disable"); + var tab = Shuffleboard.getTab("Shooter Tuner"); + var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 3); + 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.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 + public void execute() { + m_boomBoom.m_shooterTable[0].distance = SmartDashboard.getNumber("Distance to Target", -1); + } + + @Override + public void end(boolean interrupted) { + Errors.log().run(csvOutputStream::close); + m_boomBoom.loadShooterTable(); + } + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 0978374..0e81dfc 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import java.io.File; import java.io.IOException; +import java.util.Arrays; import java.util.Comparator; import java.util.Map; import java.util.Objects; @@ -31,7 +32,6 @@ import frc4388.utility.Gains; import frc4388.utility.NumericData; public class BoomBoom extends SubsystemBase { - public boolean readManualData = true; private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName()); public WPI_TalonFX m_shooterFalconLeft; public WPI_TalonFX m_shooterFalconRight; @@ -64,7 +64,7 @@ public class BoomBoom extends SubsystemBase { setShooterGains(); - updateShooterTable(); + loadShooterTable(); // Run a helper method that logs the contents of the table on a new thread. new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); } @@ -77,7 +77,7 @@ public class BoomBoom extends SubsystemBase { * @return Drum Velocity in units per 100 ms */ public Double getVelocity(final Double distance) { - return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } /** @@ -88,7 +88,7 @@ public class BoomBoom extends SubsystemBase { * @return Hood extension in units */ public Double getHood(final Double distance) { - return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } @Override @@ -140,7 +140,7 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } - public void updateShooterTable() { + public void loadShooterTable() { try { // This is a helper class that allows us to read a CSV file into a Java array. @@ -164,7 +164,7 @@ public class BoomBoom extends SubsystemBase { } }; // This is reading the CSV file into a Java array. - m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); + m_shooterTable = Arrays.stream(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;