Shooter done but not yet tested

networktables.json

@@ -0,0 +1 @@
+[]

src/main/java/frc/lib/advantagekit/LoggedIO.java

@@ -0,0 +1,5 @@
+package frc.lib.advantagekit;
+
+public interface LoggedIO<T> {
+  public void updateInputs(T inputs);
+}

src/main/java/frc/lib/advantagekit/LoggerUtil.java

@@ -0,0 +1,58 @@
+package frc.lib.advantagekit;
+
+import frc.robot.BuildConstants;
+// import frc.robot.BuildConstants;
+import frc.robot.Robot;
+import java.io.File;
+import java.nio.file.Path;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+public class LoggerUtil {
+  public static final String kLogDirectory = "advantagekit/logs";
+
+  public static void initializeLogger() {
+
+    // Record metadata
+    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    Logger.recordMetadata("GitDirty", getGitDirtyString());
+
+    // Set up data receivers & replay source
+    if (Robot.isSimulation()) {
+      Logger.addDataReceiver(new WPILOGWriter(getLogPath()));
+      Logger.addDataReceiver(new NT4Publisher());
+    } else {
+      Logger.addDataReceiver(new WPILOGWriter("/media/sda1/"));
+      Logger.addDataReceiver(new NT4Publisher());
+    }
+
+    // Start AdvantageKit logger
+    Logger.start();
+  }
+
+  private static String getGitDirtyString() {
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        return "All changes committed";
+      case 1:
+        return "Uncomitted changes";
+      default:
+        return "Unknown";
+    }
+  }
+
+  private static String getLogPath() {
+    File file = Path.of(kLogDirectory).toFile();
+
+    if (!file.exists()) {
+      file.mkdirs();
+    }
+
+    return file.getAbsolutePath();
+  }
+}

src/main/java/frc/lib/utils/TunableNumber.java

