Merge pull request #136 from TheGamer1002/main

Implement NetworkAlerts

src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java

@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;

src/main/java/swervelib/SwerveDrive.java

@@ -11,7 +11,6 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;

src/main/java/swervelib/SwerveModule.java

@@ -5,13 +5,13 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.math.SwerveMath;
 import swervelib.motors.SwerveMotor;
 import swervelib.parser.SwerveModuleConfiguration;
 import swervelib.simulation.SwerveModuleSimulation;
+import swervelib.telemetry.Alert;
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
@@ -61,6 +61,14 @@ public class SwerveModule
    * Encoder synchronization queued.
    */
   private       boolean                synchronizeEncoderQueued = false;
+  /**
+   * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails.
+   */
+  private       Alert                  encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING);
+  /**
+   * An {@link Alert} for if there is no Absolute Encoder on the module.
+   */
+  private       Alert                  noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, Alert.AlertType.WARNING);
 
   /**
    * Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -397,14 +405,11 @@ public void pushOffsetsToControllers()
       if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
       {
         angleOffset = 0;
-      } else
-      {
-        DriverStation.reportWarning(
-            "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false);
+      } else {
+        encoderOffsetWarning.set(true);
       }
-    } else
-    {
-      DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false);
+    } else {
+      noEncoderWarning.set(true);
     }
   }
 

src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java

@@ -1,8 +1,8 @@
 package swervelib.encoders;
 
 import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
+import swervelib.telemetry.Alert;
 
 /**
  * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
@@ -19,6 +19,16 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
    * Inversion state of the encoder.
    */
   private boolean     inverted = false;
+  /** An {@link Alert} for if the absolute encoder offset cannot be set. */
+  private Alert cannotSetOffset = new Alert(
+      "Encoders",
+      "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(),
+      Alert.AlertType.WARNING);
+  /** An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */
+  private Alert inaccurateVelocities = new Alert(
+      "Encoders",
+      "The Analog Absolute encoder may not report accurate velocities!",
+      Alert.AlertType.WARNING_TRACE);
 
   /**
    * Construct the Thrifty Encoder as a Swerve Absolute Encoder.
@@ -101,8 +111,7 @@ public Object getAbsoluteEncoder()
   public boolean setAbsoluteEncoderOffset(double offset)
   {
     //Do Nothing
-    DriverStation.reportWarning(
-        "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false);
+    cannotSetOffset.set(true);
     return false;
   }
 
@@ -114,7 +123,7 @@ public boolean setAbsoluteEncoderOffset(double offset)
   @Override
   public double getVelocity()
   {
-    DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true);
+    inaccurateVelocities.set(true);
     return encoder.getValue();
   }
 }

src/main/java/swervelib/encoders/CANCoderSwerve.java

@@ -9,7 +9,7 @@
 import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
 import com.ctre.phoenix6.signals.MagnetHealthValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import edu.wpi.first.wpilibj.DriverStation;
+import swervelib.telemetry.Alert;
 
 /**
  * Swerve Absolute Encoder for CTRE CANCoders.
@@ -21,6 +21,28 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
    * CANCoder with WPILib sendable and support.
    */
   public CANcoder encoder;
+  /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */
+  private Alert magnetFieldLessThanIdeal = new Alert(
+      "Encoders",
+      "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
+      Alert.AlertType.WARNING);
+  /** An {@link Alert} for if the CANCoder reading is faulty. */
+  private Alert readingFaulty = new Alert(
+      "Encoders",
+      "CANCoder " + encoder.getDeviceID() + " reading was faulty.",
+      Alert.AlertType.WARNING);
+  /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */
+  private Alert readingIgnored = new Alert(
+      "Encoders",
+      "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.",
+      Alert.AlertType.WARNING);
+  /** An {@link Alert} for if the absolute encoder offset cannot be set. */
+  private Alert cannotSetOffset = new Alert(
+      "Encoders",
+      "Failure to set CANCoder "
+          + encoder.getDeviceID()
+          + " Absolute Encoder Offset",
+      Alert.AlertType.WARNING);
 
   /**
    * Initialize the CANCoder on the standard CANBus.
@@ -91,15 +113,18 @@ public double getAbsolutePosition()
 
     if (strength != MagnetHealthValue.Magnet_Green)
     {
-      DriverStation.reportWarning(
-          "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
-    }
+      magnetFieldLessThanIdeal.set(true);
+    } else magnetFieldLessThanIdeal.set(false);
     if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
     {
       readingError = true;
-      DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false);
-//      return 0;
+      readingFaulty.set(true);
+      return 0;
+    }else
+    {
+      readingFaulty.set(false);
     }
+
     StatusSignal<Double> angle = encoder.getAbsolutePosition().refresh();
 
     // Taken from democat's library.
@@ -115,8 +140,8 @@ public double getAbsolutePosition()
     if (angle.getStatus() != StatusCode.OK)
     {
       readingError = true;
-      DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false);
-    }
+      readingIgnored.set(true);
+    } else readingIgnored.set(false);
 
     return angle.getValue() * 360;
   }
@@ -149,12 +174,16 @@ public boolean setAbsoluteEncoderOffset(double offset)
       return false;
     }
     error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
-    if (error == StatusCode.OK)
-    {
+    cannotSetOffset.setText(
+        "Failure to set CANCoder "
+            + encoder.getDeviceID()
+            + " Absolute Encoder Offset Error: "
+            + error);
+    if (error == StatusCode.OK) {
+      cannotSetOffset.set(false);
       return true;
     }
-    DriverStation.reportWarning(
-        "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false);
+    cannotSetOffset.set(true);
     return false;
   }
 

src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java

@@ -1,7 +1,7 @@
 package swervelib.encoders;
 
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import swervelib.telemetry.Alert;
 
 /**
  * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
@@ -22,6 +22,12 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
    * Inversion state.
    */
   private       boolean          isInverted;
