Addressing issue #7 by reading CANCoder values until successful with 10ms delay between readings. Fall back to reading relative encoder.

2026-06-19 06:21:40 +00:00 · 2023-02-20 20:59:31 -06:00
parent 8f9ffdf031
commit dd28a657b2
43 changed files with 570 additions and 363 deletions
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -19,14 +19,15 @@ public class SwerveMath

  /**
   * Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
-   * degree <=> (maxVolts) / (maxSpeed)
+   * degree == (maxVolts) / (maxSpeed)
   *
   * @param optimalVoltage    Optimal voltage to use when calculating the angle kV.
   * @param motorFreeSpeedRPM Motor free speed in Rotations per Minute.
   * @param angleGearRatio    Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.
   * @return angle kV for feedforward.
   */
-  public static double calculateAngleKV(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
+  public static double calculateAngleKV(
+      double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
  {
    double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s
    return optimalVoltage / maxAngularVelocity;
@@ -41,7 +42,8 @@ public class SwerveMath
   * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder.
   * @return Meters per rotation for the drive motor.
   */
-  public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)
+  public static double calculateMetersPerRotation(
+      double wheelDiameter, double driveGearRatio, double pulsePerRotation)
  {
    return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation);
  }
@@ -69,18 +71,21 @@ public class SwerveMath
  public static double applyDeadband(double value, boolean scaled, double deadband)
  {
    value = Math.abs(value) > deadband ? value : 0;
-    return scaled ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) : value;
+    return scaled
+           ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value)
+           : value;
  }

  /**
-   * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values.  Drive converts
+   * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts
   * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth.
   *
   * @param angleGearRatio   The gear ratio of the steering motor.
   * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated.
   * @return Degrees per steering rotation for the angle motor.
   */
-  public static double calculateDegreesPerSteeringRotation(double angleGearRatio, double pulsePerRotation)
+  public static double calculateDegreesPerSteeringRotation(
+      double angleGearRatio, double pulsePerRotation)
  {
    return 360 / (angleGearRatio * pulsePerRotation);
  }
@@ -93,7 +98,8 @@ public class SwerveMath
   * @param furthestModuleY Y of the furthest module in meters.
   * @return Maximum angular velocity in rad/s.
   */
-  public static double calculateMaxAngularVelocity(double maxSpeed, double furthestModuleX, double furthestModuleY)
+  public static double calculateMaxAngularVelocity(
+      double maxSpeed, double furthestModuleX, double furthestModuleY)
  {
    return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
  }
@@ -119,8 +125,12 @@ public class SwerveMath
   * @param robotMass     Mass of the robot in kg.
   * @return Theoretical maximum acceleration in m/s/s.
   */
-  public static double calculateMaxAcceleration(double stallTorqueNm, double gearRatio, double moduleCount,
-                                                double wheelDiameter, double robotMass)
+  public static double calculateMaxAcceleration(
+      double stallTorqueNm,
+      double gearRatio,
+      double moduleCount,
+      double wheelDiameter,
+      double robotMass)
  {
    return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass);
  }
@@ -137,50 +147,60 @@ public class SwerveMath
   * @param config                 The swerve drive configuration.
   * @return Maximum acceleration allowed in the robot direction.
   */
