Adding utils (motor,diag, and more).

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

@@ -59,10 +59,13 @@ public enum Mode {
     public static final double TILT_GEARING = 45.0;
     public static final boolean TILT_SIMULATE_GRAVITY = false;
 
+    public static final boolean TUNING_MODE = false;
     public static final boolean ARM_DEBUG = true;
     public static final double DRIVE_BASE_WIDTH = 0.635;
     public static final double DRIVE_BASE_LENGTH = 0.635;
     public static final double INITIAL_ROBOT_HEIGHT = 0;
 
+    public static final double GRAVITY = 9.81;
+
 }
 

src/main/java/frc/robot/utils/diag/DiagBoolean.java

@@ -0,0 +1,68 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Abstract diagnostics class for boolean sensors using DigitalInput The diagnostics will turn green
+ * once the switch has changes to reflect both positions: ON and OFF
+ */
+public abstract class DiagBoolean implements Diagnosable {
+
+  private String title;
+  private String name;
+  private GenericEntry networkTableEntry;
+
+  private boolean seenFalse;
+  private boolean seenTrue;
+
+  /**
+   * Constructor
+   *
+   * @param name the name of the unit. Will be used on the Shuffleboard
+   */
+  public DiagBoolean(String title, String name) {
+    this.title = title;
+    this.name = name;
+
+    reset();
+  }
+
+  @Override
+  public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) {
+    networkTableEntry =
+        shuffleBoardTab
+            .getLayout(title, BuiltInLayouts.kList)
+            .withSize(width, height)
+            .add(name, false)
+            .getEntry();
+  }
+
+  @Override
+  public void refresh() {
+    if (networkTableEntry != null) {
+      networkTableEntry.setBoolean(getDiagResult());
+    }
+  }
+
+  @Override
+  public void reset() {
+    seenFalse = seenTrue = false;
+  }
+
+  protected abstract boolean getValue();
+
+  // Package protected
+  boolean getDiagResult() {
+    boolean currentValue = getValue();
+    // Set the value for the state - whether the switch is pressed or not
+    if (currentValue) {
+      seenTrue = true;
+    } else {
+      seenFalse = true;
+    }
+
+    return seenTrue && seenFalse;
+  }
+}

src/main/java/frc/robot/utils/diag/DiagDistanceTraveled.java

@@ -0,0 +1,60 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Base class for Diagnosable that have an initial value (their "current" value) and that are
+ * required to change their value by a certain amount from that
+ */
+public abstract class DiagDistanceTraveled implements Diagnosable {
+
+  protected String name;
+  protected String title;
+  protected double requiredTravel;
+  protected GenericEntry networkTableEntry;
+  private double initialValue;
+  private boolean traveledDistance;
+
+  public DiagDistanceTraveled(String title, String name, double requiredTravel) {
+    this.title = title;
+    this.name = name;
+    this.requiredTravel = requiredTravel;
+  }
+
+  @Override
+  public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) {
+    networkTableEntry =
+        shuffleBoardTab
+            .getLayout(title, BuiltInLayouts.kList)
+            .withSize(width, height)
+            .add(name, false)
+            .getEntry();
+  }
+
+  @Override
+  public void refresh() {
+    if (networkTableEntry != null) {
+      networkTableEntry.setBoolean(getDiagResult());
+    }
+  }
+
+  @Override
+  public void reset() {
+    traveledDistance = false;
+    initialValue = getCurrentValue();
+  }
+
+  boolean getDiagResult() {
+    double currentValue = getCurrentValue();
+
+    if (Math.abs(currentValue - initialValue) >= requiredTravel) {
+      traveledDistance = true;
+    }
+
+    return this.traveledDistance;
+  }
+
+  protected abstract double getCurrentValue();
+}

src/main/java/frc/robot/utils/diag/DiagDutyCycleEncoder.java

@@ -0,0 +1,33 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+/**
+ * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has
+ * traveled at least a given distance from its initial position (measured at initialization or after
+ * a reset)
+ */
+public class DiagDutyCycleEncoder extends DiagDistanceTraveled {
+
+  private DutyCycleEncoder encoder;
+
+  /**
+   * Constructor
+   *
+   * @param name - the name of the unit. Will be used on the Shuffleboard
+   * @param requiredTravel - the required difference between the initial position to qualify for
+   *     success
+   * @param encoder - the encoder instance to test
+   */
+  public DiagDutyCycleEncoder(
+      String title, String name, double requiredTravel, DutyCycleEncoder encoder) {
+    super(title, name, requiredTravel);
+    this.encoder = encoder;
+    reset();
+  }
+
+  @Override
+  protected double getCurrentValue() {
+    return encoder.get();
+  }
+}

src/main/java/frc/robot/utils/diag/DiagEncoder.java