+  /** An {@link Alert}  for if the encoder cannot report accurate velocities. */
+  private Alert inaccurateVelocities = new Alert(
+      "Encoders",
+      "The PWM Duty Cycle encoder may not report accurate velocities!",
+      Alert.AlertType.WARNING_TRACE);
+
 
   /**
    * Constructor for the PWM duty cycle encoder.
@@ -74,7 +80,7 @@ public Object getAbsoluteEncoder()
   @Override
   public double getVelocity()
   {
-    DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true);
+    inaccurateVelocities.set(true);
     return encoder.get();
   }
 

src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java

@@ -4,9 +4,10 @@
 import com.revrobotics.REVLibError;
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkAnalogSensor.Mode;
-import edu.wpi.first.wpilibj.DriverStation;
+
 import java.util.function.Supplier;
 import swervelib.motors.SwerveMotor;
+import swervelib.telemetry.Alert;
 
 /**
  * SparkMax absolute encoder, attached through the data port analog pin.
@@ -18,6 +19,17 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
    * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
    */
   public SparkAnalogSensor encoder;
+  /** An {@link Alert} for if there is a failure configuring the encoder. */
+  private Alert failureConfiguring = new Alert(
+      "Encoders",
+      "Failure configuring SparkMax Analog Encoder",
+      Alert.AlertType.WARNING_TRACE);
+  /** An {@link Alert} for if the absolute encoder does not support integrated offsets. */
+  private Alert doesNotSupportIntegratedOffsets = new Alert(
+      "Encoders",
+      "SparkMax Analog Sensors do not support integrated offsets",
+      Alert.AlertType.WARNING_TRACE);
+
 
   /**
    * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
@@ -50,7 +62,7 @@ private void configureSparkMax(Supplier<REVLibError> config)
         return;
       }
     }
-    DriverStation.reportWarning("Failure configuring encoder", true);
+    failureConfiguring.set(true);
   }
 
   /**
@@ -113,7 +125,7 @@ public Object getAbsoluteEncoder()
   @Override
   public boolean setAbsoluteEncoderOffset(double offset)
   {
-    DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true);
+    doesNotSupportIntegratedOffsets.set(true);
     return false;
   }
 

src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java

@@ -4,9 +4,10 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.SparkAbsoluteEncoder.Type;
-import edu.wpi.first.wpilibj.DriverStation;
+
 import java.util.function.Supplier;
 import swervelib.motors.SwerveMotor;
+import swervelib.telemetry.Alert;
 
 /**
  * SparkMax absolute encoder, attached through the data port.
@@ -18,6 +19,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
    * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
    */
   public AbsoluteEncoder encoder;
+  /** An {@link Alert} for if there is a failure configuring the encoder. */
+  private Alert failureConfiguring = new Alert(
+      "Encoders",
+      "Failure configuring SparkMax Analog Encoder",
+      Alert.AlertType.WARNING_TRACE);
+  private Alert offsetFailure = new Alert(
+      "Encoders",
+      "Failure to set Absolute Encoder Offset",
+      Alert.AlertType.WARNING_TRACE);
 
   /**
    * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor.
@@ -52,7 +62,7 @@ private void configureSparkMax(Supplier<REVLibError> config)
         return;
       }
     }
-    DriverStation.reportWarning("Failure configuring encoder", true);
+    failureConfiguring.set(true);
   }
 
   /**
@@ -124,7 +134,8 @@ public boolean setAbsoluteEncoderOffset(double offset)
         return true;
       }
     }
-    DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false);
+    offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error);
+    offsetFailure.set(true);
     return false;
   }
 

src/main/java/swervelib/imu/NavXSwerve.java

@@ -1,14 +1,14 @@
 package swervelib.imu;
 
 import com.kauailabs.navx.frc.AHRS;
-import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import swervelib.telemetry.Alert;
+
 import java.util.Optional;
 
 /**
@@ -25,6 +25,8 @@ public class NavXSwerve extends SwerveIMU
    * Offset for the NavX.
    */
   private Rotation3d offset = new Rotation3d();
+  /** An {@link Alert} for if there is an error instantiating the NavX. */
+  private Alert      navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE);
 
   /**
    * Constructor for the NavX swerve.
@@ -43,7 +45,8 @@ public NavXSwerve(SerialPort.Port port)
       SmartDashboard.putData(gyro);
     } catch (RuntimeException ex)
     {
-      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
+      navXError.setText("Error instantiating NavX: " + ex.getMessage());
+      navXError.set(true);
     }
   }
 
@@ -64,7 +67,8 @@ public NavXSwerve(SPI.Port port)
       SmartDashboard.putData(gyro);
     } catch (RuntimeException ex)
     {
-      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
+      navXError.setText("Error instantiating NavX: " + ex.getMessage());
+      navXError.set(true);
     }
   }
 
@@ -85,7 +89,8 @@ public NavXSwerve(I2C.Port port)
       SmartDashboard.putData(gyro);
     } catch (RuntimeException ex)
     {
-      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
+      navXError.setText("Error instantiating NavX: " + ex.getMessage());
+      navXError.set(true);
     }
   }
 

src/main/java/swervelib/math/SwerveMath.java

@@ -13,8 +13,6 @@
 import swervelib.SwerveModule;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.parser.SwerveModuleConfiguration;
-import swervelib.telemetry.SwerveDriveTelemetry;
-import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
 /**
  * Mathematical functions which pertain to swerve drive.