Update documentation

Author: viggy96

README.md

@@ -48,10 +48,12 @@ Custom library for 418 Purple Haze
   * Module must be calibrated using calibration tool provided by module manufacturer
   * Mixing motor controller vendors within a module is NOT supported (ex. TalonFX for drive, Spark Max for rotation)
   * More info [here](src/main/java/org/lasarobotics/drive/swerve/README.md)
-* Robot rotation PID
-* Automatic module "locking"
-* Traction control
-* Swerve second order kinematics correction
+* Swerve drive parent class
+  * Robot rotation PID
+  * Automatic module "locking"
+  * Traction control
+  * Swerve second order kinematics correction
+  * More info [here](src/main/java/org/lasarobotics/drive/swerve/README.md)
 * Health monitoring and automatic recovery
   * Available in the hardware wrappers and for subsystems
 * Configurable input maps

src/main/java/org/lasarobotics/drive/RotatePIDController.java

@@ -7,7 +7,6 @@
 import java.util.HashMap;
 
 import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
-import org.lasarobotics.utils.GlobalConstants;
 import org.lasarobotics.utils.PIDConstants;
 
 import edu.wpi.first.math.MathUtil;
@@ -16,17 +15,19 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Dimensionless;
+import edu.wpi.first.units.measure.Time;
 
 /** Rotate PID controller */
 public class RotatePIDController {
   private final double MIN_DEADBAND = 0.001;
   private final double MAX_DEADBAND = 0.2;
   private final double FILTER_FACTOR = 1.0 / 3.0;
+  private final Time MAX_LOOKAHEAD = Units.Seconds.of(2);
 
   private HashMap<Double, Double> m_rotateInputMap = new HashMap<Double, Double>();
   private PIDController m_pidController;
   private Angle m_rotateScalar;
-  private int m_lookAhead;
+  private Time m_lookAhead;
   private double m_deadband;
   private double m_rotateRequest;
   private boolean m_isRotating;
@@ -39,11 +40,15 @@ public class RotatePIDController {
    * @param deadband Controller deadband
    * @param lookAhead Number of loops to look ahead by
    */
-  public RotatePIDController(PolynomialSplineFunction rotateInputCurve, PIDConstants pidf, Angle rotateScalar, Dimensionless deadband, int lookAhead) {
+  public RotatePIDController(PolynomialSplineFunction rotateInputCurve,
+                             PIDConstants pidf,
+                             Angle rotateScalar,
+                             Dimensionless deadband,
+                             Time lookAhead) {
     this.m_pidController = new PIDController(pidf.kP, 0.0, pidf.kD, pidf.period.in(Units.Seconds));
     this.m_rotateScalar = rotateScalar;
     this.m_deadband = MathUtil.clamp(deadband.in(Units.Percent), MIN_DEADBAND, MAX_DEADBAND);
-    this.m_lookAhead = lookAhead;
+    this.m_lookAhead = Units.Seconds.of(MathUtil.clamp(lookAhead.in(Units.Seconds), 0.0, MAX_LOOKAHEAD.in(Units.Seconds)));
     this.m_isRotating = false;
 
     // Enable PID position wrapping
@@ -84,7 +89,7 @@ public AngularVelocity calculate(Angle currentAngle, AngularVelocity rotateRate,
     } else {
       // When rotation is complete, set setpoint to current angle
       if (m_isRotating) {
-        m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + (rotateRate.in(Units.DegreesPerSecond) * m_lookAhead * GlobalConstants.ROBOT_LOOP_HZ.asPeriod().in(Units.Seconds)));
+        m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + (rotateRate.in(Units.DegreesPerSecond) * m_lookAhead.in(Units.Seconds)));
         m_isRotating = false;
       }
     }

src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java

@@ -13,10 +13,8 @@
 import java.util.function.Supplier;
 
 import org.lasarobotics.drive.swerve.SwerveModule;
+import org.lasarobotics.hardware.IMU;
 import org.lasarobotics.hardware.PurpleManager;
-import org.lasarobotics.hardware.ctre.Pigeon2;
-import org.lasarobotics.hardware.kauailabs.NavX2;
-import org.lasarobotics.hardware.reduxrobotics.Canandgyro;
 import org.lasarobotics.utils.GlobalConstants;
 import org.lasarobotics.vision.AprilTagCamera;
 import org.littletonrobotics.junction.AutoLog;
@@ -55,9 +53,7 @@ public static class SwervePoseEstimatorServiceInputs {
   private static final String ESTIMATED_POSES_LOG_ENTRY = "/Vision/EstimatedPoses";
 
   private boolean m_running;
-  private Supplier<Rotation2d> m_rotation2dSupplier;
-  private Supplier<AngularVelocity> m_yawRateSupplier;
-  private Supplier<Boolean> m_imuConnectedSupplier;
+  private IMU m_imu;
   private Supplier<SwerveModulePosition[]> m_swerveModulePositionSupplier;
   private Consumer<Pose2d> m_poseResetMethod;
   private SwerveDriveKinematics m_kinematics;
@@ -81,68 +77,13 @@ public static class SwervePoseEstimatorServiceInputs {
    * <p>
    * This service runs in the background and keeps track of where the robot is on the field
    * @param odometryStdDev Standard deviation of wheel odometry measurements
-   * @param imu NavX2 MXP IMU
+   * @param imu IMU installed on robot
    * @param modules Swerve modules
    */
-  public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, NavX2 imu, SwerveModule... modules) {
-    this(
-      odometryStdDev,
-      () -> imu.getInputs().rotation2d,
-      () -> imu.getInputs().yawRate,
-      () -> imu.getInputs().isConnected,
-      modules
-    );
-  }
-
-  /**
-   * Create Swerve Pose Estimator Service
-   * <p>
-   * This service runs in the background and keeps track of where the robot is on the field
-   * @param odometryStdDev Standard deviation of wheel odometry measurements
-   * @param imu CTRE Pigeon 2.0 IMU
-   * @param modules Swerve modules
-   */
-  public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Pigeon2 imu, SwerveModule... modules) {
-    this(
-      odometryStdDev,
-      () -> imu.getInputs().rotation2d,
-      () -> imu.getInputs().yawRate,
-      () -> true,
-      modules
-    );
-  }
-
-  /**
-   * Create Swerve Pose Estimator Service
-   * <p>
-   * This service runs in the background and keeps track of where the robot is on the field
-   * @param odometryStdDev Standard deviation of wheel odometry measurements
-   * @param imu Redux Canandgyro IMU
-   * @param modules Swerve modules
-   */
-  public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Canandgyro imu, SwerveModule... modules) {
-    this(
-      odometryStdDev,
-      () -> imu.getInputs().rotation2d,
-      () -> imu.getInputs().yawRate,
-      () -> true,
-      modules
-    );
-  }
-
-  private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
-                                     Supplier<Rotation2d> rotation2dSupplier,
-                                     Supplier<AngularVelocity> yawRateSupplier,
-                                     Supplier<Boolean> imuConnectedSupplier,
-                                     SwerveModule... modules) {
+  public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, IMU imu, SwerveModule... modules) {
     if (modules.length != 4) throw new IllegalArgumentException("Four (4) modules must be used!");
+    this.m_imu = imu;
     this.m_running = false;
-    // Remember how to get rotation2d from IMU
-    this.m_rotation2dSupplier = rotation2dSupplier;
-    // Remember how to get yaw rate from IMU
-    this.m_yawRateSupplier = yawRateSupplier;
-    // Remember how to check if IMU is connected
-    this.m_imuConnectedSupplier = imuConnectedSupplier;
 
     // Get each individual module
     var moduleList = Arrays.asList(modules);
@@ -182,7 +123,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
     // Initialise pose estimator
     this.m_poseEstimator = new SwerveDrivePoseEstimator(
       m_kinematics,
-      m_rotation2dSupplier.get(),
+      m_imu.getRotation2d(),
       m_swerveModulePositionSupplier.get(),
       new Pose2d(),
       odometryStdDev,
@@ -207,7 +148,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
       m_currentTimestamp = Logger.getRealTimestamp();
 
       // Check if IMU is connected
-      boolean isIMUConnected = m_imuConnectedSupplier.get();
+      boolean isIMUConnected = m_imu.isConnected();
 
       // Get synchronized swerve module positions
       var currentModulePositions = m_swerveModulePositionSupplier.get();
@@ -222,9 +163,9 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
         m_previousModulePositions[i] = currentModulePositions[i];
       }
       var previousYawAngle = m_yawAngle;
-      m_yawAngle = (isIMUConnected) ? m_rotation2dSupplier.get() :
+      m_yawAngle = (isIMUConnected) ? m_imu.getRotation2d() :
         m_yawAngle.plus(new Rotation2d(m_kinematics.toTwist2d(moduleDeltas).dtheta));
-      var yawRate = (isIMUConnected) ? m_yawRateSupplier.get() :
+      var yawRate = (isIMUConnected) ? m_imu.getYawRate() :
         Units.Radians.of(m_yawAngle.minus(previousYawAngle).getRadians()).divide(Units.Microseconds.of(m_currentTimestamp - m_previousTimestamp));
 
       // If no cameras or yaw rate is too high, just update pose based on odometry and exit
@@ -272,7 +213,7 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
     m_thread.setName(NAME);
 
     // Remember how to reset pose
-    this.m_poseResetMethod = pose -> m_poseEstimator.resetPosition(m_rotation2dSupplier.get(), m_swerveModulePositionSupplier.get(), pose);
+    this.m_poseResetMethod = pose -> m_poseEstimator.resetPosition(m_imu.getRotation2d(), m_swerveModulePositionSupplier.get(), pose);
 
     // Register service as pose supplier with PurpleManager for simulation
     PurpleManager.setPoseSupplier(this::getPose);

src/main/java/org/lasarobotics/drive/TractionControlController.java

@@ -52,7 +52,7 @@ private State(int value) {
 
   private double m_optimalSlipRatio;
   private double m_mass;
-  private double m_maxLinearSpeed;
+  private double m_maxLinearVelocity;
   private double m_maxAcceleration;
   private double m_staticCoF;
   private double m_dynamicCoF;
@@ -94,7 +94,7 @@ public TractionControlController(Dimensionless staticCoF,
     this.m_dynamicCoF = dynamicCoF.in(Units.Value);
     this.m_optimalSlipRatio = MathUtil.clamp(optimalSlipRatio.in(Units.Value), MIN_SLIP_RATIO, MAX_SLIP_RATIO);
     this.m_mass = mass.divide(4).in(Units.Kilograms);
-    this.m_maxLinearSpeed = Math.floor(maxLinearSpeed.in(Units.MetersPerSecond) * 1000) / 1000;
+    this.m_maxLinearVelocity = Math.floor(maxLinearSpeed.in(Units.MetersPerSecond) * 1000) / 1000;
     this.m_maxAcceleration = m_staticCoF * Units.Gs.one().in(Units.MetersPerSecondPerSecond);
     this.m_maxPredictedSlipRatio = (m_maxAcceleration * GlobalConstants.ROBOT_LOOP_HZ.in(Units.Hertz))
       / (m_staticCoF * m_mass * Units.Gs.one().in(Units.MetersPerSecondPerSecond));
@@ -143,12 +143,12 @@ public LinearVelocity calculate(LinearVelocity velocityRequest,
     // Get current slip ratio, and check if slipping
     double currentSlipRatio = Math.abs(
       inertialVelocity.lte(INERTIAL_VELOCITY_THRESHOLD)
-        ? wheelSpeed.in(Units.MetersPerSecond) / m_maxLinearSpeed
+        ? wheelSpeed.in(Units.MetersPerSecond) / m_maxLinearVelocity
         : (Math.abs(wheelSpeed.in(Units.MetersPerSecond)) - inertialVelocity.in(Units.MetersPerSecond)) / inertialVelocity.in(Units.MetersPerSecond)
     );
     m_isSlipping = m_slippingDebouncer.calculate(
       currentSlipRatio > m_optimalSlipRatio
-      & Math.abs(wheelSpeed.in(Units.MetersPerSecond)) > m_maxLinearSpeed * m_optimalSlipRatio
+      & Math.abs(wheelSpeed.in(Units.MetersPerSecond)) > m_maxLinearVelocity * m_optimalSlipRatio
       & isEnabled()
     );
 
@@ -178,8 +178,8 @@ & isEnabled()
     // Update output, clamping to max linear speed
     velocityOutput = Units.MetersPerSecond.of(MathUtil.clamp(
       velocityOutput.plus(velocityCorrection).in(Units.MetersPerSecond),
-      -m_maxLinearSpeed,
-      +m_maxLinearSpeed
+      -m_maxLinearVelocity,
+      +m_maxLinearVelocity
     ));
 
     return velocityOutput;
@@ -193,6 +193,14 @@ public boolean isSlipping() {
     return m_isSlipping;
   }
 
+  /**
+   * Get max linear velocity
+   * @return Maximum linear velocity
+   */
+  public LinearVelocity getMaxLinearVelocity() {
+    return Units.MetersPerSecond.of(m_maxLinearVelocity);
+  }
+
   /**
    * Toggle traction control
    */

src/main/java/org/lasarobotics/drive/swerve/README.md

@@ -1,4 +1,10 @@
-# REV Swerve
+# Swerve Drive template
+
+Simply extend the [SwerveDrive](src/main/java/org/lasarobotics/drive/swerve/SwerveDrive.java) for your drive subsystem, and call the `super()` constructor with the appropriate arguments.
+
+Use the [PurpleSwerve](https://github.com/lasarobotics/PurpleSwerve) repo as an example
+
+## REV Swerve
 
 In order to use Spark motor controllers, you MUST have a PWM absolute encoder directly connected to the Spark that controls the rotation motor.
 
@@ -13,8 +19,8 @@ Options include:
  ```
 REVSwerveModule lFrontModule = MAXSwerveModule.create(
    REVSwerveModule.initializeHardware(
-     Constants.DriveHardware.LEFT_FRONT_DRIVE_MOTOR_ID,
-     Constants.DriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID,
+     new Spark.ID("DriveHardware/Swerve/LeftFront/Drive", 2);
+     new Spark.ID("DriveHardware/Swerve/LeftFront/Rotate", 3);
      MotorKind.NEO_VORTEX,
      MotorKind.NEO_550
    ),
@@ -34,16 +40,50 @@ REVSwerveModule lFrontModule = MAXSwerveModule.create(
 );
 ```
 
- # CTRE Swerve
+## CTRE Swerve
 
  At the current time, the CTRE CANCoder is the ONLY absolute encoder supported.
 
-## MAXSwerve
+ Use of CANivore is HIGHLY recommended, as adjustment of status frame rates isn't easily possible while protecting the underlying sensor getter methods.
+
+ CANivore must be named "canivore" using Phoenix Tuner X.
+
+ To create an instance of a module, use the helper class for your module, and call the `create()` method for CTRE.
+
+For example, here's how to create a Swerve X2i module:
+ ```
+CTRESwerveModule lFrontModule = SwerveX2Module.create(
+   CTRESwerveModule.initializeHardware(
+     new TalonFX.ID("DriveHardware/Swerve/LeftFront/Drive", PhoenixCANBus.CANIVORE, 2);
+     new TalonFX.ID("DriveHardware/Swerve/LeftFront/Rotate", PhoenixCANBus.CANIVORE, 3);
+     new CANcoder.ID("DriveHardware/Swerve/LeftFront/Encoder", PhoenixCANBus.CANIVORE, 10);
+   ),
+   SwerveModule.Location.LeftFront,
+   SwerveModule.MountOrientation.INVERTED
+   SwerveX2Module.GearRatio.X4_3,
+   DriveWheel.create(Units.Inches.of(3.0), Units.Value.of(0.9), Units.Value.of(0.8)),
+   PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0),
+   FFConstants.of(0.2, 0.0, 0.0, 0.5),
+   PIDConstants.of(2.0, 0.0, 0.1, 0.0, 0.0),
+   FFConstants.of(0.2, 0.0, 0.0, 0.01),
+   Units.Percent.of(8.0),
+   Units.Pounds.of(135.0),
+   Units.Meters.of(0.5588),
+   Units.Meters.of(0.5588),
+   Units.Seconds.of(3.0),
+   Units.Amps.of(60.0)
+);
+```
+
+### MAXSwerve
+
+Only REV Spark motor controllers are currently supported with MAXSwerve.
 
-### Calibration
-Modules must be calibrated as per manufacturer instructions: https://docs.revrobotics.com/brushless/spark-max/encoders/maxswerve
+#### Calibration
+Modules must be calibrated as per manufacturer instructions:
+https://docs.revrobotics.com/brushless/spark-max/encoders/maxswerve
 
-### Sample PID values
+#### Sample PID values
 These values can be a good starting point for MAXSwerve.
 Note that rotate feed forward values are only used for simulation.
 
@@ -61,14 +101,14 @@ Rotate kS: 0.2
 
 Rotate kA: 0.01
 
-## Swerve X
+### Swerve X
 
-### Calibration
+#### Calibration
 Module must be calibrated as per the manufacturer instructions for each respective module:
  * [Swerve X/Xi](https://docs.wcproducts.com/wcp-swervex/overview-and-features/zeroing-module)
  * [Swerve X2](https://docs.wcproducts.com/wcp-swerve-x2/overview-and-features/zeroing-module)
 
-## SDS
+### SDS
 
-### Calibration
+#### Calibration
 SDS does not use a "calibration tool" or jig like MAXSwerve or WCP. As such, I have assumed that users will zero the modules to be straight ahead, aligned with the chassis. Therefore the "zero offset" for these modules is zero.