diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index f9cdd12..ce60173 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -5,32 +5,20 @@ package frc4388.robot; import java.io.File; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.HashMap; -import java.util.Map; import java.util.logging.Level; import java.util.logging.Logger; import com.diffplug.common.base.Errors; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.drive.Vector2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; import frc4388.utility.Vector2D; -import frc4388.utility.VelocityCorrection; -import frc4388.utility.desmos.DesmosServer; /** * The VM is configured to automatically run this class, and to call the @@ -85,45 +73,6 @@ public class Robot extends TimedRobot { m_robotContainer = new RobotContainer(); // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod); SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData("JVM Memory", new RunCommand(() -> { - }) { - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public String getName() { - if (isScheduled()) { - Runtime runtime = Runtime.getRuntime(); - long totalMemory = runtime.totalMemory() / 1_000_000; - long freeMemory = runtime.freeMemory() / 1_000_000; - long maxMemory = runtime.maxMemory() / 1_000_000; - return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB"; - } - return "Not Running"; - } - }); - SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> { - }) { - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public String getName() { - if (isScheduled()) { - File deploy = Filesystem.getDeployDirectory(); - long usedSpace = Errors.suppress().getWithDefault( - () -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), - 0l) / 1_000_000; - long usableSpace = deploy.getUsableSpace() / 1_000_000; - return usedSpace + " MB / " + usableSpace + " MB"; - } - return "Not Running"; - } - }); // desmosServer.start(); m_robotContainer.m_robotVisionOdometry.setLEDs(true); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00fd0ea..54d5714 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,12 +4,15 @@ package frc4388.robot; +import java.io.File; +import java.nio.file.Files; +import java.nio.file.StandardOpenOption; import java.util.ArrayList; -import java.util.HashMap; -import java.util.Map; +import java.util.Arrays; import java.util.Objects; import java.util.logging.Logger; +import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -19,14 +22,19 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +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.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -37,7 +45,6 @@ import frc4388.robot.commands.PathRecorder; import frc4388.robot.commands.RunCommandForTime; import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Claws; @@ -270,6 +277,39 @@ public class RobotContainer { 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")); + } + } /** diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 51f675d..0978374 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -31,6 +31,7 @@ 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; @@ -54,7 +55,7 @@ public class BoomBoom extends SubsystemBase { public Double distance, hoodExt, drumVelocity; } - private ShooterTableEntry[] m_shooterTable; + public ShooterTableEntry[] m_shooterTable; /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -63,7 +64,7 @@ public class BoomBoom extends SubsystemBase { setShooterGains(); - m_shooterTable = readShooterTable(); + updateShooterTable(); // 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(); } @@ -76,7 +77,7 @@ public class BoomBoom extends SubsystemBase { * @return Drum Velocity in units per 100 ms */ public Double getVelocity(final Double distance) { - return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } /** @@ -87,7 +88,7 @@ public class BoomBoom extends SubsystemBase { * @return Hood extension in units */ public Double getHood(final Double distance) { - return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } @Override @@ -139,7 +140,8 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } - private static ShooterTableEntry[] readShooterTable() { + public void updateShooterTable() { + try { // This is a helper class that allows us to read a CSV file into a Java array. CSV csv = new CSV<>(ShooterTableEntry::new) { @@ -162,14 +164,14 @@ public class BoomBoom extends SubsystemBase { } }; // This is reading the CSV file into a Java array. - return csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); } catch (IOException exception) { ShooterTableEntry dummyEntry = new ShooterTableEntry(); dummyEntry.distance = 0.0; dummyEntry.hoodExt = 0.0; dummyEntry.drumVelocity = 0.0; LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); - return new ShooterTableEntry[] { dummyEntry }; + m_shooterTable = new ShooterTableEntry[] { dummyEntry }; } }