Feat/elevator subsystem

.github/workflows/build.yml

@@ -34,4 +34,6 @@ jobs:
           git commit -m "auto generated"
           git push
       - name: Build Robot Code
-        run: ./gradlew build
+        run: ./gradlew build
+      - name: Run Tests
+        run: ./gradlew test

build.gradle

@@ -35,11 +35,19 @@ deploy {
                     jvmArgs.add("-XX:+UseSerialGC")
                     jvmArgs.add("-XX:MaxGCPauseMillis=50")
 
+                    // Enable VisualVM connection
+                    jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                    jvmArgs.add("-Djava.rmi.server.hostname=10.64.23.2")
+
                     // RIO 2.0 ONLY
-                    final MAX_JAVA_HEAP_SIZE_MB = 100;
-                    jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
-                    jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
-                    jvmArgs.add("-XX:+AlwaysPreTouch")
+                    // final MAX_JAVA_HEAP_SIZE_MB = 100;
+                    // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    // jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    // jvmArgs.add("-XX:+AlwaysPreTouch")
                 }
 
 

src/main/java/org/frc6423/lib/types/CanDeviceId.java

@@ -6,6 +6,7 @@
 
 package org.frc6423.lib.types;
 
+@Deprecated
 public class CanDeviceId {
   private final String busName;
   private final int canId;

src/main/java/org/frc6423/lib/utilities/TestUtils.java

@@ -6,13 +6,18 @@
 
 package org.frc6423.lib.utilities;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.frc6423.robot.Constants.Flags;
 
-/** Contains static methods for creating tests */
+/** Contains static methods for creating tests "Inspired" by FRC 1155 */
 public class TestUtils {
   /** Set up DS and initalizes HAL with default values and asserts that it doesn't fail */
   public static void setupTest() {
@@ -38,4 +43,67 @@ public static void reset(AutoCloseable... subsystems) throws Exception {
       subsystem.close();
     }
   }
+
+  /**
+   * Runs CommandScheduler and updates timer repeatedly to fast forward subsystems and run commands.
+   *
+   * @param ticks The number of times CommandScheduler is run
+   */
+  public static void fastForward(int ticks) {
+    for (int i = 0; i < ticks; i++) {
+      CommandScheduler.getInstance().run();
+      SimHooks.stepTiming(Flags.PERIOD.in(Seconds));
+    }
+  }
+
+  /**
+   * Fasts forward in time by running CommandScheduler and updating timer.
+   *
+   * @param time
+   */
+  public static void fastForward(Time time) {
+    fastForward((int) (time.in(Seconds) / Flags.PERIOD.in(Seconds)));
+  }
+
+  /**
+   * Runs CommandScheduler and updates timer to fast forward subsystems by 4 seconds and run
+   * commands.
+   */
+  public static void fastForward() {
+    fastForward(Seconds.of(4));
+  }
+
+  /**
+   * Schedules and runs a command.
+   *
+   * @param command The command to run.
+   */
+  public static void run(Command command) {
+    run(command, 1);
+  }
+
+  /**
+   * Schedules and runs a command.
+   *
+   * @param command The command to run.
+   * @param runs The number of times CommandScheduler is run.
+   */
+  public static void run(Command command, int runs) {
+    command.schedule();
+    fastForward(runs);
+  }
+
+  /**
+   * Schedules a command and runs it until it ends. Be careful -- if the command you give never
+   * ends, this will be an infinite loop!
+   *
+   * @param command
+   */
+  public static void runToCompletion(Command command) {
+    command.schedule();
+    fastForward(1);
+    while (command.isScheduled()) {
+      fastForward(1);
+    }
+  }
 }

src/main/java/org/frc6423/robot/Constants.java

@@ -28,6 +28,7 @@ public class Flags {
   }
 
   /** Constants representing port IDs that devices are connected to */
+  @Deprecated
   public class Ports {
     // * CANIVORE LOOP
     public static final CANBus CANCHAN = new CANBus("CANchan"); // :3

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivot.java

@@ -47,10 +47,9 @@ public class ArmPivot extends SubsystemBase implements AutoCloseable {
 
   private final LinearFilter pivotCurrentFilter = LinearFilter.movingAverage(5);
 
-  @Logged(name = "Filted Pivot Motor Current (Amps)")
+  @Logged(name = "Filted Pivot Motor Stator Current (Amps)")
   private double filteredPivotCurrent;
 
-  @Logged(name = "Is Zeroed (bool)")
   private boolean isZeroed = false;
 
   private final Mechanism2d canvas =
@@ -78,6 +77,14 @@ public void periodic() {
         Rotation2d.fromRadians(hardware.getAngleRads()).unaryMinus().rotateBy(Rotation2d.k180deg));
   }
 
+  /**
+   * @return true if arm has been homed
+   */
+  @Logged(name = "Is Zeroed (bool)")
+  public boolean isZeroed() {
+    return isZeroed;
+  }
+
   /**
    * @return true if arm is within a certain tolerance of the setpoint angle
    */
@@ -94,10 +101,7 @@ public boolean isNearSetpointAngle() {
    */
   // TODO CHECK VALUES
   public Command runCurrentHoming() {
-    return this.run(
-            () -> {
-              hardware.setVolts(-2.5);
-            })
+    return this.run(() -> hardware.setVolts(-2.5))
         .until(() -> Math.abs(filteredPivotCurrent) > 50.0)
         .finallyDo(
             (interupted) -> {

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivotIO.java

@@ -32,7 +32,7 @@ public interface ArmPivotIO extends AutoCloseable {
   public double getStatorCurrentAmps();
 
   /**
-   * Reset relative encoder to specified position
+   * Reset relative encoder of pivot motor to specified position
    *
    * @param poseRads position to reset to in radians
    */

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivotIOSim.java

@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+/** Simulated {@link ArmPivotIOReal} */
 public class ArmPivotIOSim implements ArmPivotIO {
   private final DCMotor pivotModel = DCMotor.getKrakenX60Foc(1);
   // TODO stddevs