Update YAGSL to handle controls better

swervelib/math/SwerveMath.java

@@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.wpilibj.smartdashboard.SmartDashboard;
 import java.util.List;
@@ -20,22 +21,6 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 public class SwerveMath
 {
 
-  /**
-   * Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
-   * 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)
-  {
-    double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s
-    return optimalVoltage / maxAngularVelocity;
-  }
-
   /**
    * Calculate the meters per rotation for the integrated encoder. Calculation: 4in diameter wheels * pi [circumfrence]
    * / gear ratio.
@@ -214,6 +199,32 @@ public class SwerveMath
     return maxAccel;
   }
 
+  /**
+   * Logical inverse of the Pose exponential from 254. Taken from team 3181.
+   *
+   * @param transform Pose to perform the log on.
+   */
+  public static Twist2d PoseLog(final Pose2d transform)
+  {
+
+    final double kEps          = 1E-9;
+    final double dtheta        = transform.getRotation().getRadians();
+    final double half_dtheta   = 0.5 * dtheta;
+    final double cos_minus_one = transform.getRotation().getCos() - 1.0;
+    double       halftheta_by_tan_of_halfdtheta;
+    if (Math.abs(cos_minus_one) < kEps)
+    {
+      halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else
+    {
+      halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one;
+    }
+    final Translation2d translation_part = transform.getTranslation()
+                                                    .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta,
+                                                                             -half_dtheta));
+    return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta);
+  }
+
   /**
    * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link SwerveMath#calcMaxAccel}.
    * Note that this takes and returns field-relative velocities.
@@ -310,43 +321,6 @@ public class SwerveMath
     return configuration;
   }
 
-  /**
-   * Optimize the angle of the {@link SwerveModuleState2} to be the closest angle to the current angle. Taken from Team
-   * 3181 at
-   * https://github.com/pittsfordrobotics/REVSwerve2023/blob/master/src/main/java/com/team3181/lib/swerve/SwerveOptimizer.java
-   *
-   * @param desiredState             Desired {@link SwerveModuleState2} to achieve.
-   * @param currentAngle             Current angle as a {@link Rotation2d}.
-   * @param secondOrderOffsetDegrees Offset calculated using 2nd order kinematics.
-   * @return Optimized {@link SwerveModuleState2}
-   */
-  public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
-                                            double secondOrderOffsetDegrees)
-  {
-    double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(),
-                                                       desiredState.angle.getDegrees() + secondOrderOffsetDegrees);
-    double targetSpeed = desiredState.speedMetersPerSecond;
-    double delta       = targetAngle - currentAngle.getDegrees();
-    if (Math.abs(delta) > 90)
-    {
-      targetSpeed = -targetSpeed;
-      if (delta > 90)
-      {
-        targetAngle -= 180;
-      } else
-      {
-        targetAngle += 180;
-      }
-    }
-    // Ensure outputted angle is positive.
-    while (targetAngle < 0)
-    {
-      targetAngle += 360;
-    }
-    return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle),
-                                  desiredState.omegaRadPerSecond);
-  }
-
   /**
    * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes
    * the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg.
@@ -399,8 +373,7 @@ public class SwerveMath
     if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01))
     {
       moduleState.angle = lastModuleState.angle;
-//      moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
-      moduleState.omegaRadPerSecond = 0;
+      moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
     }
   }
 }