-  private static double calcMaxAccel(Rotation2d angle, double chassisMass, double robotMass,
-                                     Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+  private static double calcMaxAccel(
+      Rotation2d angle,
+      double chassisMass,
+      double robotMass,
+      Translation3d chassisCenterOfGravity,
+      SwerveDriveConfiguration config)
  {
    double xMoment = (chassisCenterOfGravity.getX() * chassisMass);
    double yMoment = (chassisCenterOfGravity.getY() * chassisMass);
-    // Calculate the vertical mass moment using the floor as the datum.  This will be used later to calculate max
+    // Calculate the vertical mass moment using the floor as the datum.  This will be used later to
+    // calculate max
    // acceleration
    double        zMoment      = (chassisCenterOfGravity.getZ() * (chassisMass));
    Translation3d robotCG      = new Translation3d(xMoment, yMoment, zMoment).div(robotMass);
    Translation2d horizontalCG = robotCG.toTranslation2d();

-    Translation2d projectedHorizontalCg = new Translation2d(
-        (angle.getSin() * angle.getCos() * horizontalCG.getY()) + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
-        (angle.getSin() * angle.getCos() * horizontalCG.getX()) + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())
-    );
+    Translation2d projectedHorizontalCg =
+        new Translation2d(
+            (angle.getSin() * angle.getCos() * horizontalCG.getY())
+            + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
+            (angle.getSin() * angle.getCos() * horizontalCG.getX())
+            + (Math.pow(angle.getSin(), 2) * horizontalCG.getY()));

-    // Projects the edge of the wheelbase onto the direction line.  Assumes the wheelbase is rectangular.
-    // Because a line is being projected, rather than a point, one of the coordinates of the projected point is
+    // Projects the edge of the wheelbase onto the direction line.  Assumes the wheelbase is
+    // rectangular.
+    // Because a line is being projected, rather than a point, one of the coordinates of the
+    // projected point is
    // already known.
    Translation2d projectedWheelbaseEdge;
    double        angDeg = angle.getDegrees();
    if (angDeg <= 45 && angDeg >= -45)
    {
      SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
-      projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(),
-                                                 conf.moduleLocation.getX() * angle.getTan());
+      projectedWheelbaseEdge =
+          new Translation2d(
+              conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
    } else if (135 >= angDeg && angDeg > 45)
    {
      SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
-      projectedWheelbaseEdge = new Translation2d(
-          conf.moduleLocation.getY() / angle.getTan(),
-          conf.moduleLocation.getY());
+      projectedWheelbaseEdge =
+          new Translation2d(
+              conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
    } else if (-135 <= angDeg && angDeg < -45)
    {
      SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false);
-      projectedWheelbaseEdge = new Translation2d(
-          conf.moduleLocation.getY() / angle.getTan(),
-          conf.moduleLocation.getY());
+      projectedWheelbaseEdge =
+          new Translation2d(
+              conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
    } else
    {
      SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true);
-      projectedWheelbaseEdge = new Translation2d(
-          conf.moduleLocation.getX(),
-          conf.moduleLocation.getX() * angle.getTan());
+      projectedWheelbaseEdge =
+          new Translation2d(
+              conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
    }

    double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
@@ -192,7 +212,7 @@ public class SwerveMath

  /**
   * Limits a commanded velocity to prevent exceeding the maximum acceleration given by
-   * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}.  Note that
+   * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that
   * this takes and returns field-relative velocities.
   *
   * @param commandedVelocity      The desired velocity
@@ -204,12 +224,18 @@ public class SwerveMath
   * @param robotMass              The weight of the robot in kg. (Including manipulators, etc).
   * @param chassisCenterOfGravity Chassis center of gravity.
   * @param config                 The swerve drive configuration.
-   * @return The limited velocity.  This is either the commanded velocity, if attainable, or the closest attainable
+   * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
   * velocity.
   */
-  public static Translation2d limitVelocity(Translation2d commandedVelocity, ChassisSpeeds fieldVelocity,
-                                            Pose2d robotPose, double loopTime, double chassisMass, double robotMass,
-                                            Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+  public static Translation2d limitVelocity(
+      Translation2d commandedVelocity,
+      ChassisSpeeds fieldVelocity,
+      Pose2d robotPose,
+      double loopTime,
+      double chassisMass,
+      double robotMass,
+      Translation3d chassisCenterOfGravity,
+      SwerveDriveConfiguration config)
  {
    // Get the robot's current field-relative velocity
    Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
@@ -222,12 +248,18 @@ public class SwerveMath

    // Creates an acceleration vector with the direction of delta V and a magnitude
    // of the maximum allowed acceleration in that direction
-    Translation2d maxAccel = new Translation2d(
-        calcMaxAccel(deltaV
-                         // Rotates the velocity vector to convert from field-relative to robot-relative
-                         .rotateBy(robotPose.getRotation().unaryMinus())
-                         .getAngle(), chassisMass, robotMass, chassisCenterOfGravity, config),
-        deltaV.getAngle());
+    Translation2d maxAccel =
+        new Translation2d(
+            calcMaxAccel(
+                deltaV
+                    // Rotates the velocity vector to convert from field-relative to robot-relative
+                    .rotateBy(robotPose.getRotation().unaryMinus())
+                    .getAngle(),
+                chassisMass,
+                robotMass,
+                chassisCenterOfGravity,
+                config),
+            deltaV.getAngle());

    // Calculate the maximum achievable velocity by the next loop cycle.
    // delta V = Vf - Vi = at
@@ -251,17 +283,22 @@ public class SwerveMath
   * @param left    True = furthest left, False = furthest right.
   * @return Module location which is the furthest from center and abides by parameters.
   */
-  public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left)
+  public static SwerveModuleConfiguration getSwerveModule(
+      SwerveModule[] modules, boolean front, boolean left)
  {
    Translation2d             target        = modules[0].configuration.moduleLocation, current, temp;
    SwerveModuleConfiguration configuration = modules[0].configuration;
    for (SwerveModule module : modules)
    {
      current = module.configuration.moduleLocation;
-      temp = front ? (target.getY() >= current.getY() ? current : target)
-                   : (target.getY() <= current.getY() ? current : target);
-      target = left ? (target.getX() >= temp.getX() ? temp : target)
-                    : (target.getX() <= temp.getX() ? temp : target);
+      temp =
+          front
+          ? (target.getY() >= current.getY() ? current : target)
+          : (target.getY() <= current.getY() ? current : target);
+      target =
+          left
+          ? (target.getX() >= temp.getX() ? temp : target)
+          : (target.getX() <= temp.getX() ? temp : target);
      configuration = current.equals(target) ? module.configuration : configuration;
    }
    return configuration;