Merge branch 'shooter-tuner' into cleanup

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);

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.Arrays;
-import java.util.Map;
 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"));
+    }
+
   }
 
   /**

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<ShooterTableEntry> 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 };
     }
   }