Merge pull request #292 from konnorreynolds/dev

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

@@ -0,0 +1,169 @@
+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.SparkFlexConfig;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import swervelib.motors.SparkFlexSwerve;
+import swervelib.motors.SwerveMotor;
+
+import java.util.function.Supplier;
+
+/**
+ * SparkFlex absolute encoder, attached through the data port.
+ */
+public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder
+{
+
+  /**
+   * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex.
+   */
+  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 SparkFlexSwerve} 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 SparkFlex 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 SparkFlexEncoder is not a CANSparkFlex");
+    }
+  }
+
+  /**
+   * Run the configuration until it succeeds or times out.
+   *
+   * @param config Lambda supplier returning the error state.
+   */
+  private void configureSparkFlex(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 SparkFlexSwerve)
+    {
+      SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.inverted(inverted);
+      ((SparkFlexSwerve) 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 SparkFlex'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 SparkFlexSwerve)
+    {
+      SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.zeroOffset(offset);
+      ((SparkFlexSwerve) 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;
+  }
+}

src/main/java/swervelib/parser/json/DeviceJson.java

@@ -9,24 +9,8 @@
 
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
-import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
-import swervelib.encoders.CANCoderSwerve;
-import swervelib.encoders.CanAndMagSwerve;
-import swervelib.encoders.PWMDutyCycleEncoderSwerve;
-import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
-import swervelib.encoders.SparkMaxEncoderSwerve;
-import swervelib.encoders.SwerveAbsoluteEncoder;
-import swervelib.encoders.TalonSRXEncoderSwerve;
-import swervelib.encoders.ThriftyNovaEncoderSwerve;
-import swervelib.imu.ADIS16448Swerve;
-import swervelib.imu.ADIS16470Swerve;
-import swervelib.imu.ADXRS450Swerve;
-import swervelib.imu.AnalogGyroSwerve;
-import swervelib.imu.CanandgyroSwerve;
-import swervelib.imu.NavXSwerve;
-import swervelib.imu.Pigeon2Swerve;
-import swervelib.imu.PigeonSwerve;
-import swervelib.imu.SwerveIMU;
+import swervelib.encoders.*;
+import swervelib.imu.*;
 import swervelib.motors.SparkFlexSwerve;
 import swervelib.motors.SparkMaxBrushedMotorSwerve;
 import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
@@ -81,6 +65,11 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
         return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
       case "sparkmax_analog5v":
         return new SparkMaxAnalogEncoderSwerve(motor, 5);
+      case "sparkflex_integrated":
+      case "sparkflex_attached":
+      case "sparkflex_canandmag":
+      case "sparkflex_canandcoder":
+        return new SparkFlexEncoderSwerve(motor, 360);
       case "canandcoder_can":
       case "canandmag_can":
         return new CanAndMagSwerve(id);
@@ -154,6 +143,8 @@ public SwerveIMU createIMU()
         return new NavXSwerve(NavXComType.kMXP_UART);
       case "pigeon":
         return new PigeonSwerve(id);
+      case "pigeon_via_talonsrx" :
+        return new PigeonViaTalonSRXSwerve(id);
       case "pigeon2":
         return new Pigeon2Swerve(id, canbus != null ? canbus : "");
       default: