Swerve

src/main/java/frc/excalib/control/math/Circle.java

@@ -1,17 +1,13 @@
 package frc.excalib.control.math;
 
+import edu.wpi.first.math.geometry.Ellipse2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 
 /**
  * Represents a circle in a 2D plane, defined by its center and radius.
  */
-public class Circle {
-    // The radius of the circle
-    public final double r;
-    // The center of the circle as a Translation2d object
-    public final Translation2d center;
-
+public class Circle extends Ellipse2d {
     /**
      * Constructs a Circle object with the specified center coordinates and radius.
      *
@@ -20,8 +16,7 @@ public class Circle {
      * @param r The radius of the circle.
      */
     public Circle(double a, double b, double r) {
-        this.center = new Translation2d(a, b);
-        this.r = r;
+        super(new Translation2d(a, b), r);
     }
 
     /**
@@ -32,11 +27,11 @@ public Circle(double a, double b, double r) {
      */
     public Line getTangent(Translation2d pointOnCircle) {
         return new Line(
-                pointOnCircle.getX() - center.getX(),
-                pointOnCircle.getY() - center.getY(),
-                -center.getX() * (pointOnCircle.getX() - center.getX())
-                        - center.getY() * (pointOnCircle.getY() - center.getY()) -
-                        this.r * this.r
+                pointOnCircle.getX() - super.getCenter().getX(),
+                pointOnCircle.getY() - super.getCenter().getY(),
+                -super.getCenter().getX() * (pointOnCircle.getX() - super.getCenter().getX())
+                        - super.getCenter().getY() * (pointOnCircle.getY() - super.getCenter().getY()) -
+                        super.getXSemiAxis() * super.getXSemiAxis()
         );
     }
 
@@ -45,18 +40,18 @@ public Line getTangent(Translation2d pointOnCircle) {
      *
      * @param point A point outside or on the circle.
      * @return An array of Line objects representing the tangent lines.
-     *         Returns an empty array if the point is inside the circle.
+     * Returns an empty array if the point is inside the circle.
      */
     public Line[] getTangents(Translation2d point) {
-        if (point.getDistance(this.center) < this.r) {
+        if (point.getDistance(super.getCenter().getTranslation()) < super.getXSemiAxis()) {
             return new Line[0];
-        } else if (point.getDistance(this.center) == this.r) {
+        } else if (point.getDistance(super.getCenter().getTranslation()) == super.getXSemiAxis()) {
             return new Line[]{
                     getTangent(point)
             };
         }
-        double centersDistance = this.center.getDistance(point);
-        double newRad = Math.sqrt(Math.pow(centersDistance, 2) - Math.pow(this.r, 2));
+        double centersDistance = super.getCenter().getTranslation().getDistance(point);
+        double newRad = Math.sqrt(Math.pow(centersDistance, 2) - Math.pow(super.getXSemiAxis(), 2));
         Circle newCircle = new Circle(point.getX(), point.getY(), newRad);
         Translation2d[] intersections = getInterSections(newCircle);
         Translation2d firstTanPoint = intersections[0];
@@ -68,35 +63,121 @@ public Line[] getTangents(Translation2d point) {
         };
     }
 
+    /**
+     * Constructs a Line that passes through the given point and the center of the circle.
+     *
+     * @param point The point from which the line to the circle's center is drawn.
+     * @return A Line object representing the line passing through the given point and the circle's center.
+     */
+    public Line getLineToCenter(Translation2d point) {
+        Translation2d center = super.getCenter().getTranslation();
+        return new Line(
+                point.getY() - center.getY(),
+                center.getX() - point.getX(),
+                point.getX() * center.getY() - center.getX() * point.getY()
+        );
+    }
+
+    /**
+     * Calculates the intersection points between this circle and a line.
+     *
+     * @param line The line to intersect with.
+     * @return An array of Translation2d objects representing the intersection points.
+     *         Returns an empty array if there are no intersections.
+     */
+    public Translation2d[] getIntersections(Line line) {
+        double h = super.getCenter().getX();
+        double k = super.getCenter().getY();
+        double r = super.getXSemiAxis();
+        double a = line.a;
+        double b = line.b;
+        double c = line.c;
+
+        if (b == 0) {
+            // Line: x = -c / a
+            double x = -c / a;
+            double dx = x - h;
+            double underSqrt = r * r - dx * dx;
+            if (underSqrt < 0) return new Translation2d[0];
+            else if (underSqrt == 0) {
+                return new Translation2d[]{ new Translation2d(x, k) };
+            } else {
+                double sqrtVal = Math.sqrt(underSqrt);
+                double y1 = k + sqrtVal;
+                double y2 = k - sqrtVal;
+                return new Translation2d[]{
+                        new Translation2d(x, y1),
+                        new Translation2d(x, y2)
+                };
+            }
+        }
+
+        // General case: express y in terms of x from line equation
+        // y = (-a*x - c) / b
+        // Plug into circle: (x - h)^2 + (y - k)^2 = r^2
+        double m = -a / b;
+        double yInt = -c / b;
+
+        // (x - h)^2 + (m*x + yInt - k)^2 = r^2
+        double A = 1 + m * m;
+        double B = 2 * (m * (yInt - k) - h);
+        double C = h * h + (yInt - k) * (yInt - k) - r * r;
+
+        double discriminant = B * B - 4 * A * C;
+
+        if (discriminant < 0) {
+            return new Translation2d[0];
+        } else if (discriminant == 0) {
+            double x = -B / (2 * A);
+            double y = m * x + yInt;
+            return new Translation2d[]{ new Translation2d(x, y) };
+        } else {
+            double sqrtDisc = Math.sqrt(discriminant);
+            double x1 = (-B + sqrtDisc) / (2 * A);
+            double y1 = m * x1 + yInt;
+            double x2 = (-B - sqrtDisc) / (2 * A);
+            double y2 = m * x2 + yInt;
+            return new Translation2d[]{
+                    new Translation2d(x1, y1),
+                    new Translation2d(x2, y2)
+            };
+        }
+    }
+
+
     /**
      * Calculates the intersection points between this circle and another circle.
      *
      * @param other The other circle to find intersections with.
      * @return An array of Translation2d objects representing the intersection points.
-     *         Returns an empty array if there are no intersections.
+     * Returns an empty array if there are no intersections.
      */
     public Translation2d[] getInterSections(Circle other) {
+        Translation2d thisCenter = super.getCenter().getTranslation();
+        Translation2d otherCenter = other.getCenter().getTranslation();
 
-        if (other.center.getDistance(this.center) > other.r + r) return new Translation2d[0];
-        if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) return new Translation2d[0];
+        if (otherCenter.getDistance(thisCenter) > other.getXSemiAxis() + super.getXSemiAxis())
+            return new Translation2d[0];
+        if (otherCenter.getDistance(thisCenter) < Math.abs(other.getXSemiAxis() - super.getXSemiAxis()))
+            return new Translation2d[0];
 
-        if (other.center.getDistance(this.center) == other.r + r) {
+        if (otherCenter.getDistance(thisCenter) == other.getXSemiAxis() + super.getXSemiAxis()) {
             return new Translation2d[]{
-                    this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle()))
+                    thisCenter.plus(new Translation2d(super.getXSemiAxis(), otherCenter.minus(thisCenter).getAngle()))
             };
         }
-        if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) {
+        if (otherCenter.getDistance(thisCenter) < Math.abs(other.getXSemiAxis() - super.getXSemiAxis())) {
             return new Translation2d[]{ // Check for edge case
-                    this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle()))
+                    thisCenter.plus(new Translation2d(super.getXSemiAxis(), otherCenter.minus(thisCenter).getAngle()))
             };
         }
         Rotation2d alpha = new Rotation2d(Math.acos(
-                (Math.pow(other.r, 2) - Math.pow(this.r, 2) - Math.pow(this.center.getDistance(other.center), 2))
-                        / (-2 * this.center.getDistance(other.center) * this.r)
+                (Math.pow(other.getXSemiAxis(), 2) - Math.pow(super.getXSemiAxis(), 2) - Math.pow(thisCenter.getDistance(otherCenter), 2))
+                        / (-2 * thisCenter.getDistance(otherCenter) * super.getXSemiAxis())
         ));
         return new Translation2d[]{
-                this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().plus(alpha))),
-                this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().minus(alpha)))
+                thisCenter.plus(new Translation2d(super.getXSemiAxis(), otherCenter.minus(thisCenter).getAngle().plus(alpha))),
+                thisCenter.plus(new Translation2d(super.getXSemiAxis(), otherCenter.minus(thisCenter).getAngle().minus(alpha)))
         };
     }
 }

