Angle refactor + tests!

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
           distribution: 'zulu'
           java-version: 17
       - name: Run spotless linter
-        run: ./gradlew spotlessCheck
+        run: ./gradlew spotlessCheck
+
+  tests:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - uses: actions/setup-java@v4
+        with:
+          distribution: 'zulu'
+          java-version: 17
+      - name: Run tests
+        run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
    * @return the angle to set the steering motor to
    */
   public double calculateNewAngleSetpoint(double angle) {
-    double shortestAngle = Util.deltaAngle(currentAngle, angle);
+    double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
     return currentAngle = currentAngle + shortestAngle;
   }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
    * opposite direction and rotate less than 90 degrees
    */
   public double calculateOptimizedAngle(double angle) {
-    double deltaAngle = Util.deltaAngle(currentAngle, angle);
+    double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
     if (Math.abs(deltaAngle) > Math.PI / 2) {
       motorFlipper = -1;
       return angle + Math.PI;

quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+  private Vec2d poseEstimate = new Vec2d(0, 0);
+  private double looptime = 0;
+  private ArrayList<KalmanPose2d> velocities =
+      new ArrayList<KalmanPose2d>(); // / list of velocities, most recent last
+  public double heading = 0;
+
+  public KalmanFilterLocalizer(Vec2d initialPose, double looptime) {
+    poseEstimate = initialPose;
+    this.looptime = looptime;
+  }
+
+  /**
+   * Updates the pose estimate based on the current velocity and the time since the last vision
+   * update
+   *
+   * @param observedPose the current pose estimate (get this from vision usually)
+   * @param velocity the current velocity
+   * @param poseEstimateLatency the time since the last vision update
+   */
+  public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+    KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+    velocities.add(currentVelocity); // add the current velocity to the list of velocities
+    // trim the list of velocities to only include the relevant ones.
+    while (velocities.size() > 0
+        && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+      velocities.remove(0);
+    }
+    // distance traveled since the vision update
+    Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+    for (KalmanPose2d vel : velocities) {
+      deltaPosSinceVision =
+          deltaPosSinceVision.add(
+              vel); // don't scale here, we'll do it later (distributive property)
+    }
+    deltaPosSinceVision =
+        deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+    // update the vision pose estimate with the delta from velocity
+    Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+    // update the last pose estimate with the velocity
+    Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(this.looptime));
+    // update the pose estimate with a weighted average of the vision and kinematics pose estimates
+    w = MathUtil.clamp(w, 0, 1); // make sure w is between 0 and 1 (inclusive)
+    this.poseEstimate = ((visionPoseEstimate.scale(w)).add(kinematicsPoseEstimate.scale(1 - w)));
+    return this.poseEstimate;
+  }
+
+  public Pose2d getPose() {
+    return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+  }
+
+  public void setPose(Pose2d pose) {
+    this.poseEstimate = new Vec2d(pose.x, pose.y);
+  }
+
+  public void setHeading(double heading) {
+    this.heading = heading;
+  }
+}
+
+class KalmanPose2d extends Vec2d {
+  public double timestamp = 0;
+
+  public KalmanPose2d(double x, double y, double timestamp) {
+    super(x, y);
+    this.timestamp = timestamp;
+  }
+
+  public KalmanPose2d(Vec2d vec, double timestamp) {
+    super(vec.x, vec.y);
+    this.timestamp = timestamp;
+  }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
-  private double velocity;
-  private double acceleration;
+  private final double velocity;
+  private final double acceleration;
 
   /**
-   * Represents a pair of constraints (max velocity and max acceleration) for use in the path
-   * follower Can be either translational or angular velocity. If angular velocity, units are in
-   * radians.
+   * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
    *
-   * @param maxVelocity your units/s
-   * @param maxAcceleration your units/s^2
+   * @param maxVelocity the maximum velocity in units/s
+   * @param maxAcceleration the maximum acceleration in units/s^2
    */
   public ConstraintsPair(double maxVelocity, double maxAcceleration) {
     this.velocity = maxVelocity;
     this.acceleration = maxAcceleration;
   }
 
   /**
-   * Returns the constraints' max velocity
+   * Returns the maximum velocity of the constraints.
    *
-   * @return
+   * @return the maximum velocity in units/s
    */
   public double getMaxVelocity() {
     return velocity;
   }
 
   /**
-   * Returns the constraints' max acceleration
+   * Returns the maximum acceleration of the constraints.
    *
-   * @return
+   * @return the maximum acceleration in units/s^2
    */
   public double getMaxAcceleration() {
     return acceleration;