Half-finished yourmomsmouth subsystem

Yourmomsmouth command + button bindings (UNFINISHED DO NOT BRANCH)

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

@@ -23,24 +23,27 @@
 
 @SuppressWarnings("java:S1118")
 /**
- * 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
+ * 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
  * globally (i.e. public static). Do not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
 
   public static final class Config {
     // maybe tune PID values?
-    public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG =
-        new HolonomicPathFollowerConfig(
-            new PIDConstants(20, 0, 0),
-            new PIDConstants(10, 0, 0),
-            Drive.MAX_VELOCITY_METERS_PER_SECOND,
-            Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
-            new ReplanningConfig());
+    public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig(
+        new PIDConstants(20, 0, 0),
+        new PIDConstants(10, 0, 0),
+        Drive.MAX_VELOCITY_METERS_PER_SECOND,
+        Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
+        new ReplanningConfig());
 
     /** turn this off before comp. */
     public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
@@ -51,11 +54,14 @@ public static final class Config {
     /** keep this on for pigeon, disable if absolutely necessary */
     public static final boolean FLIP_GYROSCOPE = true;
 
-    /** def turn this off unless you are using it, generates in excess of 100k rows for a match. */
+    /**
+     * def turn this off unless you are using it, generates in excess of 100k rows
+     * for a match.
+     */
     public static final boolean WRITE_APRILTAG_DATA = false;
 
-    public static final Path APRILTAG_DATA_PATH =
-        Filesystem.getDeployDirectory().toPath().resolve("poseEstimationsAtDistances.csv");
+    public static final Path APRILTAG_DATA_PATH = Filesystem.getDeployDirectory().toPath()
+        .resolve("poseEstimationsAtDistances.csv");
     public static final double REAL_X = 0.0;
     public static final double REAL_Y = 0.0;
   }
@@ -69,63 +75,63 @@ public static final class Drive {
     public static final double MAX_VOLTAGE = 12.0;
     // maximum velocity
     // FIXME measure this value experimentally
-    public static final double MAX_VELOCITY_METERS_PER_SECOND =
-        6380.0 // falcon 500 free speed rpm
-            / 60.0
-            * 0.10033
-            * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
-            * Math.PI;
+    public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 // falcon 500 free speed rpm
+        / 60.0
+        * 0.10033
+        * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
+        * Math.PI;
     // theoretical value
     // FIXME measure and validate experimentally
-    public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND =
-        MAX_VELOCITY_METERS_PER_SECOND
-            / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
-            * .5;
+    public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND
+        / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
+        * .5;
 
     /** the maximum amount of angular error pid loops will tolerate for rotation */
     public static final double ANGULAR_ERROR = 1.0;
-    /** the minimum magnitude of the right stick for it to be used as a new rotation angle */
+    /**
+     * the minimum magnitude of the right stick for it to be used as a new rotation
+     * angle
+     */
     public static final double ROTATE_VECTOR_MAGNITUDE = .7;
 
     public static final class Dims {
       // FIXME validate with hardware
-      public static final double TRACKWIDTH_METERS =
-          .5207; // 20.5 inches (source: cad) converted to meters
+      public static final double TRACKWIDTH_METERS = .5207; // 20.5 inches (source: cad) converted to meters
       public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
 
       public static final double BUMPER_WIDTH_METERS_X = .9779;
       public static final double BUMPER_WIDTH_METERS_Y = .8382;
     }
 
     /*
-     module layout:
-        |──────
-     |->│#   ##steer motor
-     │  │  ##cancoder
-     │  │##drive motor
-     module number
-
-     steer is always left
-     from corner perspective
-
-     robot visualization:
-    |──────────────────────|
-    │2   10          04   1│
-    │  25              24  │
-    │11     S      D     03│
-    │     D          S     │
-    │                      │
-    │                      │
-    │     S          D     │
-    │       D      S       │
-    │12    |────────|    02│
-    │  26  │        │  27  │
-    │3   13│  batt  │01   4│
-    |──────┴───┬┬───┴──────|
-               ││
-               ││
-               ▼▼
-         software front
+     * module layout:
+     * |──────
+     * |->│# ##steer motor
+     * │ │ ##cancoder
+     * │ │##drive motor
+     * module number
+     * 
+     * steer is always left
+     * from corner perspective
+     * 
+     * robot visualization:
+     * |──────────────────────|
+     * │2 10 04 1│
+     * │ 25 24 │
+     * │11 S D 03│
+     * │ D S │
+     * │ │
+     * │ │
+     * │ S D │
+     * │ D S │
+     * │12 |────────| 02│
+     * │ 26 │ │ 27 │
+     * │3 13│ batt │01 4│
+     * |──────┴───┬┬───┴──────|
+     * ││
+     * ││
+     * ▼▼
+     * software front
      */
 
     public static final class Modules {
@@ -134,14 +140,12 @@ public static final class Params {
         public static final double COUPLING_GEAR_RATIO = 3.125;
         public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
         public static final double STEER_GEAR_RATIO = 21.428571428571427;
-        public static final Slot0Configs DRIVE_MOTOR_GAINS =
-            new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0.32).withKV(0.11).withKA(0);
-        public static final Slot0Configs STEER_MOTOR_GAINS =
-            new Slot0Configs().withKP(11).withKI(0).withKD(0).withKS(0.4).withKV(0.6).withKA(0);
-        public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT =
-            ClosedLoopOutputType.Voltage;
-        public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT =
-            ClosedLoopOutputType.Voltage;
+        public static final Slot0Configs DRIVE_MOTOR_GAINS = new Slot0Configs().withKP(3).withKI(0).withKD(0)
+            .withKS(0.32).withKV(0.11).withKA(0);
+        public static final Slot0Configs STEER_MOTOR_GAINS = new Slot0Configs().withKP(11).withKI(0).withKD(0)
+            .withKS(0.4).withKV(0.6).withKA(0);
+        public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
+        public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
         public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
         public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
         public static final double SLIP_CURRENT = 0; // optional, unused rn

src/main/java/frc/robot/Main.java

@@ -7,17 +7,21 @@
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what
+ * you are doing, do not modify this file except to change the parameter class
+ * to the startRobot
  * call.
  */
 public final class Main {
-  private Main() {}
+  private Main() {
+  }
 
   /**
    * Main initialization function. Do not perform any initialization here.
    *
-   * <p>If you change your main robot class, change the parameter type.
+   * <p>
+   * If you change your main robot class, change the parameter type.
    */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);

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

@@ -10,9 +10,12 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the
+ * name of this class or
+ * the package after creating this project, you must also update the
+ * build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -23,30 +26,39 @@ public class Robot extends TimedRobot {
   private boolean matchStartingMethodCalled = false;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
+   * This function is run when the robot is first started up and should be used
+   * for any
    * initialization code.
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
 
     LiveWindow.disableAllTelemetry();
   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   * This function is called every robot packet, no matter the mode. Use this for
+   * items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and
+   * test.
    *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and
    * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // and running subsystem periodic() methods. This must be called from the
+    // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
@@ -62,7 +74,10 @@ public void disabledPeriodic() {
     // This method intentionally left empty
   }
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
   @Override
   public void autonomousInit() {
     // trigger the match starting method