src/main/java/frc/excalib/control/math/Line.java

@@ -26,12 +26,24 @@ public Line(double a, double b, double c) {
         this.c = c;
     }
 
+    /**
+     * Constructs a Line given a point and a slope.
+     *
+     * @param point A point the line passes through.
+     * @param slope The slope of the line.
+     */
+    public Line(Translation2d point, double slope) {
+        this.a = slope;
+        this.b = -1;
+        this.c = point.getY() - slope * point.getX();
+    }
+
     /**
      * Finds the intersection point of this line with another line.
      *
      * @param other The other line to find the intersection with.
      * @return A Translation2d object representing the intersection point.
-     *         If the lines are parallel, the result may be undefined.
+     * If the lines are parallel, the result may be undefined.
      */
     public Translation2d findIntersection(Line other) {
         return new Translation2d(
@@ -40,6 +52,18 @@ public Translation2d findIntersection(Line other) {
         );
     }
 
+    /**
+     * Calculates and returns the slope of the line.
+     *
+     * @return The slope of the line. If the line is vertical (i.e., b == 0), returns {@code Double.POSITIVE_INFINITY}.
+     */
+    public double getSlope() {
+        if (b == 0) {
+            return Double.POSITIVE_INFINITY;
+        }
+        return -a / b;
+    }
+
     /**
      * Returns a string representation of the line in the format "a: [a], b: [b], c: [c]".
      *

src/main/java/frc/excalib/control/math/MathUtils.java

@@ -34,7 +34,56 @@ public static double limitTo(double limit, double value) {
     }
 
     /**
-     * Calculates the optimal target position for a robot to reach a target while avoiding a reef.
+     * Converts a slope of a line (m) to a direction in radians.
+     *
+     * @param m The slope of the line.
+     * @return The direction in radians (0 to <i>pi</i>). Returns <i>pi</i>/2 if the slope is infinite (i.e., vertical line).
+     */
+    public static double getDirectionBySlope(double m) {
+        if (Double.isInfinite(m)) {
+            return 90.0;
+        }
+        double angle = Math.atan(m);
+        return (angle + Math.PI) % Math.PI;
+    }
+
+    /**
+     * Finds the nearest point to a given reference point from a list of points.
+     *
+     * @param mainPoint The reference point to compare distances from.
+     * @param points The list of points to search through.
+     * @return The point closest to the reference point.
+     */
+    public static Translation2d getNearestPoint(Translation2d mainPoint, Translation2d... points) {
+        Translation2d nearestPoint = new Translation2d();
+        for (Translation2d point : points) {
+            if (mainPoint.getDistance(nearestPoint) > mainPoint.getDistance(point)) {
+                nearestPoint = point;
+            }
+        }
+        return nearestPoint;
+    }
+
+    /**
+     * Finds the farthest point from a given reference point from a list of points.
+     *
+     * @param mainPoint The reference point to compare distances from.
+     * @param points The list of points to search through.
+     * @return The point farthest from the reference point.
+     */
+    public static Translation2d getFarthestPoint(Translation2d mainPoint, Translation2d... points) {
+        Translation2d farthestPoint = new Translation2d();
+        for (Translation2d point : points) {
+            if (mainPoint.getDistance(farthestPoint) < mainPoint.getDistance(point)) {
+                farthestPoint = point;
+            }
+        }
+        return farthestPoint;
+    }
+
+
+    /**
+     * Calculates the optimal target position for a robot to reach a target while avoiding an obstacle.
      *
      * @param robot      The current position of the robot as a Translation2d.
      * @param target     The target position as a Translation2d.

src/main/java/frc/excalib/control/math/Vector2D.java

@@ -1,5 +1,6 @@
 package frc.excalib.control.math;
 
+import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
@@ -8,6 +9,7 @@
  *
  * @author Itay
  */
+@Logged
 public class Vector2D {
     private double m_x, m_y;
 

src/main/java/frc/excalib/slam/mapper/Odometry.java → src/main/java/frc/excalib/control/odometry/Odometry.java

@@ -1,4 +1,4 @@
-package frc.excalib.slam.mapper;
+package frc.excalib.control.odometry;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;

src/main/java/frc/excalib/control/odometry/VisionMeasurement.java

@@ -0,0 +1,5 @@
+package frc.excalib.control.odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public record VisionMeasurement(Pose2d estimatedRobotPose, double timestampSeconds) {}

src/main/java/frc/excalib/mechanisms/Mechanism.java

@@ -167,12 +167,7 @@ public Command coastCommand(SubsystemBase... requirements) {
      * @param isLinear true if the mechanism is linear, false if it is angular
      * @return a Command that performs a quasistatic test
      */
-    public Command sysIdQuasistatic(Direction direction,
-                                    SubsystemBase subsystem,
-                                    DoubleSupplier positionSupplier,
-                                    SysidConfig config,
-                                    boolean isLinear
-    ) {
+    public Command sysIdQuasistatic(Direction direction, SubsystemBase subsystem, DoubleSupplier positionSupplier, SysidConfig config, boolean isLinear) {
         if (isLinear) return getLinearSysIdRoutine(subsystem, positionSupplier, config).quasistatic(direction);
         return getAngularSysIdRoutine(subsystem, positionSupplier, config).quasistatic(direction);
     }
@@ -187,13 +182,9 @@ public Command sysIdQuasistatic(Direction direction,
      * @param isLinear true if the mechanism is linear, false if it is angular
      * @return a Command that performs a dynamic test
      */
-    public Command sysIdDynamic(Direction direction,
-                                SubsystemBase subsystem,
-                                DoubleSupplier positionSupplier,
-                                SysidConfig config,
-                                boolean isLinear) {
+    public Command sysIdDynamic(Direction direction, SubsystemBase subsystem, DoubleSupplier positionSupplier, SysidConfig config, boolean isLinear) {
         if (isLinear)
             return getLinearSysIdRoutine(subsystem, positionSupplier, config).dynamic(direction);
-        return getAngularSysIdRoutine(subsystem, positionSupplier, config).dynamic(direction).withName("quadForward");
+        return getAngularSysIdRoutine(subsystem, positionSupplier, config).dynamic(direction);
     }
 }

src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java

@@ -39,6 +39,10 @@ public FlyWheel(Motor motor, double maxAcceleration, double maxJerk, Gains gains
         this.m_FF_CONTROLLER = new SimpleMotorFeedforward(gains.ks, gains.kv, gains.ka);
     }
 
+    public FlyWheel(Motor motor, FlyWheel other) {
+        this(motor, other.maxAcceleration, other.maxJerk, other.m_gains);
+    }
+
     /**
      * @param velocitySupplier a dynamic velocity setpoint.
      * @return a command that controls the FlyWheels velocity with high precision