@@ -0,0 +1,32 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.wpilibj.Encoder;
+
+/**
+ * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has
+ * traveled at least a given distance from its initial position (measured at initialization or after
+ * a reset)
+ */
+public class DiagEncoder extends DiagDistanceTraveled {
+
+  private Encoder encoder;
+
+  /**
+   * Constructor
+   *
+   * @param name - the name of the unit. Will be used on the Shuffleboard
+   * @param requiredTravel - the required difference between the initial position to qualify for
+   *     success
+   * @param encoder - the encoder instance to test
+   */
+  public DiagEncoder(String title, String name, double requiredTravel, Encoder encoder) {
+    super(title, name, requiredTravel);
+    this.encoder = encoder;
+    reset();
+  }
+
+  @Override
+  protected double getCurrentValue() {
+    return encoder.get();
+  }
+}

src/main/java/frc/robot/utils/diag/DiagGyro.java

@@ -0,0 +1,18 @@
+package frc.robot.utils.diag;
+
+import frc.robot.utils.logging.input.GyroValues;
+import frc.robot.utils.logging.io.gyro.ThreadedGyro;
+
+public class DiagGyro extends DiagDistanceTraveled {
+  private final ThreadedGyro gyro;
+
+  public DiagGyro(String title, String name, double requiredTravel, ThreadedGyro gyro) {
+    super(title, name, requiredTravel);
+    this.gyro = gyro;
+  }
+//broken currently fix return
+  @Override
+  protected double getCurrentValue() {
+    return 0;
+  }
+}

src/main/java/frc/robot/utils/diag/DiagLimelight.java

@@ -0,0 +1,47 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+public class DiagLimelight implements Diagnosable {
+  private DoubleSubscriber subscriber;
+  private String title;
+  private String name;
+  private GenericEntry networkTableEntry;
+
+  public DiagLimelight(String title, String name) {
+    this.title = title;
+    this.name = name;
+    reset();
+  }
+
+  protected boolean getDiagResult() {
+    return subscriber.get() == 1;
+  }
+
+  @Override
+  public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) {
+    networkTableEntry =
+        shuffleBoardTab
+            .getLayout(title, BuiltInLayouts.kList)
+            .withSize(width, height)
+            .add(name, false)
+            .getEntry();
+  }
+
+  @Override
+  public void refresh() {
+    if (networkTableEntry != null) {
+      networkTableEntry.setBoolean(getDiagResult());
+    }
+  }
+
+  @Override
+  public void reset() {
+    subscriber =
+        NetworkTableInstance.getDefault().getTable("limelight").getDoubleTopic("tv").subscribe(-1);
+  }
+}

src/main/java/frc/robot/utils/diag/DiagMinMax.java

@@ -0,0 +1,71 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Abstract diagnostics class for devices that need a minimum and maximum reading to be tested. The
+ * diagnostics will turn green once the device has reached both points.
+ */
+public abstract class DiagMinMax implements Diagnosable {
+
+  private String name;
+  private String title;
+  private double minValue;
+  private double maxValue;
+  private GenericEntry networkTableEntry;
+
+  private boolean seenMinValue;
+  private boolean seenMaxValue;
+
+  /**
+   * Constructor
+   *
+   * @param name - the name of the unit. Will be used on the Shuffleboard
+   * @param minValue - the minimum value the object needs to hit to qualify for success
+   * @param maxValue - the maximum value the object needs to hit to qualify for success
+   */
+  public DiagMinMax(String title, String name, double minValue, double maxValue) {
+    this.name = name;
+    this.title = title;
+    this.minValue = minValue;
+    this.maxValue = maxValue;
+
+    reset();
+  }
+
+  @Override
+  public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) {
+    networkTableEntry =
+        shuffleBoardTab
+            .getLayout(title, BuiltInLayouts.kList)
+            .withSize(width, height)
+            .add(name, false)
+            .getEntry();
+  }
+
+  @Override
+  public void refresh() {
+    if (networkTableEntry != null) {
+      networkTableEntry.setBoolean(getDiagResult(getSensorReading()));
+    }
+  }
+
+  @Override
+  public void reset() {
+    seenMinValue = seenMaxValue = false;
+  }
+
+  abstract double getSensorReading();
+
+  boolean getDiagResult(double value) {
+    if (value >= maxValue) {
+      seenMaxValue = true;
+    } else if (value <= minValue) {
+      seenMinValue = true;
+    }
+
+    return seenMaxValue && seenMinValue;
+  }
+}

src/main/java/frc/robot/utils/diag/DiagPot.java

@@ -0,0 +1,31 @@
+package frc.robot.utils.diag;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+
+/**
+ * A diagnostics class for analog potentiometer. It is a DiagMinMax object. Give it the maximum and
+ * minimum voltages for testing with.
+ */
+public class DiagPot extends DiagMinMax {
+
+  private AnalogPotentiometer pot;
+
+  /**
+   * Constructor
+   *
+   * @param name - the name of the unit. Will be used on the Shuffleboard
+   * @param minVoltage - the minimum value the pot needs to hit to qualify for success
+   * @param maxVoltage - the maximum value the pot needs to hit to qualify for success
+   * @param pot - the pot instance to test
+   */
+  public DiagPot(
+      String title, String name, double minVoltage, double maxVoltage, AnalogPotentiometer pot) {
+    super(title, name, minVoltage, maxVoltage);
+    this.pot = pot;
+  }
+
+  @Override
+  double getSensorReading() {
+    return pot.get();
+  }
+}