Adding PigeonViaTalonSRX and attached SparkFlex

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -127,6 +127,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
                                   Constants.MAX_SPEED,
                                   new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
                                              Rotation2d.fromDegrees(0)));
+
   }
 
   /**

src/main/java/swervelib/SwerveDrive.java

@@ -11,6 +11,7 @@
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
 
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -58,12 +59,14 @@
 import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
 import swervelib.encoders.CANCoderSwerve;
 import swervelib.imu.Pigeon2Swerve;
+import swervelib.imu.PigeonViaTalonSRXSwerve;
 import swervelib.imu.SwerveIMU;
 import swervelib.math.SwerveMath;
 import swervelib.motors.TalonFXSwerve;
 import swervelib.parser.Cache;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
+import swervelib.parser.SwerveParser;
 import swervelib.simulation.SwerveIMUSimulation;
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -944,7 +947,6 @@ public SwerveModulePosition[] getModulePositions()
     }
     return positions;
   }
-
   /**
    * Getter for the {@link SwerveIMU}.
    *

src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java

@@ -0,0 +1,170 @@
+package swervelib.encoders;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.REVLibError;
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import swervelib.motors.SparkMaxBrushedMotorSwerve;
+import swervelib.motors.SparkMaxSwerve;
+import swervelib.motors.SwerveMotor;
+
+import java.util.function.Supplier;
+
+/**
+ * SparkMax absolute encoder, attached through the data port.
+ */
+public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder
+{
+
+  /**
+   * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
+   */
+  public  SparkAbsoluteEncoder encoder;
+  /**
+   * An {@link Alert} for if there is a failure configuring the encoder.
+   */
+  private Alert                failureConfiguring;
+  /**
+   * An {@link Alert} for if there is a failure configuring the encoder offset.
+   */
+  private Alert                offsetFailure;
+  /**
+   * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance.
+   */
+  private SwerveMotor sparkFlex;
+
+  /**
+   * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex}
+   * motor.
+   *
+   * @param motor            Motor to create the encoder from.
+   * @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
+   */
+  public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor)
+  {
+    failureConfiguring = new Alert(
+        "Encoders",
+        "Failure configuring SparkMax Absolute Encoder",
+        AlertType.kWarning);
+    offsetFailure = new Alert(
+        "Encoders",
+        "Failure to set Absolute Encoder Offset",
+        AlertType.kWarning);
+    if (motor.getMotor() instanceof SparkFlex)
+    {
+      sparkFlex = motor;
+      encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder();
+      motor.setAbsoluteEncoder(this);
+      motor.configureIntegratedEncoder(conversionFactor);
+    } else
+    {
+      throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
+    }
+  }
+
+  /**
+   * Run the configuration until it succeeds or times out.
+   *
+   * @param config Lambda supplier returning the error state.
+   */
+  private void configureSparkMax(Supplier<REVLibError> config)
+  {
+    for (int i = 0; i < maximumRetries; i++)
+    {
+      if (config.get() == REVLibError.kOk)
+      {
+        return;
+      }
+    }
+    failureConfiguring.set(true);
+  }
+
+  /**
+   * Reset the encoder to factory defaults.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Clear sticky faults on the encoder.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Configure the absolute encoder to read from [0, 360) per second.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  @Override
+  public void configure(boolean inverted)
+  {
+    if (sparkFlex instanceof SparkMaxSwerve)
+    {
+      SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.inverted(inverted);
+      ((SparkMaxSwerve) sparkFlex).updateConfig(cfg);
+    }
+  }
+
+  /**
+   * Get the absolute position of the encoder.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+  @Override
+  public double getAbsolutePosition()
+  {
+    return encoder.getPosition();
+  }
+
+  /**
+   * Get the instantiated absolute encoder Object.
+   *
+   * @return Absolute encoder object.
+   */
+  @Override
+  public Object getAbsoluteEncoder()
+  {
+    return encoder;
+  }
+
+  /**
+   * Sets the Absolute Encoder Offset inside of the SparkMax's Memory.
+   *
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    if (sparkFlex instanceof SparkMaxSwerve)
+    {
+      SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.zeroOffset(offset);
+      ((SparkMaxSwerve) sparkFlex).updateConfig(cfg);
+      return true;
+    } 
+    return false;
+  }
+
+  /**
+   * Get the velocity in degrees/sec.
+   *
+   * @return velocity in degrees/sec.
+   */
+  @Override
+  public double getVelocity()
+  {
+    return encoder.getVelocity();
+  }
+}

src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java

@@ -0,0 +1,152 @@
+package swervelib.imu;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+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.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import java.util.Optional;
+
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+/**
+ * SwerveIMU interface for the {@link WPI_PigeonIMU}.
+ */
+public class PigeonViaTalonSRXSwerve extends SwerveIMU
+{
+
+
+  /**
+   * {@link TalonSRX} TalonSRX the IMU is attached to.
+   */
+  private final TalonSRX talon;
+
+  /**
+   * {@link WPI_PigeonIMU} IMU device.
+   */
+  private final WPI_PigeonIMU      imu;
+  /**
+   * Mutable {@link AngularVelocity} for readings.
+   */
+  private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
+  /**
+   * Offset for the {@link WPI_PigeonIMU}.
+   */
+  private       Rotation3d         offset      = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private       boolean            invertedIMU = false;
+
+  /**
+   * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}.
+   *
+   * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus.
+   */
+  public PigeonViaTalonSRXSwerve(int canid)
+  {
+    talon = new TalonSRX(canid);
+    imu = new WPI_PigeonIMU(talon);
+    offset = new Rotation3d();
+    SmartDashboard.putData(imu);
+  }
+
+  /**
+   * Reset IMU to factory default.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    imu.configFactoryDefault();
+  }
+
+  /**
+   * Clear sticky faults on IMU.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    imu.clearStickyFaults();
+  }
+
+  /**
+   * Set the gyro offset.
+   *
+   * @param offset gyro offset as a {@link Rotation3d}.
+   */
+  public void setOffset(Rotation3d offset)
+  {
+    this.offset = offset;
+  }
+
+  /**
+   * Set the gyro to invert its default direction
+   *
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
+  /**
+   * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  @Override
+  public Rotation3d getRawRotation3d()
+  {
+    double[] wxyz = new double[4];
+    imu.get6dQuaternion(wxyz);
+    Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
+    return invertedIMU ? reading.unaryMinus() : reading;
+  }
+
+  /**
+   * Fetch the {@link Rotation3d} from the IMU. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  @Override
+  public Rotation3d getRotation3d()
+  {
+    return getRawRotation3d().minus(offset);
+  }
+
+  /**
+   * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
+   * empty.
+   *
+   * @return {@link Translation3d} of the acceleration as an {@link Optional}.
+   */
+  @Override
+  public Optional<Translation3d> getAccel()
+  {
+    short[] initial = new short[3];
+    imu.getBiasedAccelerometer(initial);
+    return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
+  }
+
+  @Override
+  public MutAngularVelocity getYawAngularVelocity()
+  {
+    return yawVel.mut_setMagnitude(imu.getRate());
+  }
+
+  /**
+   * Get the instantiated {@link WPI_PigeonIMU} IMU object.
+   *
+   * @return IMU object.
+   */
+  @Override
+  public Object getIMU()
+  {
+    return imu;
+  }
+}