@@ -0,0 +1,90 @@
+package frc.lib.utils;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class TunableNumber {
+  private static final String tableKey = "TunableNumbers";
+
+  private String key;
+  private double defaultValue;
+  private double lastHasChangedValue = defaultValue;
+
+  /**
+   * Create a new TunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public TunableNumber(String tableString, String dashboardKey) {
+    this.key = tableString + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new TunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public TunableNumber(String dashboardKey, double defaultValue) {
+    this(tableKey, dashboardKey);
+    setDefault(defaultValue);
+  }
+
+  public TunableNumber(String dashboardKey, double defaultValue, String tableKey) {
+    this(tableKey, dashboardKey);
+    setDefault(defaultValue);
+  }
+
+  /**
+   * Get the default value for the number that has been set
+   *
+   * @return The default value
+   */
+  public double getDefault() {
+    return defaultValue;
+  }
+
+  /**
+   * Set the default value of the number
+   *
+   * @param defaultValue The default value
+   */
+  public void setDefault(double defaultValue) {
+    this.defaultValue = defaultValue;
+    if (Constants.tuningMode) {
+      // This makes sure the data is on NetworkTables but will not change it
+      SmartDashboard.putNumber(key, SmartDashboard.getNumber(key, defaultValue));
+    } else {
+      SmartDashboard.clearPersistent(key);
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode
+   *
+   * @return The current value
+   */
+  public double get() {
+    return Constants.tuningMode ? SmartDashboard.getNumber(key, defaultValue) : defaultValue;
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise
+   */
+  public boolean hasChanged() {
+    double currentValue = get();
+    if (currentValue != lastHasChangedValue) {
+      lastHasChangedValue = currentValue;
+      return true;
+    }
+
+    return false;
+  }
+}

src/main/java/frc/robot/Constants.java

@@ -13,6 +13,12 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.util.Units;
+import frc.lib.utils.TunableNumber;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -22,16 +28,50 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final boolean tuningMode = false;
+
+  public static final class ShooterConstants {
+    // Shooter pivot
+    public static final double kPivotGearRatio = 46.722;
+    // TODO: untuned values, fix later
+    public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(70);
+    public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(30);
+    public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(45);
+
+    public static final TunableNumber kPivotP = new TunableNumber("Shooter Pivot P", 1.0);
+    public static final TunableNumber kPivotI = new TunableNumber("Shooter Pivot I", 0.0);
+    public static final TunableNumber kPivotD = new TunableNumber("Shooter Pivot D", 0.0);
+    public static final double kPivotVelocity = 5.0;
+    public static final double kPivotAcceleration = 10.0;
+    public static final ProfiledPIDController kPivotController =
+        new ProfiledPIDController(
+            kPivotP.get(),
+            kPivotI.get(),
+            kPivotD.get(),
+            new Constraints(kPivotVelocity, kPivotAcceleration));
 
-  public static enum Mode {
-    /** Running on a real robot. */
-    REAL,
+    // Shooter flywheel
+    public static final double kFlywheelDiameter = Units.inchesToMeters(4.0);
+
+    public static final TunableNumber kFlywheelP = new TunableNumber("Shooter Flywheel P", 5.0);
+    public static final TunableNumber kFlywheelI = new TunableNumber("Shooter Flywheel I", 0.0);
+    public static final TunableNumber kFlywheelD = new TunableNumber("Shooter Flywheel D", 0.0);
+    public static final double kFlywheelVelocity = 5.0;
+    public static final double kFlywheelAcceleration = 10.0;
+    public static final ProfiledPIDController kFlywheelController =
+        new ProfiledPIDController(
+            kFlywheelP.get(),
+            kFlywheelI.get(),
+            kFlywheelD.get(),
+            new Constraints(kFlywheelVelocity, kFlywheelAcceleration));
+  }
 
-    /** Running a physics simulator. */
-    SIM,
+  public static final class Ports {
+    public static final int kShooterPivotLeader = 0;
+    public static final int kShooterPivotFollower = 0;
+    public static final int kShooterPivotEncoder = 0;
 
-    /** Replaying from a log file. */
-    REPLAY
+    public static final int kFlywheelLeft = 0;
+    public static final int kFlywheelRight = 0;
   }
 }

src/main/java/frc/robot/Robot.java

@@ -13,14 +13,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import org.littletonrobotics.junction.LogFileUtil;
+import frc.lib.advantagekit.LoggerUtil;
 import org.littletonrobotics.junction.LoggedRobot;
-import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.NT4Publisher;
-import org.littletonrobotics.junction.wpilog.WPILOGReader;
-import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -38,52 +35,14 @@ public class Robot extends LoggedRobot {
    */
   @Override
   public void robotInit() {
-    // Record metadata
-    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
-    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
-    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
-    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
-    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
-    switch (BuildConstants.DIRTY) {
-      case 0:
-        Logger.recordMetadata("GitDirty", "All changes committed");
-        break;
-      case 1:
-        Logger.recordMetadata("GitDirty", "Uncomitted changes");
-        break;
-      default:
-        Logger.recordMetadata("GitDirty", "Unknown");
-        break;
-    }
 
-    // Set up data receivers & replay source
-    switch (Constants.currentMode) {
-      case REAL:
-        // Running on a real robot, log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new WPILOGWriter());
-        Logger.addDataReceiver(new NT4Publisher());
-        break;
-
-      case SIM:
-        // Running a physics simulator, log to NT
-        Logger.addDataReceiver(new NT4Publisher());
-        break;
-
-      case REPLAY:
-        // Replaying a log, set up replay source
-        setUseTiming(false); // Run as fast as possible
-        String logPath = LogFileUtil.findReplayLog();
-        Logger.setReplaySource(new WPILOGReader(logPath));
-        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
-        break;
-    }
+    LoggerUtil.initializeLogger();
+
+    DataLogManager.logNetworkTables(true);
 
     // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit.
     // Logger.disableDeterministicTimestamps()
 
-    // Start AdvantageKit logger
-    Logger.start();
-
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
@@ -111,7 +70,7 @@ public void disabledPeriodic() {}
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
+    // autonomousCommand = robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
     if (autonomousCommand != null) {