diff --git a/src/main/java/frc/excalib/control/math/Circle.java b/src/main/java/frc/excalib/control/math/Circle.java
index 30c3f7b..451d213 100644
--- a/src/main/java/frc/excalib/control/math/Circle.java
+++ b/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)))
};
}
}
\ No newline at end of file
diff --git a/src/main/java/frc/excalib/control/math/Line.java b/src/main/java/frc/excalib/control/math/Line.java
index 1613b96..cf2b19e 100644
--- a/src/main/java/frc/excalib/control/math/Line.java
+++ b/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]".
*
diff --git a/src/main/java/frc/excalib/control/math/MathUtils.java b/src/main/java/frc/excalib/control/math/MathUtils.java
index a6c993e..9451d7c 100644
--- a/src/main/java/frc/excalib/control/math/MathUtils.java
+++ b/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 pi). Returns pi/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.
diff --git a/src/main/java/frc/excalib/control/math/Vector2D.java b/src/main/java/frc/excalib/control/math/Vector2D.java
index 9680aa4..2f7c994 100644
--- a/src/main/java/frc/excalib/control/math/Vector2D.java
+++ b/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;
diff --git a/src/main/java/frc/excalib/slam/mapper/Odometry.java b/src/main/java/frc/excalib/control/odometry/Odometry.java
similarity index 97%
rename from src/main/java/frc/excalib/slam/mapper/Odometry.java
rename to src/main/java/frc/excalib/control/odometry/Odometry.java
index 72e0427..e906a36 100644
--- a/src/main/java/frc/excalib/slam/mapper/Odometry.java
+++ b/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;
diff --git a/src/main/java/frc/excalib/control/odometry/VisionMeasurement.java b/src/main/java/frc/excalib/control/odometry/VisionMeasurement.java
new file mode 100644
index 0000000..d31f97a
--- /dev/null
+++ b/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) {}
diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanisms/Mechanism.java
index 6db6e89..e569a13 100644
--- a/src/main/java/frc/excalib/mechanisms/Mechanism.java
+++ b/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);
}
}
diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java
index 28160d8..6142623 100644
--- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java
+++ b/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
diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java
index 7803b86..1de86cb 100644
--- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java
+++ b/src/main/java/frc/excalib/mechanisms/turret/Turret.java
@@ -15,11 +15,16 @@
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
+import static java.lang.Double.NEGATIVE_INFINITY;
+import static java.lang.Double.POSITIVE_INFINITY;
+
/**
* A class representing a Turret Mechanism
*/
public final class Turret extends Mechanism {
private final ContinuousSoftLimit m_rotationLimit;
+ private final Gains m_gains;
+ private final double m_PIDtolerance;
private final PIDController m_anglePIDcontroller;
private final SimpleMotorFeedforward m_angleFFcontroller;
private final DoubleSupplier m_POSITION_SUPPLIER;
@@ -35,6 +40,8 @@ public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains,
super(motor);
m_rotationLimit = rotationLimit;
+ m_gains = angleGains;
+ m_PIDtolerance = PIDtolerance;
m_anglePIDcontroller = new PIDController(angleGains.kp, angleGains.ki, angleGains.kd);
m_angleFFcontroller = new SimpleMotorFeedforward(angleGains.ks, angleGains.kv, angleGains.ka);
@@ -44,6 +51,14 @@ public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains,
m_POSITION_SUPPLIER = positionSupplier;
}
+ public Turret(Motor motor, DoubleSupplier positionSupplier, Turret other) {
+ this(motor, other.m_rotationLimit, other.m_gains, other.m_PIDtolerance, positionSupplier);
+ }
+
+ public Turret(Motor motor, Gains angleGains, double PIDtolerance, DoubleSupplier positionSupplier) {
+ this(motor, new ContinuousSoftLimit(() -> NEGATIVE_INFINITY, () -> POSITIVE_INFINITY), angleGains, PIDtolerance, positionSupplier);
+ }
+
/**
* @param wantedPosition a Rotation2d dynamic setpoint
* @return a Command that moves the turret tho the given setpoint
diff --git a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java b/src/main/java/frc/excalib/slam/mapper/AuroraClient.java
deleted file mode 100644
index d9457bc..0000000
--- a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java
+++ /dev/null
@@ -1,110 +0,0 @@
-package frc.excalib.slam.mapper;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.wpilibj.DriverStation;
-
-import java.io.DataInputStream;
-import java.io.IOException;
-import java.net.ServerSocket;
-import java.net.Socket;
-
-public class AuroraClient {
- private ServerSocket serverSocket;
- private Thread serverThread;
- private volatile boolean running = true;
-
- // Pose Data (volatile for thread safety)
- private volatile float x = 0;
- private volatile float y = 0;
- private volatile float z = 0;
- private volatile float roll = 0;
- private volatile float pitch = 0;
- private volatile float yaw = 0;
-
- public AuroraClient(int port) {
- serverThread = new Thread(() -> {
- try {
- serverSocket = new ServerSocket(port);
- DriverStation.reportWarning("Localization server started on port " + port, false);
-
- while (running) {
- try (Socket clientSocket = serverSocket.accept();
- DataInputStream in = new DataInputStream(clientSocket.getInputStream())) {
-
- DriverStation.reportWarning("Localization client connected!", false);
-
- while (running) {
- try {
- x = in.readFloat();
- y = in.readFloat();
- z = in.readFloat();
- roll = in.readFloat();
- pitch = in.readFloat();
- yaw = in.readFloat();
- } catch (IOException e) {
- DriverStation.reportError("Error reading localization data: " + e.getMessage(), false);
- break;
- }
- }
- } catch (IOException e) {
- DriverStation.reportError("Client connection error: " + e.getMessage(), false);
- }
- }
- } catch (IOException e) {
- DriverStation.reportError("Localization server error: " + e.getMessage(), false);
- }
- });
-
- serverThread.setDaemon(true);
- serverThread.start();
- }
-
- // Getter methods for retrieving pose data
- public float getX() {
- return x;
- }
-
- public float getY() {
- return y;
- }
-
- public float getZ() {
- return z;
- }
-
- public float getRoll() {
- return roll;
- }
-
- public float getPitch() {
- return pitch;
- }
-
- public float getYaw() {
- return yaw;
- }
-
- public Pose3d getPose3d() {
- return new Pose3d(x, y, z, new Rotation3d(roll, pitch, yaw));
- }
-
- public Pose2d getPose2d() {
- return new Pose2d(x, y, new Rotation2d(yaw));
- }
-
- // Stops the server
- public void stop() {
- running = false;
- try {
- if (serverSocket != null) {
- serverSocket.close();
- }
- } catch (IOException e) {
- DriverStation.reportError("Failed to close localization server: " + e.getMessage(), false);
- }
- }
-}
-
diff --git a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java b/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java
deleted file mode 100644
index ecbea3e..0000000
--- a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java
+++ /dev/null
@@ -1,91 +0,0 @@
-package frc.excalib.slam.mapper;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import monologue.Logged;
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.targeting.PhotonPipelineResult;
-
-import java.util.List;
-import java.util.Optional;
-import java.util.function.BiConsumer;
-
-import static org.photonvision.PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY;
-import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
-
-public class PhotonAprilTagsCamera implements Logged {
- private final PhotonCamera camera;
- private final AprilTagFieldLayout fieldLayout;
- private final PhotonPoseEstimator photonPoseEstimator;
-
- public PhotonAprilTagsCamera(String cameraName, AprilTagFields aprilTagField, Transform3d robotToCamera) {
- camera = new PhotonCamera(cameraName);
- camera.setDriverMode(false);
-
- fieldLayout = AprilTagFieldLayout.loadField(aprilTagField);
-
- photonPoseEstimator = new PhotonPoseEstimator(fieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera);
- photonPoseEstimator.setMultiTagFallbackStrategy(LOWEST_AMBIGUITY);
- }
-
- public PhotonCamera getCamera() {
- return camera;
- }
-
- public void setDriverMode(boolean isDriverMode) {
- camera.setDriverMode(isDriverMode);
- }
-
- public void setPipeline(int index) {
- camera.setPipelineIndex(index);
- }
-
- public PhotonPipelineResult getFirstLatestResult() {
- var result = camera.getAllUnreadResults();
-
- if (result != null) return result.getFirst();
- return new PhotonPipelineResult();
- }
-
- public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
- List unreadResults = camera.getAllUnreadResults();
-
- for (PhotonPipelineResult result : unreadResults)
- if (result.hasTargets() && result.getBestTarget().getPoseAmbiguity() < 0.2) {
- Translation2d targetTranslation = result.getBestTarget().getBestCameraToTarget().getTranslation().toTranslation2d();
- // m
- double TOO_FAR = 3.5;
- if (targetTranslation.getDistance(new Translation2d(0, 0)) < TOO_FAR) {
- photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
- return photonPoseEstimator.update(result);
- }
- }
-
- return Optional.empty();
- }
-
- public boolean updateFromAprilTagPose(BiConsumer toUpdate) {
- var unreadResults = camera.getAllUnreadResults();
- for (PhotonPipelineResult result : unreadResults)
- if (!result.hasTargets()) return false;
-
- for (PhotonPipelineResult result : unreadResults) {
- var id = result.getBestTarget().getFiducialId();
- if (id == -1) return false;
- var tag = fieldLayout.getTagPose(id);
- if (tag.isEmpty()) return false;
- toUpdate.accept(
- tag.get().plus(
- result.getBestTarget().getBestCameraToTarget()).toPose2d(),
- result.getTimestampSeconds()
- );
- }
- return true;
- }
-}
-
diff --git a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java b/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java
deleted file mode 100644
index 03e377d..0000000
--- a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java
+++ /dev/null
@@ -1,21 +0,0 @@
-package frc.excalib.slam.mapper;
-
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-
-import java.util.function.Supplier;
-
-public class PoseEstimator extends SwerveDrivePoseEstimator {
- private final Supplier m_YAW_SUPPLIER;
- private Pose2d m_robotPose;
-
- public PoseEstimator(SwerveDriveKinematics kinematics, Supplier yawSupplier, SwerveModulePosition[] modulePositions, Pose2d initialPose) {
- super(kinematics, yawSupplier.get(), modulePositions, initialPose);
- m_YAW_SUPPLIER = yawSupplier;
- }
-
-
-}
diff --git a/src/main/java/frc/excalib/swerve/ModulesHolder.java b/src/main/java/frc/excalib/swerve/ModulesHolder.java
index 2cf7661..5c06049 100644
--- a/src/main/java/frc/excalib/swerve/ModulesHolder.java
+++ b/src/main/java/frc/excalib/swerve/ModulesHolder.java
@@ -1,28 +1,28 @@
package frc.excalib.swerve;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.NotLogged;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.excalib.control.math.Vector2D;
-import monologue.Logged;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
-import static frc.robot.SwerveConstants.MAX_VEL;
-import static monologue.Annotations.Log;
-
-public class ModulesHolder implements Logged {
- public final SwerveModule m_frontLeft;
- public final SwerveModule m_frontRight;
- public final SwerveModule m_backLeft;
- public final SwerveModule m_backRight;
+@Logged
+public class ModulesHolder {
+ final SwerveModule m_frontLeft;
+ final SwerveModule m_frontRight;
+ final SwerveModule m_backLeft;
+ final SwerveModule m_backRight;
+ @NotLogged
private final SwerveDriveKinematics m_swerveDriveKinematics;
- private final SwerveModulePosition[] m_modulePositions;
+ private final SwerveModulePosition[] m_modulesPositions;
/**
* A constructor that initialize the ModulesHolder.
@@ -32,25 +32,20 @@ public class ModulesHolder implements Logged {
* @param backLeft A SwerveModule represents the back-left module.
* @param backRight A SwerveModule represents the back-right module.
*/
- public ModulesHolder(
- SwerveModule frontLeft,
- SwerveModule frontRight,
- SwerveModule backLeft,
- SwerveModule backRight) {
- this.m_frontLeft = frontLeft;
- this.m_frontRight = frontRight;
- this.m_backLeft = backLeft;
- this.m_backRight = backRight;
-
- // Initialize SwerveDriveKinematics once since module locations are constant
- this.m_swerveDriveKinematics = new SwerveDriveKinematics(
- frontLeft.m_MODULE_LOCATION,
- frontRight.m_MODULE_LOCATION,
- backLeft.m_MODULE_LOCATION,
- backRight.m_MODULE_LOCATION
+ public ModulesHolder(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft, SwerveModule backRight) {
+ m_frontLeft = frontLeft;
+ m_frontRight = frontRight;
+ m_backLeft = backLeft;
+ m_backRight = backRight;
+
+ m_swerveDriveKinematics = new SwerveDriveKinematics(
+ frontLeft.m_moduleLocation,
+ frontRight.m_moduleLocation,
+ backLeft.m_moduleLocation,
+ backRight.m_moduleLocation
);
- m_modulePositions = new SwerveModulePosition[]{
+ m_modulesPositions = new SwerveModulePosition[]{
m_frontLeft.getModulePosition(),
m_frontRight.getModulePosition(),
m_backLeft.getModulePosition(),
@@ -59,76 +54,34 @@ public ModulesHolder(
}
/**
- * Stops all swerve modules.
- */
- public void stop() {
- m_frontLeft.stopModule();
- m_frontRight.stopModule();
- m_backLeft.stopModule();
- m_backRight.stopModule();
- }
-
- /**
- * Gets the robot's average velocity based on the velocities of all modules.
- *
- * @return a Vector2D representing the robot's velocity.
- */
- public Vector2D getVelocity() {
- // Sum the velocities of all modules
- double totalX = m_frontLeft.getVelocity().getX()
- + m_frontRight.getVelocity().getX()
- + m_backLeft.getVelocity().getX()
- + m_backRight.getVelocity().getX();
-
- double totalY = m_frontLeft.getVelocity().getY()
- + m_frontRight.getVelocity().getY()
- + m_backLeft.getVelocity().getY()
- + m_backRight.getVelocity().getY();
-
- // Compute the average velocity
- return new Vector2D(totalX * 0.25, totalY * 0.25);
- }
-
- @Log.NT(key = "angular vel")
- public double getOmegaRadPerSec() {
- return new SwerveDriveKinematics(
- m_frontLeft.m_MODULE_LOCATION,
- m_frontRight.m_MODULE_LOCATION,
- m_backLeft.m_MODULE_LOCATION,
- m_backRight.m_MODULE_LOCATION
- ).toChassisSpeeds(logStates()).omegaRadiansPerSecond;
- }
-
- @Log.NT(key = "swerve velocity")
- public double getVelocityDistance() {
- return getVelocity().getDistance();
- }
-
- /**
- * Calculates the minimum velocity ratio limit among all modules.
+ * A method that calculates the minimum velocity ratio limit among all modules.
*
- * @param translationVelocity The desired translation velocity.
- * @param omegaRadPerSec The desired rotation rate in radians per second.
+ * @param translationVelocity The desired translation velocity of the robot.
+ * @param omegaRadPerSec The desired angular velocity of the robot in radians per second.
* @return The velocity ratio limit.
*/
private double calcVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) {
- double flLimit = m_frontLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec);
- double frLimit = m_frontRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec);
- double blLimit = m_backLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec);
- double brLimit = m_backRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec);
-
- double velocityRatioLimit = Math.min(Math.min(flLimit, frLimit), Math.min(blLimit, brLimit));
+ double velocityRatioLimit = Math.min(
+ Math.min(
+ m_frontLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec),
+ m_frontRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec)
+ ),
+ Math.min(
+ m_backLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec),
+ m_backRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec)
+ )
+ );
return Math.min(1.0, velocityRatioLimit); // Ensure the limit does not exceed 1.0
}
/**
- * Sets the velocities of all modules based on the desired translation and rotation velocities.
+ * A method that sets the velocities of all modules based on the desired translation and rotation velocities of the robot.
*
- * @param omega The desired rotation rate supplier.
* @param translationalVel The desired translation velocity supplier.
- * @return A command to set the velocities.
+ * @param omega The desired angular velocity supplier.
+ * @return A command that sets the velocities.
*/
- public Command setVelocitiesCommand(Supplier translationalVel, DoubleSupplier omega) {
+ Command setVelocitiesCommand(Supplier translationalVel, DoubleSupplier omega) {
return new ParallelCommandGroup(
m_frontLeft.setVelocityCommand(
translationalVel,
@@ -153,7 +106,20 @@ public Command setVelocitiesCommand(Supplier translationalVel, DoubleS
);
}
- public Command coastCommand() {
+ /**
+ * A method that stops all swerve modules.
+ */
+ void stop() {
+ m_frontLeft.stopModule();
+ m_frontRight.stopModule();
+ m_backLeft.stopModule();
+ m_backRight.stopModule();
+ }
+
+ /**
+ * @return A Command that sets the idle state of all modules to coast.
+ */
+ Command coastCommand() {
return new ParallelCommandGroup(
m_frontLeft.coastCommand(),
m_frontRight.coastCommand(),
@@ -162,8 +128,12 @@ public Command coastCommand() {
);
}
- public void setModulesStates(SwerveModuleState[] states) {
- SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VEL);
+ /**
+ * A method that sets the states of the modules to the desired states.
+ *
+ * @param states an array represents the wanted states of the modules.
+ */
+ void setModulesStates(SwerveModuleState[] states) {
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_backLeft.setDesiredState(states[2]);
@@ -171,27 +141,72 @@ public void setModulesStates(SwerveModuleState[] states) {
}
/**
- * Logs the states of all modules.
+ * A method to get the robot's average sigma velocity based on the velocities of all modules.
+ *
+ * @return a Vector2D represents the robot's velocity.
+ */
+ Vector2D getSigmaVelocity() {
+ // Sum the velocities of all modules
+ double totalX = m_frontLeft.getVelocity().getX()
+ + m_frontRight.getVelocity().getX()
+ + m_backLeft.getVelocity().getX()
+ + m_backRight.getVelocity().getX();
+
+ double totalY = m_frontLeft.getVelocity().getY()
+ + m_frontRight.getVelocity().getY()
+ + m_backLeft.getVelocity().getY()
+ + m_backRight.getVelocity().getY();
+
+ // Compute the average velocity
+ return new Vector2D(totalX / 4, totalY /4);
+ }
+
+ /**
+ * A method to get the linear velocity of the robot.
+ *
+ * @return the linear velocity of the robot.
+ */
+ Vector2D getLinearVelocity() {
+ return new Vector2D(
+ m_swerveDriveKinematics.toChassisSpeeds(getStates()).vxMetersPerSecond,
+ m_swerveDriveKinematics.toChassisSpeeds(getStates()).vyMetersPerSecond
+ );
+ }
+
+ /**
+ * A method to get the angular velocity of the robot.
+ *
+ * @return the angular velocity of the robot.
+ */
+ double getOmegaRadPerSec() {
+ return m_swerveDriveKinematics.toChassisSpeeds(getStates()).omegaRadiansPerSecond;
+ }
+
+ /**
+ * A method to get the states of all modules.
*
* @return An array of SwerveModuleState representing the states of the modules.
*/
- @Log.NT(key = "Swerve States")
- public SwerveModuleState[] logStates() {
+ SwerveModuleState[] getStates() {
return new SwerveModuleState[]{
- m_frontLeft.logState(),
- m_frontRight.logState(),
- m_backLeft.logState(),
- m_backRight.logState()
+ m_frontLeft.getState(),
+ m_frontRight.getState(),
+ m_backLeft.getState(),
+ m_backRight.getState()
};
}
- @Log.NT(key = "SetPoints")
- public SwerveModuleState[] logSetPointStates() {
+ /**
+ * A method to get the desired states of all modules.
+ *
+ * @return An array of SwerveModuleState representing the desired states of the modules.
+ */
+ SwerveModuleState[] getDesiredStates() {
return new SwerveModuleState[]{
- m_frontLeft.logSetpointState(),
- m_frontRight.logSetpointState(),
- m_backLeft.logSetpointState(),
- m_backRight.logSetpointState()
+ m_frontLeft.getDesiredState(),
+ m_frontRight.getDesiredState(),
+ m_backLeft.getDesiredState(),
+ m_backRight.getDesiredState()
};
}
@@ -200,7 +215,7 @@ public SwerveModuleState[] logSetPointStates() {
*
* @return The SwerveDriveKinematics instance.
*/
- public SwerveDriveKinematics getSwerveDriveKinematics() {
+ SwerveDriveKinematics getSwerveDriveKinematics() {
return m_swerveDriveKinematics;
}
@@ -209,10 +224,13 @@ public SwerveDriveKinematics getSwerveDriveKinematics() {
*
* @return An array of SwerveModulePosition representing the positions of the modules.
*/
- public SwerveModulePosition[] getModulesPositions() {
- return m_modulePositions;
+ SwerveModulePosition[] getModulesPositions() {
+ return m_modulesPositions;
}
+ /**
+ * A method that updates the modules positions of all modules. should be called periodically.
+ */
public void periodic() {
m_frontLeft.periodic();
m_frontRight.periodic();
diff --git a/src/main/java/frc/excalib/swerve/Swerve.java b/src/main/java/frc/excalib/swerve/Swerve.java
deleted file mode 100644
index 3779bf5..0000000
--- a/src/main/java/frc/excalib/swerve/Swerve.java
+++ /dev/null
@@ -1,501 +0,0 @@
-package frc.excalib.swerve;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import com.pathplanner.lib.path.PathPlannerPath;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj2.command.*;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-import frc.excalib.additional_utilities.AllianceUtils;
-import frc.excalib.additional_utilities.Elastic;
-import frc.excalib.control.gains.SysidConfig;
-import frc.excalib.control.imu.IMU;
-import frc.excalib.control.math.Vector2D;
-import frc.excalib.control.motor.controllers.SparkMaxMotor;
-import frc.excalib.control.motor.controllers.TalonFXMotor;
-import frc.excalib.slam.mapper.Odometry;
-import frc.excalib.slam.mapper.PhotonAprilTagsCamera;
-import monologue.Logged;
-import org.json.simple.parser.ParseException;
-import org.photonvision.EstimatedRobotPose;
-
-import java.io.IOException;
-import java.util.Optional;
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-import java.util.function.Supplier;
-
-import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless;
-import static edu.wpi.first.apriltag.AprilTagFields.*;
-import static frc.excalib.additional_utilities.Elastic.Notification.NotificationLevel.WARNING;
-import static frc.robot.SwerveConstants.*;
-import static monologue.Annotations.Log;
-
-/**
- * A class representing a swerve subsystem.
- */
-public class Swerve extends SubsystemBase implements Logged {
- private final ModulesHolder modules;
- private final IMU imu;
- private final Odometry odometry;
- PhotonAprilTagsCamera exampleCamera;
- private ChassisSpeeds desiredChassisSpeeds = new ChassisSpeeds();
- private final Trigger finishTrigger;
- private final Rotation2d PI = new Rotation2d(Math.PI);
- private final InterpolatingDoubleTreeMap velocityLimit = new InterpolatingDoubleTreeMap();
-
- private final SwerveDriveKinematics swerveDriveKinematics;
-
- private final PIDController angleController = new PIDController(ANGLE_GAINS.kp, ANGLE_GAINS.ki, ANGLE_GAINS.kd);
- private final PIDController xController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd);
- private final PIDController yController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd);
- public final Field2d field = new Field2d();
- private Supplier angleSetpoint = Rotation2d::new;
- private Supplier translationSetpoint = Translation2d::new;
-
- /**
- * A constructor that initialize the Swerve Subsystem
- *
- * @param modules The ModulesHolder containing all swerve modules.
- * @param imu IMU sensor.
- * @param initialPosition The initial position of the robot.
- */
- public Swerve(ModulesHolder modules, IMU imu, Pose2d initialPosition) {
- this.modules = modules;
- this.imu = imu;
- this.imu.resetIMU();
-
- angleController.enableContinuousInput(-Math.PI, Math.PI);
- angleController.setTolerance(ANGLE_CONTROLLER_TOLORANCE);
- xController.setTolerance(X_CONTROLLER_TOLORANCE);
- yController.setTolerance(Y_CONTROLLER_TOLORANCE);
-
- finishTrigger = new Trigger(xController::atSetpoint).and(yController::atSetpoint).and(angleController::atSetpoint).debounce(0.1);
- this.odometry = new Odometry(modules.getSwerveDriveKinematics(), modules.getModulesPositions(), this.imu::getZRotation, initialPosition);
- PhotonAprilTagsCamera m_exampleCamera = new PhotonAprilTagsCamera("example", k2025ReefscapeWelded, new Transform3d(0, 0, 0, new Rotation3d()));
-
- swerveDriveKinematics = this.modules.getSwerveDriveKinematics();
- velocityLimit.put(0.1, 0.4);
- velocityLimit.put(0.7, 2.0);
- velocityLimit.put(1.5, MAX_VEL);
-
- initAutoBuilder();
- }
-
- /**
- * Creates a drive command for the swerve drive.
- *
- * @param velocityMPS Supplier for the desired linear velocity in meters per second.
- * @param omegaRadPerSec Supplier for the desired rotational velocity in radians per second.
- * @param fieldOriented Supplier indicating whether the control is field-oriented.
- * @return A command that drives the robot.
- */
- public Command driveCommand(
- Supplier velocityMPS,
- DoubleSupplier omegaRadPerSec,
- BooleanSupplier fieldOriented) {
-
- // Precompute values to avoid redundant calculations
- Supplier adjustedVelocitySupplier = () -> {
- Vector2D velocity = velocityMPS.get();
-// Vector2D velocity = getSmartTranslationalVelocitySetPoint(getVelocity(), velocityMPS.get());
- if (fieldOriented.getAsBoolean()) {
- Rotation2d yaw = getRotation2D().unaryMinus();
- if (!AllianceUtils.isBlueAlliance()) yaw = yaw.plus(PI);
- return velocity.rotate(yaw);
- }
- return velocity;
- };
-
- Command driveCommand = new ParallelCommandGroup(modules.setVelocitiesCommand(
- adjustedVelocitySupplier,
- omegaRadPerSec
- ),
- new RunCommand(
- () -> desiredChassisSpeeds = new ChassisSpeeds(
- adjustedVelocitySupplier.get().getX(),
- adjustedVelocitySupplier.get().getY(),
- omegaRadPerSec.getAsDouble())
- )
- );
- driveCommand.setName("Drive Command");
- driveCommand.addRequirements(this);
- return driveCommand;
- }
-
- /**
- * A non-command function that drives the robot (for pathplanner).
- *
- * @param speeds A ChassisSpeeds object represents ROBOT RELATIVE speeds desired speeds.
- */
- public void driveRobotRelativeChassisSpeeds(ChassisSpeeds speeds) {
- modules.setModulesStates(swerveDriveKinematics.toSwerveModuleStates(speeds));
- }
-
- /**
- * A method that turns the robot to a desired angle.
- *
- * @param angleSetpoint The desired angle in radians.
- * @return A command that turns the robot to the wanted angle.
- */
- public Command turnToAngleCommand(Supplier velocityMPS, Supplier angleSetpoint) {
- return new SequentialCommandGroup(
- new InstantCommand(() -> this.angleSetpoint = angleSetpoint),
- driveCommand(
- velocityMPS,
- () -> angleController.calculate(getRotation2D().getRadians(), angleSetpoint.get().getRadians()),
- () -> true
- )
- ).withName("Turn To Angle");
- }
-
- public Command pidToPoseCommand(Supplier poseSetpoint) {
- return new SequentialCommandGroup(
- new InstantCommand(
- () -> {
- xController.calculate(getPose2D().getX(), poseSetpoint.get().getX());
- yController.calculate(getPose2D().getY(), poseSetpoint.get().getY());
- angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians());
- translationSetpoint = () -> poseSetpoint.get().getTranslation();
- angleSetpoint = () -> poseSetpoint.get().getRotation();
- }
- ),
- driveCommand(
- () -> {
- Vector2D vel = new Vector2D(
- xController.calculate(getPose2D().getX(), poseSetpoint.get().getX()),
- yController.calculate(getPose2D().getY(), poseSetpoint.get().getY())
- );
- double distance = getPose2D().getTranslation().getDistance(poseSetpoint.get().getTranslation());
- vel.setMagnitude(Math.min(vel.getDistance(), velocityLimit.get(distance)));
-// vel = vel.rotate(poseSetpoint.get().getRotation());
-// vel.setX(Math.signum(vel.getX()) * Math.min(Math.abs(vel.getX()), 1.2));
-// vel.setY(Math.signum(vel.getY()) * Math.min(Math.abs(vel.getY()), 0.4));
-// vel = vel.rotate(poseSetpoint.get().getRotation().unaryMinus());
- if (!AllianceUtils.isBlueAlliance()) return vel.rotate(PI);
- return vel;
- },
- () -> angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()),
- () -> true
- )
- ).until(finishTrigger).withName("PID To Pose");
- }
-
- /**
- * A method that drives the robot to a desired pose.
- *
- * @param setPoint The desired pose.
- * @return A command that drives the robot to the wanted pose.
- */
- public Command driveToPoseCommand(Pose2d setPoint) {
- return AutoBuilder.pathfindToPose(
- setPoint,
- MAX_PATH_CONSTRAINTS
- ).withName("Pathfinding Command");
- }
-
- public Command driveToPoseWithOverrideCommand(
- Pose2d setPoint,
- BooleanSupplier override,
- Supplier velocityMPS,
- DoubleSupplier omegaRadPerSec) {
- Command driveToPoseCommand = driveToPoseCommand(setPoint);
- return new SequentialCommandGroup(
- driveToPoseCommand.until(() -> velocityMPS.get().getDistance() != 0 && override.getAsBoolean()),
- driveCommand(
- velocityMPS,
- omegaRadPerSec,
- () -> true
- ).until(() -> velocityMPS.get().getDistance() == 0)
- ).repeatedly().until(driveToPoseCommand::isFinished).withName("Pathfinding With Override Command");
- }
-
- /**
- * A method that drives the robot to the starting pose of a path, then follows the path.
- *
- * @param pathName The path which the robot needs to follow.
- * @return A command that turns the robot to the wanted angle.
- */
- public Command pathfindThenFollowPathCommand(String pathName) {
- PathPlannerPath path;
- try {
- path = PathPlannerPath.fromPathFile(pathName);
- } catch (IOException | ParseException e) {
- Elastic.sendNotification(new Elastic.Notification(
- WARNING,
- "Path Creating Error",
- "the path file " + pathName + " doesn't exist")
- );
- return new PrintCommand("this path file doesn't exist");
- }
-
- return AutoBuilder.pathfindThenFollowPath(
- path,
- MAX_PATH_CONSTRAINTS
- );
- }
-
- public Command resetAngleCommand() {
- return new InstantCommand(imu::resetIMU).ignoringDisable(true);
- }
-
- public Command coastCommand() {
- Command coastCommand = modules.coastCommand().ignoringDisable(true).withName("Coast Command");
- coastCommand.addRequirements(this);
- return coastCommand;
- }
-
- /**
- * Updates the robot's odometry.
- */
- public void updateOdometry() {
- odometry.updateOdometry(modules.getModulesPositions());
- Optional backPose = exampleCamera.getEstimatedGlobalPose(odometry.getEstimatedPosition());
- backPose.ifPresent(
- estimatedRobotPose -> odometry.addVisionMeasurement(
- estimatedRobotPose.estimatedPose.toPose2d(),
- estimatedRobotPose.timestampSeconds
- )
- );
- }
-
- /**
- * A method that restarts the odometry.
- *
- * @param newPose the wanted new Pose2d of the robot.
- */
- public void resetOdometry(Pose2d newPose) {
- odometry.resetOdometry(modules.getModulesPositions(), newPose);
- }
-
- /**
- * Gets the robot's rotation.
- *
- * @return The current rotation of the robot.
- */
- @Log.NT(key = "Robot Rotation")
- public Rotation2d getRotation2D() {
- return getPose2D().getRotation();
- }
-
- @Log.NT(key = "Angle Setpoint")
- public Rotation2d getAngleSetpoint() {
- return angleSetpoint.get();
- }
-
- @Log.NT(key = "Translation Setpoint")
- public Translation2d getTranslationSetpoint() {
- return translationSetpoint.get();
- }
-
- /**
- * Gets the robot's pose.
- *
- * @return The current pose of the robot.
- */
- @Log.NT(key = "Robot Pose")
- public Pose2d getPose2D() {
- return odometry.getRobotPose();
- }
-
- /**
- * Gets the current velocity of the robot.
- *
- * @return The robot's velocity as a Vector2D.
- */
- public Vector2D getVelocity() {
- return modules.getVelocity();
- }
-
- /**
- * Gets the current acceleration of the robot.
- *
- * @return The robot's acceleration as a Vector2D.
- */
- @Log.NT(key = "Acceleration")
- public double getAccelerationDistance() {
- return new Vector2D(imu.getAccX(), imu.getAccY()).getDistance();
- }
-
- /**
- * Gets the current robot relative speed.
- *
- * @return The robot's speed as a ChassisSpeeds.
- */
- @Log.NT(key = "Measured Chassis Speeds")
- public ChassisSpeeds getRobotRelativeSpeeds() {
- return swerveDriveKinematics.toChassisSpeeds(modules.logStates());
- }
-
- @Log.NT
- public ChassisSpeeds getDesiredChassisSpeeds() {
- return desiredChassisSpeeds;
- }
-
- public Command stopCommand() {
- return driveCommand(() -> new Vector2D(0, 0), () -> 0, () -> true);
- }
-
- /**
- * A function that initialize the AutoBuilder for pathplanner.
- */
- private void initAutoBuilder() {
- // Load the RobotConfig from the GUI settings. You should probably
- // store this in your Constants file
- RobotConfig config = null;
- try {
- config = RobotConfig.fromGUISettings();
- } catch (Exception e) {
- // Handle exception as needed
- e.printStackTrace();
- }
-
- // Configure AutoBuilder last
- assert config != null;
- AutoBuilder.configure(
- this::getPose2D, // Robot pose supplier
- this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
- this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
- (speeds, feedforwards) -> driveRobotRelativeChassisSpeeds(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
- new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
- TRANSLATION_PID_CONSTANTS, // Translation PID constants
- ANGLE_PID_CONSTANTS // Rotation PID constants
- ),
- config, // The robot configuration
- () -> {
- // Boolean supplier that controls when the path will be mirrored for the red alliance
- // This will flip the path being followed to the red side of the field.
- // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
- var alliance = DriverStation.getAlliance();
- return alliance.filter(value -> value == DriverStation.Alliance.Red).isPresent();
- },
- this // Reference to this subsystem to set requirements
- );
- }
-
-
- /**
- * Runs a system identification routine on a specific module's angle.
- *
- * @param module The module index (0-3).
- * @param dir The direction of the sysid routine.
- * @param dynamic Whether to perform a dynamic or quasistatic test.
- * @return The command to perform the sysid routine.
- */
- public Command driveSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) {
- SwerveModule selectedModule;
-
- switch (module) {
- case 0 -> selectedModule = modules.m_frontLeft;
- case 1 -> selectedModule = modules.m_frontRight;
- case 2 -> selectedModule = modules.m_backLeft;
- case 3 -> selectedModule = modules.m_backRight;
- default -> {
- throw new IllegalArgumentException("Invalid module index: " + module);
- }
- }
- return dynamic ?
- selectedModule.driveSysIdDynamic(dir, this, sysidConfig)
- : selectedModule.driveSysIdQuas(dir, this, sysidConfig);
- }
-
- /**
- * Runs a system identification routine on a specific module's angle.
- *
- * @param module The module index (0-3).
- * @param dir The direction of the sysid routine.
- * @param dynamic Whether to perform a dynamic or quasistatic test.
- * @return The command to perform the sysid routine.
- */
- public Command angleSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) {
- SwerveModule selectedModule;
-
- switch (module) {
- case 0 -> selectedModule = modules.m_frontLeft;
- case 1 -> selectedModule = modules.m_frontRight;
- case 2 -> selectedModule = modules.m_backLeft;
- case 3 -> selectedModule = modules.m_backRight;
- default -> {
- throw new IllegalArgumentException("Invalid module index: " + module);
- }
- }
- return dynamic ?
- selectedModule.angleSysIdDynamic(dir, this, sysidConfig)
- : selectedModule.angleSysIdQuas(dir, this, sysidConfig);
- }
-
- public static Swerve configureSwerve(Pose2d initialPose) {
- return new Swerve(
- new ModulesHolder(
- new SwerveModule(
- new TalonFXMotor(FRONT_LEFT_DRIVE_ID, SWERVE_CANBUS),
- new SparkMaxMotor(FRONT_LEFT_ROTATION_ID, kBrushless),
- SWERVE_DRIVE_MODULE_GAINS,
- SWERVE_ROTATION_MODULE_GAINS,
- PID_TOLERANCE,
- FRONT_LEFT_TRANSLATION,
- () -> FRONT_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI,
- MAX_MODULE_VEL,
- VELOCITY_CONVERSION_FACTOR,
- POSITION_CONVERSION_FACTOR,
- ROTATION_VELOCITY_CONVERSION_FACTOR
- ),
- new SwerveModule(
- new TalonFXMotor(FRONT_RIGHT_DRIVE_ID, SWERVE_CANBUS),
- new SparkMaxMotor(FRONT_RIGHT_ROTATION_ID, kBrushless),
- SWERVE_DRIVE_MODULE_GAINS,
- SWERVE_ROTATION_MODULE_GAINS,
- PID_TOLERANCE,
- FRONT_RIGHT_TRANSLATION,
- () -> FRONT_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI,
- MAX_MODULE_VEL,
- VELOCITY_CONVERSION_FACTOR,
- POSITION_CONVERSION_FACTOR,
- ROTATION_VELOCITY_CONVERSION_FACTOR
- ),
- new SwerveModule(
- new TalonFXMotor(BACK_LEFT_DRIVE_ID, SWERVE_CANBUS),
- new SparkMaxMotor(BACK_LEFT_ROTATION_ID, kBrushless),
- SWERVE_DRIVE_MODULE_GAINS,
- SWERVE_ROTATION_MODULE_GAINS,
- PID_TOLERANCE,
- BACK_LEFT_TRANSLATION,
- () -> BACK_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI,
- MAX_MODULE_VEL,
- VELOCITY_CONVERSION_FACTOR,
- POSITION_CONVERSION_FACTOR,
- ROTATION_VELOCITY_CONVERSION_FACTOR
- ),
- new SwerveModule(
- new TalonFXMotor(BACK_RIGHT_DRIVE_ID, SWERVE_CANBUS),
- new SparkMaxMotor(BACK_RIGHT_ROTATION_ID, kBrushless),
- SWERVE_DRIVE_MODULE_GAINS,
- SWERVE_ROTATION_MODULE_GAINS,
- PID_TOLERANCE,
- BACK_RIGHT_TRANSLATION,
- () -> BACK_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI,
- MAX_MODULE_VEL,
- VELOCITY_CONVERSION_FACTOR,
- POSITION_CONVERSION_FACTOR,
- ROTATION_VELOCITY_CONVERSION_FACTOR
- )),
- GYRO,
- initialPose
- );
- }
-
- @Override
- public void periodic() {
- modules.periodic();
- field.setRobotPose(getPose2D());
- }
-}
diff --git a/src/main/java/frc/excalib/swerve/SwerveMechanism.java b/src/main/java/frc/excalib/swerve/SwerveMechanism.java
new file mode 100644
index 0000000..650fe5c
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/SwerveMechanism.java
@@ -0,0 +1,333 @@
+package frc.excalib.swerve;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.excalib.additional_utilities.AllianceUtils;
+import frc.excalib.control.gains.SysidConfig;
+import frc.excalib.control.imu.IMU;
+import frc.excalib.control.math.Vector2D;
+import frc.excalib.control.odometry.Odometry;
+import frc.excalib.control.odometry.VisionMeasurement;
+import frc.excalib.swerve.swerve_utils.SwerveAccUtils;
+import frc.excalib.swerve.swerve_utils.SwerveModuleOption;
+import frc.excalib.swerve.swerve_utils.SwerveSpecs;
+import frc.excalib.swerve.swerve_utils.SysIdRoutineOption;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.math.geometry.Rotation2d.kPi;
+import static frc.excalib.swerve.swerve_utils.SwerveAccUtils.getSmartTranslationalVelocitySetpoint;
+
+/**
+ * A class representing a swerve subsystem.
+ */
+@Logged
+public class SwerveMechanism {
+ private final ModulesHolder m_modules;
+ private final IMU m_imu;
+ private final SwerveSpecs m_specs;
+ private final Odometry m_odometry;
+ private ChassisSpeeds m_desiredChassisSpeeds = new ChassisSpeeds();
+
+ /**
+ * A constructor that initialize the Swerve Subsystem
+ *
+ * @param modules The ModulesHolder containing all swerve modules.
+ * @param imu The IMU sensor.
+ * @param initialPosition The initial position of the robot.
+ */
+ public SwerveMechanism(ModulesHolder modules, SwerveSpecs swerveSpecs, IMU imu, Pose2d initialPosition) {
+ m_modules = modules;
+ m_specs = swerveSpecs;
+ m_imu = imu;
+
+ m_odometry = new Odometry(
+ modules.getSwerveDriveKinematics(),
+ modules.getModulesPositions(),
+ m_imu::getZRotation,
+ initialPosition
+ );
+
+ SwerveAccUtils.setSwerveSpecs(m_specs);
+ initModulesDashboard();
+ }
+
+ /**
+ * Creates a drive command for the swerve drive.
+ *
+ * @param velocityMPS Supplier for the desired linear velocity in meters per second.
+ * @param omegaRadPerSec Supplier for the desired angular velocity in radians per second.
+ * @param fieldOriented Supplier indicating whether the control is field-oriented.
+ * @param withAccLimits whether to use all limits or only the skid limit
+ * @return A command that drives the robot.
+ */
+ public Command driveCommand(Supplier velocityMPS, DoubleSupplier omegaRadPerSec,
+ BooleanSupplier fieldOriented, boolean withAccLimits, SubsystemBase... requirements) {
+
+ Supplier adjustedVelocitySupplier = () -> adjustVectorByOrientation(
+ getSmartTranslationalVelocitySetpoint(getSigmaRobotVelocity(), velocityMPS.get(), withAccLimits),
+ fieldOriented
+ );
+
+ Command driveCommand = new ParallelCommandGroup(
+ m_modules.setVelocitiesCommand(adjustedVelocitySupplier, omegaRadPerSec),
+ new RunCommand(
+ () -> m_desiredChassisSpeeds = new ChassisSpeeds(
+ adjustedVelocitySupplier.get().getX(),
+ adjustedVelocitySupplier.get().getY(),
+ omegaRadPerSec.getAsDouble()
+ )
+ )
+ );
+
+ driveCommand.setName("swerve drive command");
+ driveCommand.addRequirements(requirements);
+ return driveCommand;
+ }
+
+ /**
+ * A non-command function that drives the robot (for pathplanner).
+ *
+ * @param speeds A ChassisSpeeds object represents ROBOT RELATIVE desired speeds.
+ */
+ public void driveRobotRelativeChassisSpeeds(ChassisSpeeds speeds) {
+ SwerveModuleState[] desiredStates = m_modules.getSwerveDriveKinematics().toSwerveModuleStates(speeds);
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, m_specs.maxVelocity());
+ m_modules.setModulesStates(desiredStates);
+ }
+
+ /**
+ * Creates a Command that stops the swerve
+ *
+ * @return A command that stops the swerve
+ */
+ public Command stopCommand() {
+ return new InstantCommand(m_modules::stop);
+ }
+
+ /**
+ * @return A Command that sets the idle state of the swerve to coast.
+ */
+ public Command coastCommand() {
+ return m_modules.coastCommand().ignoringDisable(true).withName("Coast Command");
+ }
+
+ /**
+ * @return A command that resets the IMU angle.
+ */
+ public Command resetAngleCommand() {
+ return new InstantCommand(m_imu::resetIMU).ignoringDisable(true);
+ }
+
+ /**
+ * Updates the robot's odometry.
+ *
+ * @param visionMeasurements Optionally, you can add a vision measurement to the odometry.
+ */
+ public void updateOdometry(VisionMeasurement... visionMeasurements) {
+ m_odometry.updateOdometry(m_modules.getModulesPositions());
+
+ for (VisionMeasurement measurement : visionMeasurements) {
+ m_odometry.addVisionMeasurement(measurement.estimatedRobotPose(), measurement.timestampSeconds());
+ }
+ }
+
+ /**
+ * A method that resets the odometry.
+ *
+ * @param newPose the wanted new Pose2d of the robot.
+ */
+ public void resetOdometry(Pose2d newPose) {
+ m_odometry.resetOdometry(m_modules.getModulesPositions(), newPose);
+ }
+
+ /**
+ * A method that adjusts the vector if driving field oriented
+ *
+ * @param velocity current velocity vector
+ * @param fieldOriented is driving field oriented
+ * @return the fixed vector
+ */
+ private Vector2D adjustVectorByOrientation(Vector2D velocity, BooleanSupplier fieldOriented) {
+ Rotation2d yaw;
+ if (fieldOriented.getAsBoolean()) {
+ yaw = getRotation2D().unaryMinus();
+ if (AllianceUtils.isRedAlliance()) yaw = yaw.plus(kPi);
+ return velocity.rotate(yaw);
+ }
+ return velocity;
+ }
+
+ /**
+ * Gets the robot's heading.
+ *
+ * @return The current heading of the robot.
+ */
+ @Logged(name = "robot yaw angle")
+ public Rotation2d getRotation2D() {
+ return getPose2D().getRotation();
+ }
+
+ /**
+ * Gets the robot's pose.
+ *
+ * @return The current pose of the robot.
+ */
+ @Logged(name = "robot pose")
+ public Pose2d getPose2D() {
+ return m_odometry.getRobotPose();
+ }
+
+ /**
+ * Gets the current sigma velocity of the robot.
+ *
+ * @return The robot's sigma velocity as a Vector2D.
+ */
+ @Logged(name = "robot sigma velocity")
+ public Vector2D getSigmaRobotVelocity() {
+ return m_modules.getSigmaVelocity();
+ }
+
+ /**
+ * Gets the current linear velocity of the robot.
+ *
+ * @return The robot's linear velocity.
+ */
+ @Logged(name = "robot linear velocity")
+ public Vector2D getLinearRobotVelocity() {
+ return m_modules.getLinearVelocity();
+ }
+
+ /**
+ * Gets the current angular velocity of the robot.
+ *
+ * @return The robot's angular velocity.
+ */
+ @Logged(name = "robot angular velocity")
+ public double getOmegaRadPerSec() {
+ return m_modules.getOmegaRadPerSec();
+ }
+
+ /**
+ * Gets the current acceleration of the robot.
+ *
+ * @return The robot's acceleration as a Vector2D.
+ */
+ @Logged(name = "robot acceleration")
+ public Vector2D getRobotAcceleration() {
+ return new Vector2D(m_imu.getAccX(), m_imu.getAccY());
+ }
+
+ /**
+ * Gets the current robot relative speed.
+ *
+ * @return The robot's speed as a ChassisSpeeds.
+ */
+ @Logged(name = "measured chassis speeds")
+ public ChassisSpeeds getRobotChassisSpeeds() {
+ return m_modules.getSwerveDriveKinematics().toChassisSpeeds(m_modules.getStates());
+ }
+
+ /**
+ * Gets the desired robot relative speed.
+ *
+ * @return The robot's desired speed as a ChassisSpeeds.
+ */
+ @Logged(name = "desired chassis speeds")
+ public ChassisSpeeds getDesiredChassisSpeeds() {
+ return m_desiredChassisSpeeds;
+ }
+
+ /**
+ * Gets the current modules states.
+ *
+ * @return The robot's modules states.
+ */
+ @Logged(name = "modules states")
+ public SwerveModuleState[] getModulesStates() {
+ return m_modules.getStates();
+ }
+
+ /**
+ * Gets the desired modules states.
+ *
+ * @return The robot's desired modules states.
+ */
+ @Logged(name = "modules desired states")
+ public SwerveModuleState[] getDesiredModulesStates() {
+ return m_modules.getDesiredStates();
+ }
+
+ /**
+ * A function that initialize the Swerve data for the dashboard.
+ */
+ public void initModulesDashboard() {
+ SmartDashboard.putData("Swerve Drive", builder -> {
+ builder.setSmartDashboardType("SwerveDrive");
+
+ builder.addDoubleProperty("Front Left Angle", () -> m_modules.m_frontLeft.getPosition().getRadians(), null);
+ builder.addDoubleProperty("Front Left Velocity", () -> m_modules.m_frontLeft.getVelocity().getDistance(), null);
+
+ builder.addDoubleProperty("Front Right Angle", () -> m_modules.m_frontRight.getPosition().getRadians(), null);
+ builder.addDoubleProperty("Front Right Velocity", () -> m_modules.m_frontLeft.getVelocity().getDistance(), null);
+
+ builder.addDoubleProperty("Back Left Angle", () -> m_modules.m_backLeft.getPosition().getRadians(), null);
+ builder.addDoubleProperty("Back Left Velocity", () -> m_modules.m_frontLeft.getVelocity().getDistance(), null);
+
+ builder.addDoubleProperty("Back Right Angle", () -> m_modules.m_backRight.getPosition().getRadians(), null);
+ builder.addDoubleProperty("Back Right Velocity", () -> m_modules.m_frontLeft.getVelocity().getDistance(), null);
+
+ builder.addDoubleProperty("Robot Angle", () -> getRotation2D().getRadians(), null);
+ });
+ }
+
+ /**
+ * Runs a System Identification routine for a swerve module
+ *
+ * @param module swerve module
+ * @param direction of the module (forward or reverse)
+ * @param swerveSubsystem the object of the swerve mechanism
+ * @param sysidConfig routine of the sysid (e.g. step voltage)
+ * @param option either angle or drive routine (different motors!)
+ * @param dynamic is dynamically accelerating or not
+ * @return a command of the full routine
+ */
+ public Command moduleSysidRoutineCommand(SwerveModuleOption module, Direction direction, SubsystemBase swerveSubsystem,
+ SysidConfig sysidConfig, SysIdRoutineOption option, boolean dynamic) {
+ SwerveModule selectedModule;
+
+ switch (module) {
+ case FRONT_LEFT -> selectedModule = m_modules.m_frontLeft;
+ case FRONT_RIGHT -> selectedModule = m_modules.m_frontRight;
+ case BACK_LEFT -> selectedModule = m_modules.m_backLeft;
+ case BACK_RIGHT -> selectedModule = m_modules.m_backRight;
+ default -> throw new IllegalArgumentException("Invalid module parameter: " + module);
+ }
+
+ if (option.equals(SysIdRoutineOption.ANGLE)) {
+ if (dynamic)
+ return selectedModule.driveSysIdDynamic(direction, swerveSubsystem, sysidConfig);
+ return selectedModule.driveSysIdQuas(direction, swerveSubsystem, sysidConfig);
+ } else {
+ if (dynamic)
+ return selectedModule.angleSysIdDynamic(direction, swerveSubsystem, sysidConfig);
+ return selectedModule.angleSysIdQuas(direction, swerveSubsystem, sysidConfig);
+ }
+ }
+
+ /**
+ * A method that updates the modules positions of all modules. should be called periodically.
+ */
+ public void periodic() {
+ m_modules.periodic();
+ }
+}
diff --git a/src/main/java/frc/excalib/swerve/SwerveModule.java b/src/main/java/frc/excalib/swerve/SwerveModule.java
index e21768d..eab93c7 100644
--- a/src/main/java/frc/excalib/swerve/SwerveModule.java
+++ b/src/main/java/frc/excalib/swerve/SwerveModule.java
@@ -1,5 +1,7 @@
package frc.excalib.swerve;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.NotLogged;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -7,225 +9,323 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.excalib.control.gains.Gains;
import frc.excalib.control.gains.SysidConfig;
-import frc.excalib.control.limits.ContinuousSoftLimit;
import frc.excalib.control.math.Vector2D;
-import frc.excalib.control.motor.controllers.Motor;
import frc.excalib.mechanisms.fly_wheel.FlyWheel;
import frc.excalib.mechanisms.turret.Turret;
-import monologue.Logged;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
-import static frc.excalib.control.motor.motor_specs.DirectionState.FORWARD;
-import static frc.excalib.control.motor.motor_specs.DirectionState.REVERSE;
-import static frc.excalib.control.motor.motor_specs.IdleState.BRAKE;
+import static edu.wpi.first.math.geometry.Rotation2d.kPi;
/**
- * A class representing a swerve module
+ * A class representing a swerve module.
*
- * @author Yoav Cohen & Itay Keller
+ * @author Yoav Cohen & Itay Keller.
*/
-public class SwerveModule implements Logged {
- private final FlyWheel m_driveWheel;
- private final Turret m_turret;
- public final Translation2d m_MODULE_LOCATION;
- private final double m_MAX_VEL;
- private final Rotation2d m_moduleAnglePlus90;
- private final Vector2D m_setPoint = new Vector2D(0, 0);
+@Logged
+public class SwerveModule {
+ private final FlyWheel m_drivingMechanism;
+ private final Turret m_steeringMechanism;
+
private final SwerveModulePosition m_swerveModulePosition;
- /**
- * A constructor for the SwerveModule
- */
- public SwerveModule(Motor driveMotor, Motor rotationMotor, Gains angleGains, Gains velocityGains,
- double PIDTolerance, Translation2d moduleLocation, DoubleSupplier angleSupplier,
- double maxVel, double velocityConversionFactor, double positionConversionFactor,
- double rotationVelocityConversionFactor) {
- driveMotor.setInverted(FORWARD);
- driveMotor.setVelocityConversionFactor(velocityConversionFactor);
- driveMotor.setIdleState(BRAKE);
- driveMotor.setPositionConversionFactor(positionConversionFactor);
- driveMotor.setCurrentLimit(0, 60);
+ @NotLogged
+ final Translation2d m_moduleLocation;
+ @NotLogged
+ private final Rotation2d m_perpendicularModuleAngle;
+ @NotLogged
+ private final double kMaxVel;
- rotationMotor.setIdleState(BRAKE);
- rotationMotor.setVelocityConversionFactor(rotationVelocityConversionFactor);
- rotationMotor.setInverted(REVERSE);
+ @NotLogged
+ private final double kVelocityMinTolerance = 0.05;
- m_driveWheel = new FlyWheel(driveMotor, 10, 10, velocityGains);
+ private final Vector2D m_setpoint = new Vector2D(0, 0);
- m_turret = new Turret(rotationMotor, new ContinuousSoftLimit(() -> Double.NEGATIVE_INFINITY, () -> Double.POSITIVE_INFINITY),
- angleGains, PIDTolerance, angleSupplier);
+ /**
+ * A constructor for the SwerveModule.
+ *
+ * @param drivingMechanism the drive wheel presented as FlyWheel.
+ * @param steeringMechanism the steering mechanism presented as Turret.
+ * @param moduleLocation the location of the module relative to the center of the robot.
+ * @param maxVel the max velocity of the module.
+ */
+ public SwerveModule(FlyWheel drivingMechanism, Turret steeringMechanism, Translation2d moduleLocation, double maxVel) {
+ m_drivingMechanism = drivingMechanism;
+ m_steeringMechanism = steeringMechanism;
- m_MODULE_LOCATION = moduleLocation;
- m_MAX_VEL = maxVel;
+ m_moduleLocation = moduleLocation;
+ m_perpendicularModuleAngle = m_moduleLocation.getAngle().plus(Rotation2d.kCCW_Pi_2);
- // Precompute the rotated module angle (module angle + 90 degrees)
- m_moduleAnglePlus90 = m_MODULE_LOCATION.getAngle().plus(new Rotation2d(Math.PI / 2));
+ m_swerveModulePosition = new SwerveModulePosition(
+ m_drivingMechanism.logPosition(),
+ m_steeringMechanism.getPosition()
+ );
- m_swerveModulePosition = new SwerveModulePosition(m_driveWheel.logPosition(), m_turret.getPosition());
+ kMaxVel = maxVel;
}
+ /**
+ * A method to get the ratio limit of the module.
+ *
+ * @param translationVelocity the wanted linear velocity for the robot.
+ * @param omegaRadPerSec the wanted angular velocity for the robot.
+ * @return the ratio limit of this module.
+ */
double getVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) {
- Vector2D rotationVector = new Vector2D(
- omegaRadPerSec,
- m_moduleAnglePlus90
- );
+ Vector2D rotationVector = new Vector2D(omegaRadPerSec, m_perpendicularModuleAngle);
Vector2D sigmaVel = translationVelocity.plus(rotationVector);
double sigmaVelDistance = sigmaVel.getDistance();
- // Avoid division by zero
- if (sigmaVelDistance == 0) {
+ if (sigmaVelDistance == 0)
return 0;
- }
- return m_MAX_VEL / sigmaVelDistance;
+ return kMaxVel / sigmaVelDistance;
}
+ /**
+ * A method to get the sigma velocity for the module.
+ *
+ * @param translationVelocity the wanted linear velocity for the robot.
+ * @param omegaRadPerSec the wanted angular velocity for the robot.
+ * @param velocityRatioLimit the velocity ratio limit for this module.
+ * @return a Vector2D represents the sigma velocity.
+ */
Vector2D getSigmaVelocity(Vector2D translationVelocity, double omegaRadPerSec, double velocityRatioLimit) {
- Vector2D rotationVector = new Vector2D(
- omegaRadPerSec,
- m_moduleAnglePlus90
- );
+ Vector2D rotationVector = new Vector2D(omegaRadPerSec, m_perpendicularModuleAngle);
Vector2D sigmaVel = translationVelocity.plus(rotationVector);
sigmaVel = sigmaVel.mul(velocityRatioLimit);
return sigmaVel;
}
- public boolean isOptimizable(Vector2D moduleVelocitySetPoint) {
- Rotation2d setPointDirection = moduleVelocitySetPoint.getDirection();
- Rotation2d currentDirection = m_turret.getPosition();
- double deltaDirection = Math.cos(setPointDirection.minus(currentDirection).getRadians());
+ /**
+ * A method to check if the module should be optimized.
+ *
+ * @param moduleVelocitySetPoint the wanted velocity setpoint for this module.
+ * @return a boolean says if the module should be optimized.
+ */
+ boolean isOptimizable(Vector2D moduleVelocitySetPoint) {
+ Rotation2d setpointDirection = moduleVelocitySetPoint.getDirection();
+ Rotation2d currentDirection = m_steeringMechanism.getPosition();
+ double deltaDirection = Math.cos(setpointDirection.minus(currentDirection).getRadians());
// If the dot product is negative, reversing the wheel direction may be beneficial
return deltaDirection < 0;
}
- public Command setVelocityCommand(Supplier moduleVelocity) {
+
+ /**
+ * A method that sets the velocity of the module to the wanted velocity.
+ *
+ * @param moduleVelocity a supplier of Vector2D represents the wanted velocity.
+ * @return a Command that sets the module velocity to the wanted velocity.
+ */
+ Command setVelocityCommand(Supplier moduleVelocity) {
return new ParallelCommandGroup(
- m_driveWheel.setDynamicVelocityCommand(() -> {
- Vector2D velocity = moduleVelocity.get();
- double speed = velocity.getDistance();
+ applyDriveSpeedCommand(moduleVelocity),
+ applySteeringAngleCommand(moduleVelocity),
+ updateSetpointCommand(moduleVelocity)
+ );
+ }
- if (speed < 0.1) {
- speed = 0;
- }
+ private Command updateSetpointCommand(Supplier moduleVelocity) {
+ return new RunCommand(
+ () -> {
+ m_setpoint.setY(moduleVelocity.get().getY());
+ m_setpoint.setX(moduleVelocity.get().getX());
+ }
+ );
+ }
- boolean optimize = isOptimizable(velocity);
- return optimize ? -speed : speed;
- }),
- m_turret.setPositionCommand(() -> {
+ private Command applySteeringAngleCommand(Supplier moduleVelocity) {
+ return m_steeringMechanism.setPositionCommand(() -> {
+ Vector2D velocity = moduleVelocity.get();
+ double speed = velocity.getDistance();
+
+ if (speed < kVelocityMinTolerance) {
+ return m_steeringMechanism.getPosition();
+ }
+
+ boolean optimize = isOptimizable(velocity);
+ Rotation2d direction = velocity.getDirection();
+ return optimize ? direction.rotateBy(kPi) : direction;
+ });
+ }
+
+ private Command applyDriveSpeedCommand(Supplier moduleVelocity) {
+ return m_drivingMechanism.setDynamicVelocityCommand(
+ () -> {
Vector2D velocity = moduleVelocity.get();
double speed = velocity.getDistance();
- if (speed < 0.1) {
- return m_turret.getPosition();
+ if (speed < kVelocityMinTolerance) {
+ speed = 0;
}
boolean optimize = isOptimizable(velocity);
- Rotation2d direction = velocity.getDirection();
- return optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction;
- }),
- new RunCommand(() -> {
- m_setPoint.setY(moduleVelocity.get().getY());
- m_setPoint.setX(moduleVelocity.get().getX());
- })
+ return optimize ? -speed : speed;
+ }
);
}
- public Command setVelocityCommand(
- Supplier translationVelocity,
- DoubleSupplier omegaRadPerSec,
- DoubleSupplier velocityRatioLimit) {
-
- return setVelocityCommand(() -> getSigmaVelocity(
- translationVelocity.get(),
- omegaRadPerSec.getAsDouble(),
- velocityRatioLimit.getAsDouble()));
- }
-
- public Command coastCommand() {
- return new ParallelCommandGroup(
- m_driveWheel.coastCommand(),
- m_turret.coastCommand()
+ /**
+ * A method that sets the velocity of the module using the overall robot wanted velocity.
+ *
+ * @param translationVelocity the wanted robot linear velocity.
+ * @param omegaRadPerSec the wanted robot angular velocity.
+ * @param velocityRatioLimit the needed ratio limit.
+ * @return a Command that sets the module velocity to the wanted velocity.
+ */
+ Command setVelocityCommand(Supplier translationVelocity, DoubleSupplier omegaRadPerSec, DoubleSupplier velocityRatioLimit) {
+ return setVelocityCommand(
+ () -> getSigmaVelocity(
+ translationVelocity.get(), omegaRadPerSec.getAsDouble(), velocityRatioLimit.getAsDouble()
+ )
);
}
+ /**
+ * A method that sets the state of the module to the desired state.
+ *
+ * @param wantedState the wanted module state.
+ */
public void setDesiredState(SwerveModuleState wantedState) {
Vector2D velocity = new Vector2D(wantedState.speedMetersPerSecond, wantedState.angle);
double speed = velocity.getDistance();
Rotation2d direction = velocity.getDirection();
- if (speed < 0.1) {
+ if (speed < kVelocityMinTolerance) {
speed = 0.0;
- direction = m_turret.getPosition();
+ direction = m_steeringMechanism.getPosition();
}
boolean optimize = isOptimizable(velocity);
- m_driveWheel.setDynamicVelocity(optimize ? -speed : speed);
- m_turret.setPosition(optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction);
+ m_drivingMechanism.setDynamicVelocity(optimize ? -speed : speed);
+ m_steeringMechanism.setPosition(optimize ? direction.rotateBy(kPi) : direction);
}
/**
- * Stops the module by setting the drive wheel output to zero.
+ * A method that stops the module by setting the drive wheel output to zero.
*/
- void stopModule() {
- m_driveWheel.setOutput(0);
+ public void stopModule() {
+ m_drivingMechanism.setOutput(0);
}
/**
- * Gets the module's velocity.
+ * @return A Command that sets the idle state of the module's motors to coast.
+ */
+ public Command coastCommand() {
+ return new ParallelCommandGroup(m_drivingMechanism.coastCommand(), m_steeringMechanism.coastCommand());
+ }
+
+ /**
+ * A method to get the module's velocity.
*
- * @return a Vector2D representing the velocity.
+ * @return a Vector2D represents the module velocity.
*/
- Vector2D getVelocity() {
- return new Vector2D(m_driveWheel.getVelocity(), getPosition());
+ @Logged(name = "module velocity")
+ public Vector2D getVelocity() {
+ return new Vector2D(m_drivingMechanism.getVelocity(), getPosition());
}
/**
- * Gets the turret's position.
+ * A method to get the angle of the module.
*
- * @return the current position of the turret.
+ * @return the angle of the module as Rotation2d.
*/
- Rotation2d getPosition() {
- return m_turret.getPosition();
+ @Logged(name = "module angle")
+ public Rotation2d getPosition() {
+ return m_steeringMechanism.getPosition();
}
+ /**
+ * A method to get the position of the module.
+ *
+ * @return the module position.
+ */
public SwerveModulePosition getModulePosition() {
return m_swerveModulePosition;
}
- public SwerveModuleState logState() {
+ /**
+ * A method to get the current state of the module.
+ *
+ * @return the current state of the module.
+ */
+ @Logged(name = "module state")
+ public SwerveModuleState getState() {
Vector2D velocity = getVelocity();
return new SwerveModuleState(velocity.getDistance(), velocity.getDirection());
}
- public SwerveModuleState logSetpointState() {
- return new SwerveModuleState(m_setPoint.getDistance(), m_setPoint.getDirection());
+ /**
+ * A method to get the wanted state of the module.
+ *
+ * @return the wanted state of the module.
+ */
+ @Logged(name = "module desired state")
+ public SwerveModuleState getDesiredState() {
+ return new SwerveModuleState(m_setpoint.getDistance(), m_setpoint.getDirection());
}
-
- public Command driveSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) {
- return m_driveWheel.sysIdDynamic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false);
+ /**
+ * A command for dynamic sysId to the driving mechanism of the module
+ *
+ * @param direction the wanted direction of the driving mechanism
+ * @param swerveSubsystem the swerve subsystem (for requirements)
+ * @param sysidConfig the configuration for the sysId
+ * @return the dynamic sysId command
+ */
+ public Command driveSysIdDynamic(SysIdRoutine.Direction direction, SubsystemBase swerveSubsystem, SysidConfig sysidConfig) {
+ return m_drivingMechanism.sysIdDynamic(direction, swerveSubsystem, m_drivingMechanism::logPosition, sysidConfig, true);
}
- public Command driveSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) {
- return m_driveWheel.sysIdQuasistatic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false);
+ /**
+ * A command for quasistatic sysId to the driving mechanism of the module
+ *
+ * @param direction the wanted direction of the driving mechanism
+ * @param swerve the swerve subsystem (for requirements)
+ * @param sysidConfig the configuration for the sysId
+ * @return the quasistatic sysId command
+ */
+ public Command driveSysIdQuas(SysIdRoutine.Direction direction, SubsystemBase swerve, SysidConfig sysidConfig) {
+ return m_drivingMechanism.sysIdQuasistatic(direction, swerve, m_drivingMechanism::logPosition, sysidConfig, true);
}
- public Command angleSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) {
- return m_turret.sysIdDynamic(direction, swerve, m_turret::logPosition, sysidConfig, false);
+ /**
+ * A command for dynamic sysId to the steering mechanism of the module
+ *
+ * @param direction the wanted direction of the steering mechanism
+ * @param swerve the swerve subsystem (for requirements)
+ * @param sysidConfig the configuration for the sysId
+ * @return the dynamic sysId command
+ */
+ public Command angleSysIdDynamic(SysIdRoutine.Direction direction, SubsystemBase swerve, SysidConfig sysidConfig) {
+ return m_steeringMechanism.sysIdDynamic(direction, swerve, m_steeringMechanism::logPosition, sysidConfig, false);
}
- public Command angleSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) {
- return m_turret.sysIdQuasistatic(direction, swerve, m_turret::logPosition, sysidConfig, false);
+ /**
+ * A command for quasistatic sysId to the steering mechanism of the module
+ *
+ * @param direction the wanted direction of the steering mechanism
+ * @param swerve the swerve subsystem (for requirements)
+ * @param sysidConfig the configuration for the sysId
+ * @return the quasistatic sysId command
+ */
+ public Command angleSysIdQuas(SysIdRoutine.Direction direction, SubsystemBase swerve, SysidConfig sysidConfig) {
+ return m_steeringMechanism.sysIdQuasistatic(direction, swerve, m_steeringMechanism::logPosition, sysidConfig, false);
}
+ /**
+ * A method that updates the module position. should be called periodically.
+ */
public void periodic() {
- m_swerveModulePosition.distanceMeters = m_driveWheel.logPosition();
- m_swerveModulePosition.angle = m_turret.getPosition();
+ m_drivingMechanism.periodic();
+
+ m_swerveModulePosition.distanceMeters = m_drivingMechanism.logPosition();
+ m_swerveModulePosition.angle = m_steeringMechanism.getPosition();
}
}
diff --git a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveAccUtils.java
similarity index 55%
rename from src/main/java/frc/excalib/swerve/SwerveAccUtils.java
rename to src/main/java/frc/excalib/swerve/swerve_utils/SwerveAccUtils.java
index a3c2e5a..88ec616 100644
--- a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveAccUtils.java
@@ -1,37 +1,55 @@
-package frc.excalib.swerve;
+package frc.excalib.swerve.swerve_utils;
import frc.excalib.control.math.Vector2D;
import static frc.excalib.control.math.MathUtils.minSize;
-import static frc.robot.SwerveConstants.*;
+/**
+ * A util class that applies acceleration limits on the swerve.
+ */
public class SwerveAccUtils {
private static final double CYCLE_TIME = 0.02;
+ private static SwerveSpecs kSpecs;
/**
- * A function to get the translational velocity setpoint
+ * A function that sets the swerve specs.
*
- * @param velocitySetPoint wanted velocity setpoint
- * @return translational velocity setpoint
+ * @param specs The swerve specs
*/
- public static Vector2D getSmartTranslationalVelocitySetPoint(Vector2D currentVel, Vector2D velocitySetPoint) {
- Vector2D deltaVelocity = velocitySetPoint.plus(
+ public static void setSwerveSpecs(SwerveSpecs specs) {
+ kSpecs = specs;
+ }
+
+ /**
+ * A function to get the translational velocity setpoint.
+ *
+ * @param currentVel the current velocity of the swerve
+ * @param velocitySetpoint wanted velocity setpoint
+ * @param allLimits whether to use all limits or only the skid limit
+ * @return translational velocity setpoint with acceleration limits
+ */
+ public static Vector2D getSmartTranslationalVelocitySetpoint(Vector2D currentVel, Vector2D velocitySetpoint, boolean allLimits) {
+ Vector2D deltaVelocity = velocitySetpoint.plus(
currentVel.mul(-1));
- Vector2D actualDeltaVelocity = applyAccelerationLimits(currentVel, deltaVelocity);
+ Vector2D actualDeltaVelocity = applyAccelerationLimits(currentVel, deltaVelocity, allLimits);
return currentVel.plus(actualDeltaVelocity);
}
/**
* A function to apply the acceleration limits
*
+ * @param currentVel the current velocity of the swerve
* @param velocityError wanted velocity
+ * @param allLimits whether to use all limits or only the skid limit
* @return velocity limited by acceleration limits
*/
- private static Vector2D applyAccelerationLimits(Vector2D currentVel, Vector2D velocityError) {
+ private static Vector2D applyAccelerationLimits(Vector2D currentVel, Vector2D velocityError, boolean allLimits) {
Vector2D wantedAcceleration = velocityError.mul(1 / CYCLE_TIME);
-// wantedAcceleration = applyForwardLimit(currentVel, wantedAcceleration);
-// wantedAcceleration = applyTiltLimit(wantedAcceleration);
+ if (allLimits) {
+ wantedAcceleration = applyForwardLimit(currentVel, wantedAcceleration);
+ wantedAcceleration = applyTiltLimit(wantedAcceleration);
+ }
wantedAcceleration = applySkidLimit(wantedAcceleration);
return wantedAcceleration.mul(CYCLE_TIME);
@@ -40,13 +58,15 @@ private static Vector2D applyAccelerationLimits(Vector2D currentVel, Vector2D ve
/**
* A function to apply the forward acceleration limit
*
+ * @param currentVel the current velocity of the swerve
* @param wantedAcceleration the wanted acceleration
* @return wanted acceleration limited by forward acceleration limit
*/
private static Vector2D applyForwardLimit(Vector2D currentVel, Vector2D wantedAcceleration) {
+ // TODO: check & debug
double maxAcceleration =
- MAX_FORWARD_ACC *
- (1 - (currentVel.getDistance() / MAX_VEL));
+ kSpecs.maxForwardAcc() *
+ (1 - (currentVel.getDistance() / kSpecs.maxVelocity()));
wantedAcceleration = wantedAcceleration.limit(new Vector2D(maxAcceleration, currentVel.getDirection()));
return wantedAcceleration;
@@ -59,8 +79,9 @@ private static Vector2D applyForwardLimit(Vector2D currentVel, Vector2D wantedAc
* @return wanted acceleration limited by tilt acceleration limit
*/
private static Vector2D applyTiltLimit(Vector2D wantedAcceleration) {
- double frontAcceleration = minSize(wantedAcceleration.getX(), MAX_FRONT_ACC);
- double sideAcceleration = minSize(wantedAcceleration.getY(), MAX_SIDE_ACC);
+ // TODO: check & debug
+ double frontAcceleration = minSize(wantedAcceleration.getX(), kSpecs.maxFrontAcc().getAsDouble());
+ double sideAcceleration = minSize(wantedAcceleration.getY(), kSpecs.maxSideAcc().getAsDouble());
return new Vector2D(frontAcceleration, sideAcceleration);
}
@@ -72,7 +93,7 @@ private static Vector2D applyTiltLimit(Vector2D wantedAcceleration) {
*/
private static Vector2D applySkidLimit(Vector2D wantedAcceleration) {
return new Vector2D(
- Math.min(wantedAcceleration.getDistance(), MAX_SKID_ACC),
+ Math.min(wantedAcceleration.getDistance(), kSpecs.maxAcc()),
wantedAcceleration.getDirection()
);
}
diff --git a/src/main/java/frc/excalib/swerve/swerve_utils/SwerveConfigurationUtils.java b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveConfigurationUtils.java
new file mode 100644
index 0000000..4c68365
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveConfigurationUtils.java
@@ -0,0 +1,165 @@
+package frc.excalib.swerve.swerve_utils;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.excalib.control.gains.Gains;
+import frc.excalib.control.motor.controllers.Motor;
+import frc.excalib.control.motor.motor_specs.DirectionState;
+import frc.excalib.control.motor.motor_specs.IdleState;
+import frc.excalib.mechanisms.fly_wheel.FlyWheel;
+import frc.excalib.mechanisms.turret.Turret;
+import frc.excalib.swerve.SwerveModule;
+
+import java.util.function.DoubleSupplier;
+
+/**
+ * A class that contains functions that simplifies the swerve configuration process
+ */
+public class SwerveConfigurationUtils {
+
+ /**
+ * A method to configure the drive motors
+ *
+ * @param driveMotors an array represents the drive motors
+ * @param stallLimit the wanted stall limit for the motors
+ * @param freeLimit the wanted free limit for the motors
+ * @param directionState the wanted direction state for the motors
+ * @param idleState the wanted idle state of the motors
+ * @param moduleType the type of the module
+ */
+ private static void setupDriveMotors(Motor[] driveMotors, int stallLimit, int freeLimit,
+ DirectionState directionState, IdleState idleState,
+ SwerveModuleConfiguration moduleType) {
+ for (Motor driveMotor : driveMotors) {
+ driveMotor.setInverted(directionState);
+ driveMotor.setIdleState(idleState);
+ driveMotor.setPositionConversionFactor(moduleType.positionConversionFactor());
+ driveMotor.setVelocityConversionFactor(moduleType.velocityConversionFactor());
+ driveMotor.setCurrentLimit(stallLimit, freeLimit);
+ }
+ }
+
+ /**
+ * A method to configure the steering motors
+ *
+ * @param steeringMotors an array represents the steering motors
+ * @param stallLimit the wanted stall limit for the motors
+ * @param freeLimit the wanted free limit for the motors
+ * @param directionState the wanted direction state for the motors
+ * @param idleState the wanted idle state of the motors
+ * @param moduleType the type of the module
+ */
+ private static void setupSteeringMotors(Motor[] steeringMotors, int stallLimit, int freeLimit,
+ DirectionState directionState, IdleState idleState,
+ SwerveModuleConfiguration moduleType) {
+ for (Motor steeringMotor : steeringMotors) {
+ steeringMotor.setInverted(directionState);
+ steeringMotor.setIdleState(idleState);
+ steeringMotor.setVelocityConversionFactor(moduleType.steeringVelocityConversionFactor());
+ steeringMotor.setCurrentLimit(stallLimit, freeLimit);
+ }
+ }
+
+ /**
+ * A method to configure the fly wheels which represents the driving mechanism
+ *
+ * @param driveMotors an array represents the drive motors
+ * @param stallLimit the wanted stall limit for the motors
+ * @param freeLimit the wanted free limit for the motors
+ * @param directionState the wanted direction state for the motors
+ * @param idleState the wanted idle state of the motors
+ * @param moduleType the type of the module
+ * @param driveModuleGains the driving mechanisms gains
+ * @param maxModuleAcceleration the max acceleration of the module
+ * @param maxModuleJerk the max jerk of the module
+ *
+ * @return an array of fly wheels represents the driving mechanisms
+ */
+ public static FlyWheel[] setupDrivingMechanisms(Motor[] driveMotors, int stallLimit, int freeLimit,
+ DirectionState directionState, IdleState idleState,
+ SwerveModuleConfiguration moduleType, Gains driveModuleGains,
+ double maxModuleAcceleration, double maxModuleJerk) {
+ setupDriveMotors(driveMotors, stallLimit, freeLimit, directionState, idleState, moduleType);
+
+ FlyWheel[] drivingMechanisms = new FlyWheel[4];
+ drivingMechanisms[0] = new FlyWheel(driveMotors[0], maxModuleAcceleration, maxModuleJerk, driveModuleGains);
+
+ for (int i = 1; i < drivingMechanisms.length; i++) {
+ drivingMechanisms[i] = new FlyWheel(driveMotors[i], drivingMechanisms[0]);
+ }
+
+ return drivingMechanisms;
+ }
+
+ /**
+ * A method to configure the fly wheels which represents the driving mechanism
+ *
+ * @param steeringMotors an array represents the steering motors
+ * @param stallLimit the wanted stall limit for the motors
+ * @param freeLimit the wanted free limit for the motors
+ * @param directionState the wanted direction state for the motors
+ * @param idleState the wanted idle state of the motors
+ * @param moduleType the type of the module
+ * @param steeringModuleGains the steering mechanisms gains
+ * @param steeringModulesPositionSuppliers the supplier of the module position
+ * @param steeringPIDtolerance the tolerance for the PID of the steering mechanisms
+ *
+ * @return an array of turrets represents the steering mechanisms
+ */
+ public static Turret[] setupSteeringMechanisms(Motor[] steeringMotors, int stallLimit, int freeLimit,
+ DirectionState directionState, IdleState idleState,
+ SwerveModuleConfiguration moduleType, Gains steeringModuleGains,
+ DoubleSupplier[] steeringModulesPositionSuppliers, double steeringPIDtolerance) {
+ setupSteeringMotors(steeringMotors, stallLimit, freeLimit, directionState, idleState, moduleType);
+
+ Turret[] steeringMechanisms = new Turret[4];
+ steeringMechanisms[0] = new Turret(steeringMotors[0], steeringModuleGains, steeringPIDtolerance, steeringModulesPositionSuppliers[0]);
+
+ for (int i = 1; i < steeringMechanisms.length; i++) {
+ steeringMechanisms[i] = new Turret(steeringMotors[i], steeringModulesPositionSuppliers[i], steeringMechanisms[0]);
+ }
+
+ return steeringMechanisms;
+ }
+
+ /**
+ * A function that initializes the swerve modules
+ *
+ * @param driveWheels an array of fly wheels represents the driving mechanisms
+ * @param steeringMechanisms an array of turrets represents the steering mechanisms
+ * @param moduleLocations the locations of the modules relative to the center of the robot
+ * @param maxModuleVelocity the max velocity of the modules
+ *
+ * @return an array of SwerveModules
+ */
+ public static SwerveModule[] setupSwerveModules(FlyWheel[] driveWheels, Turret[] steeringMechanisms,
+ Translation2d[] moduleLocations, double maxModuleVelocity) {
+ SwerveModule[] swerveModules = new SwerveModule[4];
+
+ for (int i = 0; i < swerveModules.length; i++) {
+ swerveModules[i] = new SwerveModule(driveWheels[i], steeringMechanisms[i], moduleLocations[i], maxModuleVelocity);
+ }
+
+ return swerveModules;
+ }
+
+ /**
+ * A function that initializes the swerve modules
+ *
+ * @param driveWheels an array of fly wheels represents the driving mechanisms
+ * @param steeringMechanisms an array of turrets represents the steering mechanisms
+ * @param moduleLocations the locations of the modules relative to the center of the robot
+ * @param maxModulesVelocities an array of the max velocities of each module
+ *
+ * @return an array of SwerveModules
+ */
+ public static SwerveModule[] setupSwerveModules(FlyWheel[] driveWheels, Turret[] steeringMechanisms,
+ Translation2d[] moduleLocations, double[] maxModulesVelocities) {
+ SwerveModule[] swerveModules = new SwerveModule[4];
+
+ for (int i = 0; i < swerveModules.length; i++) {
+ swerveModules[i] = new SwerveModule(driveWheels[i], steeringMechanisms[i], moduleLocations[i], maxModulesVelocities[i]);
+ }
+
+ return swerveModules;
+ }
+}
diff --git a/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleConfiguration.java b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleConfiguration.java
new file mode 100644
index 0000000..b47cb18
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleConfiguration.java
@@ -0,0 +1,270 @@
+package frc.excalib.swerve.swerve_utils;
+
+import edu.wpi.first.math.util.Units;
+
+/**
+ * A record represents the module configuration
+ *
+ * @param velocityConversionFactor The velocity conversion factor of the module
+ * @param positionConversionFactor The position conversion factor of the module
+ * @param steeringVelocityConversionFactor The rotational velocity conversion factor of the module
+ */
+public record SwerveModuleConfiguration(double velocityConversionFactor,
+ double positionConversionFactor,
+ double steeringVelocityConversionFactor) {
+
+ /**
+ * an enum class contains swerve types that helps to initialize the swerve
+ */
+ public enum SwerveModuleType {
+ //-----SDS-----
+ // MK4
+ MK4_L1(Units.inchesToMeters(4) * Math.PI / 8.14,
+ Units.inchesToMeters(4) * Math.PI / 8.14,
+ (2 * Math.PI) / 12.8),
+ MK4_L2(Units.inchesToMeters(4) * Math.PI / 6.75,
+ Units.inchesToMeters(4) * Math.PI / 6.75,
+ (2 * Math.PI) / 12.8),
+ MK4_L3(Units.inchesToMeters(4) * Math.PI / 6.12,
+ Units.inchesToMeters(4) * Math.PI / 6.12,
+ (2 * Math.PI) / 12.8),
+ MK4_L4(Units.inchesToMeters(4) * Math.PI / 5.14,
+ Units.inchesToMeters(4) * Math.PI / 5.14,
+ (2 * Math.PI) / 12.8),
+
+ // MK4i,
+ MK4i_L1(Units.inchesToMeters(4) * Math.PI / 8.14,
+ Units.inchesToMeters(4) * Math.PI / 8.14,
+ (2 * Math.PI) / 21.4285714),
+ MK4i_L2(Units.inchesToMeters(4) * Math.PI / 6.75,
+ Units.inchesToMeters(4) * Math.PI / 6.75,
+ (2 * Math.PI) / 21.4285714),
+ MK4i_L3(Units.inchesToMeters(4) * Math.PI / 6.12,
+ Units.inchesToMeters(4) * Math.PI / 6.12,
+ (2 * Math.PI) / 21.4285714),
+ MK4i_L4(Units.inchesToMeters(4) * Math.PI / 5.14,
+ Units.inchesToMeters(4) * Math.PI / 5.14,
+ (2 * Math.PI) / 21.4285714),
+
+ //MK4c
+ MK4c_L1(Units.inchesToMeters(4) * Math.PI / 7.13,
+ Units.inchesToMeters(4) * Math.PI / 7.13,
+ (2 * Math.PI) / 12.8),
+ MK4c_L2(Units.inchesToMeters(4) * Math.PI / 5.9,
+ Units.inchesToMeters(4) * Math.PI / 5.9,
+ (2 * Math.PI) / 12.8),
+ MK4c_L3(Units.inchesToMeters(4) * Math.PI / 5.36,
+ Units.inchesToMeters(4) * Math.PI / 5.36,
+ (2 * Math.PI) / 12.8),
+
+ //MK4n
+ MK4n_L1(Units.inchesToMeters(4) * Math.PI / 7.13,
+ Units.inchesToMeters(4) * Math.PI / 7.13,
+ (2 * Math.PI) / 18.75),
+ MK4n_L2(Units.inchesToMeters(4) * Math.PI / 5.9,
+ Units.inchesToMeters(4) * Math.PI / 5.9,
+ (2 * Math.PI) / 18.75),
+ MK4n_L3(Units.inchesToMeters(4) * Math.PI / 5.36,
+ Units.inchesToMeters(4) * Math.PI / 5.36,
+ (2 * Math.PI) / 18.75),
+
+ //MK5n
+ MK5n_R1(Units.inchesToMeters(4) * Math.PI / 7.03,
+ Units.inchesToMeters(4) * Math.PI / 7.03,
+ (2 * Math.PI) / 26.0909090909),
+ MK5n_R2(Units.inchesToMeters(4) * Math.PI / 6.03,
+ Units.inchesToMeters(4) * Math.PI / 6.03,
+ (2 * Math.PI) / 26.0909090909),
+ MK5n_R3(Units.inchesToMeters(4) * Math.PI / 5.27,
+ Units.inchesToMeters(4) * Math.PI / 5.27,
+ (2 * Math.PI) / 26.0909090909),
+
+ //-----REV-----
+ //MAXSwerve
+ MAX_SWERVE_LOW(Units.inchesToMeters(3) * Math.PI / 5.50,
+ Units.inchesToMeters(3) * Math.PI / 5.50,
+ (2 * Math.PI) / (46.4236453202)),
+ MAX_SWERVE_MEDIUM(Units.inchesToMeters(3) * Math.PI / 5.08,
+ Units.inchesToMeters(3) * Math.PI / 5.08,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_HIGH(Units.inchesToMeters(3) * Math.PI / 5.08,
+ Units.inchesToMeters(3) * Math.PI / 5.08,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_EXTRA_HIGH_1(Units.inchesToMeters(3) * Math.PI / 4.50,
+ Units.inchesToMeters(3) * Math.PI / 4.50,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_EXTRA_HIGH_2(Units.inchesToMeters(3) * Math.PI / 4.29,
+ Units.inchesToMeters(3) * Math.PI / 4.29,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_EXTRA_HIGH_3(Units.inchesToMeters(3) * Math.PI / 4.0,
+ Units.inchesToMeters(3) * Math.PI / 4.0,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_EXTRA_HIGH_4(Units.inchesToMeters(3) * Math.PI / 3.75,
+ Units.inchesToMeters(3) * Math.PI / 3.75,
+ (2 * Math.PI) / 46.4236453202),
+ MAX_SWERVE_EXTRA_HIGH_5(Units.inchesToMeters(3) * Math.PI / 3.56,
+ Units.inchesToMeters(3) * Math.PI / 3.56,
+ (2 * Math.PI) / 46.4236453202),
+
+ //-----WCP-----
+ //Swerve X
+ SWERVE_X_X1_10T(Units.inchesToMeters(4) * Math.PI / 7.85,
+ Units.inchesToMeters(4) * Math.PI / 7.85,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X1_11T(Units.inchesToMeters(4) * Math.PI / 7.13,
+ Units.inchesToMeters(4) * Math.PI / 7.13,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X1_12T(Units.inchesToMeters(4) * Math.PI / 6.54,
+ Units.inchesToMeters(4) * Math.PI / 6.54,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X2_10T(Units.inchesToMeters(4) * Math.PI / 6.56,
+ Units.inchesToMeters(4) * Math.PI / 6.56,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X2_11T(Units.inchesToMeters(4) * Math.PI / 5.96,
+ Units.inchesToMeters(4) * Math.PI / 5.96,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X2_12T(Units.inchesToMeters(4) * Math.PI / 5.46,
+ Units.inchesToMeters(4) * Math.PI / 5.46,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X3_10T(Units.inchesToMeters(4) * Math.PI / 5.14,
+ Units.inchesToMeters(4) * Math.PI / 5.14,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X3_11T(Units.inchesToMeters(4) * Math.PI / 4.75,
+ Units.inchesToMeters(4) * Math.PI / 4.75,
+ (2 * Math.PI) / 11.3142857143),
+ SWERVE_X_X3_12T(Units.inchesToMeters(4) * Math.PI / 4.41,
+ Units.inchesToMeters(4) * Math.PI / 4.41,
+ (2 * Math.PI) / 11.3142857143),
+
+ //Swerve XS
+ SWERVE_XS_X1_12T(Units.inchesToMeters(3) * Math.PI / 6.0,
+ Units.inchesToMeters(3) * Math.PI / 6.0,
+ (2 * Math.PI) / 41.25),
+ SWERVE_XS_X1_13T(Units.inchesToMeters(3) * Math.PI / 5.54,
+ Units.inchesToMeters(3) * Math.PI / 5.54,
+ (2 * Math.PI) / 41.25),
+ SWERVE_XS_X1_14T(Units.inchesToMeters(3) * Math.PI / 5.14,
+ Units.inchesToMeters(3) * Math.PI / 5.14,
+ (2 * Math.PI) / 41.25),
+ SWERVE_XS_X2_13T(Units.inchesToMeters(3) * Math.PI / 4.71,
+ Units.inchesToMeters(3) * Math.PI / 4.71,
+ (2 * Math.PI) / 41.25),
+ SWERVE_XS_X2_15T(Units.inchesToMeters(3) * Math.PI / 4.40,
+ Units.inchesToMeters(3) * Math.PI / 4.40,
+ (2 * Math.PI) / 41.25),
+ SWERVE_XS_X2_16T(Units.inchesToMeters(3) * Math.PI / 4.13,
+ Units.inchesToMeters(3) * Math.PI / 4.13,
+ (2 * Math.PI) / 41.25),
+
+ //Swerve X Flipped
+ SWERVE_X_FLIPPED_X1_10T(Units.inchesToMeters(4) * Math.PI / 8.10,
+ Units.inchesToMeters(4) * Math.PI / 8.10,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X1_11T(Units.inchesToMeters(4) * Math.PI / 7.36,
+ Units.inchesToMeters(4) * Math.PI / 7.36,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X1_12T(Units.inchesToMeters(4) * Math.PI / 6.75,
+ Units.inchesToMeters(4) * Math.PI / 6.75,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X2_10T(Units.inchesToMeters(4) * Math.PI / 6.72,
+ Units.inchesToMeters(4) * Math.PI / 6.72,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X2_11T(Units.inchesToMeters(4) * Math.PI / 6.11,
+ Units.inchesToMeters(4) * Math.PI / 6.11,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X2_12T(Units.inchesToMeters(4) * Math.PI / 5.60,
+ Units.inchesToMeters(4) * Math.PI / 5.60,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X3_10T(Units.inchesToMeters(4) * Math.PI / 5.51,
+ Units.inchesToMeters(4) * Math.PI / 5.51,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X3_11T(Units.inchesToMeters(4) * Math.PI / 5.01,
+ Units.inchesToMeters(4) * Math.PI / 5.01,
+ (2 * Math.PI) / 13.3714285714),
+ SWERVE_X_FLIPPED_X3_12T(Units.inchesToMeters(4) * Math.PI / 4.59,
+ Units.inchesToMeters(4) * Math.PI / 4.59,
+ (2 * Math.PI) / 13.3714285714),
+
+ //Swerve X2, X2i, X2t
+ SWERVE_X2_X1_10T(Units.inchesToMeters(4) * Math.PI / 7.67,
+ Units.inchesToMeters(4) * Math.PI / 7.67,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X1_11T(Units.inchesToMeters(4) * Math.PI / 6.98,
+ Units.inchesToMeters(4) * Math.PI / 6.98,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X1_12T(Units.inchesToMeters(4) * Math.PI / 6.39,
+ Units.inchesToMeters(4) * Math.PI / 6.39,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X2_10T(Units.inchesToMeters(4) * Math.PI / 6.82,
+ Units.inchesToMeters(4) * Math.PI / 6.82,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X2_11T(Units.inchesToMeters(4) * Math.PI / 6.20,
+ Units.inchesToMeters(4) * Math.PI / 6.20,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X2_12T(Units.inchesToMeters(4) * Math.PI / 5.68,
+ Units.inchesToMeters(4) * Math.PI / 5.68,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X3_10T(Units.inchesToMeters(4) * Math.PI / 6.48,
+ Units.inchesToMeters(4) * Math.PI / 6.48,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X3_11T(Units.inchesToMeters(4) * Math.PI / 5.89,
+ Units.inchesToMeters(4) * Math.PI / 5.89,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X3_12T(Units.inchesToMeters(4) * Math.PI / 5.40,
+ Units.inchesToMeters(4) * Math.PI / 5.40,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X4_10T(Units.inchesToMeters(4) * Math.PI / 5.67,
+ Units.inchesToMeters(4) * Math.PI / 5.67,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X4_11T(Units.inchesToMeters(4) * Math.PI / 5.15,
+ Units.inchesToMeters(4) * Math.PI / 5.15,
+ (2 * Math.PI) / 12.1),
+ SWERVE_X2_X4_12T(Units.inchesToMeters(4) * Math.PI / 4.73,
+ Units.inchesToMeters(4) * Math.PI / 4.73,
+ (2 * Math.PI) / 12.1),
+
+ //Swerve X2S, X2St
+ SWERVE_X2S_X1_15T(Units.inchesToMeters(3.5) * Math.PI / 6.0,
+ Units.inchesToMeters(3.5) * Math.PI / 6.0,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X1_16T(Units.inchesToMeters(3.5) * Math.PI / 5.63,
+ Units.inchesToMeters(3.5) * Math.PI / 5.63,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X1_17T(Units.inchesToMeters(3.5) * Math.PI / 5.29,
+ Units.inchesToMeters(3.5) * Math.PI / 5.29,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X2_17T(Units.inchesToMeters(3.5) * Math.PI / 4.94,
+ Units.inchesToMeters(3.5) * Math.PI / 4.94,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X2_18T(Units.inchesToMeters(3.5) * Math.PI / 4.67,
+ Units.inchesToMeters(3.5) * Math.PI / 4.67,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X2_19T(Units.inchesToMeters(3.5) * Math.PI / 4.42,
+ Units.inchesToMeters(3.5) * Math.PI / 4.42,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X3_19T(Units.inchesToMeters(3.5) * Math.PI / 4.11,
+ Units.inchesToMeters(3.5) * Math.PI / 4.11,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X3_20T(Units.inchesToMeters(3.5) * Math.PI / 3.90,
+ Units.inchesToMeters(3.5) * Math.PI / 3.90,
+ (2 * Math.PI) / 25.9),
+ SWERVE_X2S_X3_21T(Units.inchesToMeters(3.5) * Math.PI / 3.71,
+ Units.inchesToMeters(3.5) * Math.PI / 3.71,
+ (2 * Math.PI) / 25.9);
+
+ public final SwerveModuleConfiguration m_moduleConfiguration;
+
+ /**
+ * @param velocityConversionFactor The velocity conversion factor of the module
+ * @param positionConversionFactor The position conversion factor of the module
+ * @param steeringVelocityConversionFactor The rotational velocity conversion factor of the module
+ */
+ SwerveModuleType(double velocityConversionFactor, double positionConversionFactor, double steeringVelocityConversionFactor) {
+ m_moduleConfiguration = new SwerveModuleConfiguration(velocityConversionFactor, positionConversionFactor, steeringVelocityConversionFactor);
+ }
+
+ public SwerveModuleConfiguration getModuleConfiguration() {
+ return m_moduleConfiguration;
+ }
+ }
+}
diff --git a/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleOption.java b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleOption.java
new file mode 100644
index 0000000..a7e8b2c
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveModuleOption.java
@@ -0,0 +1,8 @@
+package frc.excalib.swerve.swerve_utils;
+
+public enum SwerveModuleOption {
+ FRONT_LEFT,
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT
+}
diff --git a/src/main/java/frc/excalib/swerve/swerve_utils/SwerveSpecs.java b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveSpecs.java
new file mode 100644
index 0000000..3b6db75
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SwerveSpecs.java
@@ -0,0 +1,42 @@
+package frc.excalib.swerve.swerve_utils;
+
+import java.util.function.DoubleSupplier;
+
+/**
+ * A record that includes the swerve specifications
+ *
+ * @param maxVelocity the max linear velocity of the swerve
+ * @param maxOmegaRadPerSec the max angular velocity of the swerve
+ * @param maxAcc the max acceleration of the swerve
+ * @param maxFrontAcc the max front acceleration of the swerve as a DoubleSupplier
+ * @param maxSideAcc the max side acceleration of the swerve as a DoubleSupplier
+ * @param maxForwardAcc the max forward acceleration of the swerve
+ */
+public record SwerveSpecs(double maxVelocity, double maxOmegaRadPerSec, double maxAcc,
+ DoubleSupplier maxFrontAcc, DoubleSupplier maxSideAcc, double maxForwardAcc) {
+ /**
+ * A constructor for robots without changing height (ex: without elevator etc.)
+ *
+ * @param maxVelocity the max linear velocity of the swerve
+ * @param maxOmegaRadPerSec the max angular velocity of the swerve
+ * @param maxAcc the max acceleration of the swerve
+ * @param maxFrontAcc the max front acceleration of the swerve
+ * @param maxSideAcc the max side acceleration of the swerve
+ * @param maxForwardAcc the max forward acceleration of the swerve
+ */
+ public SwerveSpecs(double maxVelocity, double maxOmegaRadPerSec, double maxAcc,
+ double maxFrontAcc, double maxSideAcc, double maxForwardAcc) {
+ this(maxVelocity, maxOmegaRadPerSec, maxAcc, () -> maxFrontAcc, () -> maxSideAcc, maxForwardAcc);
+ }
+
+ /**
+ * A constructor for basic specifications
+ *
+ * @param maxVelocity the max linear velocity of the swerve
+ * @param maxOmegaRadPerSec the max angular velocity of the swerve
+ * @param maxAcc the max acceleration of the swerve
+ */
+ public SwerveSpecs(double maxVelocity, double maxOmegaRadPerSec, double maxAcc) {
+ this(maxVelocity, maxOmegaRadPerSec, maxAcc, () -> 10, () -> 10, 10);
+ }
+}
diff --git a/src/main/java/frc/excalib/swerve/swerve_utils/SysIdRoutineOption.java b/src/main/java/frc/excalib/swerve/swerve_utils/SysIdRoutineOption.java
new file mode 100644
index 0000000..eef91e5
--- /dev/null
+++ b/src/main/java/frc/excalib/swerve/swerve_utils/SysIdRoutineOption.java
@@ -0,0 +1,6 @@
+package frc.excalib.swerve.swerve_utils;
+
+public enum SysIdRoutineOption {
+ DRIVE,
+ ANGLE
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..6359093
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,5 @@
+package frc.robot;
+
+public class Constants {
+}
+
diff --git a/src/main/java/frc/robot/SwerveConstants.java b/src/main/java/frc/robot/SwerveConstants.java
deleted file mode 100644
index 999bc70..0000000
--- a/src/main/java/frc/robot/SwerveConstants.java
+++ /dev/null
@@ -1,77 +0,0 @@
-package frc.robot;
-
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.path.PathConstraints;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import frc.excalib.control.gains.Gains;
-import frc.excalib.control.imu.IMU;
-import frc.excalib.control.imu.Pigeon;
-
-public class SwerveConstants {
- public static final int FRONT_LEFT_DRIVE_ID = 0;
- public static final int FRONT_RIGHT_DRIVE_ID = 0;
- public static final int BACK_RIGHT_DRIVE_ID = 0;
- public static final int BACK_LEFT_DRIVE_ID = 0;
-
- public static final int FRONT_LEFT_ROTATION_ID = 0;
- public static final int FRONT_RIGHT_ROTATION_ID = 0;
- public static final int BACK_RIGHT_ROTATION_ID = 0;
- public static final int BACK_LEFT_ROTATION_ID = 0;
-
- public static final int GYRO_ID = 0;
- public static final String SWERVE_CANBUS = "Swerve";
-
- public static final double PID_TOLERANCE = 0;
-
- public static final double TRACK_WIDTH = 0; // Meters
- public static final Translation2d FRONT_LEFT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, TRACK_WIDTH / 2);
- public static final Translation2d FRONT_RIGHT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, -TRACK_WIDTH / 2);
- public static final Translation2d BACK_LEFT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, TRACK_WIDTH / 2);
- public static final Translation2d BACK_RIGHT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, -TRACK_WIDTH / 2);
-
- public static final double MAX_MODULE_VEL = 0;
- public static final double MAX_FRONT_ACC = 0;
- public static final double MAX_SIDE_ACC = 0;
- public static final double MAX_SKID_ACC = 0;
- public static final double MAX_FORWARD_ACC = 0;
- public static final double MAX_VEL = 0;
- public static final double MAX_OMEGA_RAD_PER_SEC = 0;
- public static final double MAX_OMEGA_RAD_PER_SEC_SQUARE = 0;
-
- public static final double ANGLE_CONTROLLER_TOLORANCE = 0;
- public static final double X_CONTROLLER_TOLORANCE = 0;
- public static final double Y_CONTROLLER_TOLORANCE = 0;
-
- public static final PathConstraints MAX_PATH_CONSTRAINTS = new PathConstraints(
- MAX_VEL,
- MAX_SKID_ACC,
- MAX_OMEGA_RAD_PER_SEC,
- MAX_OMEGA_RAD_PER_SEC_SQUARE,
- 12.0,
- false
- );
-
- public static final CANcoder FRONT_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS);
- public static final CANcoder FRONT_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS);
- public static final CANcoder BACK_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS);
- public static final CANcoder BACK_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS);
-
- public static final double VELOCITY_CONVERSION_FACTOR = 0;
- public static final double POSITION_CONVERSION_FACTOR = 0;
- public static final double ROTATION_VELOCITY_CONVERSION_FACTOR = 0;
-
- public static final PIDConstants TRANSLATION_PID_CONSTANTS = new PIDConstants(0, 0, 0);
- public static final PIDConstants ANGLE_PID_CONSTANTS = new PIDConstants(0, 0, 0);
- public static final Gains ANGLE_GAINS = new Gains(0, 0, 0);
- public static final Gains TRANSLATION_GAINS = new Gains(0, 0, 0);
-
- public static final Gains SWERVE_DRIVE_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0);
- public static final Gains SWERVE_ROTATION_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0);
-
- public static final IMU GYRO = new Pigeon(GYRO_ID, SWERVE_CANBUS, new Rotation3d());
-
-
-}
-
diff --git a/src/main/java/frc/robot/subsystems/swerve_example/Constants.java b/src/main/java/frc/robot/subsystems/swerve_example/Constants.java
new file mode 100644
index 0000000..f3ef970
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve_example/Constants.java
@@ -0,0 +1,83 @@
+package frc.robot.subsystems.swerve_example;
+
+import com.pathplanner.lib.config.PIDConstants;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.excalib.control.gains.Gains;
+import frc.excalib.control.math.Circle;
+import frc.excalib.swerve.swerve_utils.SwerveModuleConfiguration.SwerveModuleType;
+
+import static frc.excalib.swerve.swerve_utils.SwerveModuleConfiguration.SwerveModuleType.MK4i_L3;
+
+public class Constants {
+ public static final String SWERVE_CANBUS = "CTRESwerve";
+
+ // Motors & Sensors IDs
+ public static final int FRONT_LEFT_DRIVE_ID = 0;
+ public static final int FRONT_RIGHT_DRIVE_ID = 0;
+ public static final int BACK_RIGHT_DRIVE_ID = 0;
+ public static final int BACK_LEFT_DRIVE_ID = 0;
+
+ public static final int FRONT_LEFT_ROTATION_ID = 0;
+ public static final int FRONT_RIGHT_ROTATION_ID = 0;
+ public static final int BACK_RIGHT_ROTATION_ID = 0;
+ public static final int BACK_LEFT_ROTATION_ID = 0;
+
+ public static final int FRONT_LEFT_CANCODER_ID = 0;
+ public static final int FRONT_RIGHT_CANCODER_ID = 0;
+ public static final int BACK_RIGHT_CANCODER_ID = 0;
+ public static final int BACK_LEFT_CANCODER_ID = 0;
+
+ public static final int GYRO_ID = 0;
+ public static final Rotation3d GYRO_OFFSET = new Rotation3d();
+
+ // Module Specs
+ public static final SwerveModuleType MODULE_TYPE = MK4i_L3;
+
+ // Driving Mechanism Constraints
+ public static final double MAX_MODULE_VELOCITY = 0;
+ public static final double MAX_MODULE_ACCELERATION = 0;
+ public static final double MAX_MODULE_JERK = 0;
+ public static final int STALL_DRIVING_MODULE_LIMIT = 0;
+ public static final int FREE_DRIVING_MODULE_LIMIT = 0;
+ public static final Gains DRIVING_MODULE_GAINS = new Gains();
+
+ // Steering Mechanism Constraints
+ public static final int STALL_STEERING_MODULE_LIMIT = 0;
+ public static final int FREE_STEERING_MODULE_LIMIT = 0;
+ public static final Gains STEERING_MODULE_GAINS = new Gains();
+ public static final double STEERING_PID_TOLERANCE = 0;
+
+ public static final Translation2d[] MODULE_LOCATIONS =
+ new Translation2d[]{
+ new Translation2d(),
+ new Translation2d(),
+ new Translation2d(),
+ new Translation2d()
+ };
+
+ // Swerve Constraints
+ public static final double MAX_VELOCITY = 0;
+ public static final double MAX_OMEGA_RAD_PER_SEC = 0;
+ public static final double MAX_ACC = 0;
+ public static final double MAX_FRONT_ACC = 0;
+ public static final double MAX_SIDE_ACC = 0;
+ public static final double MAX_FORWARD_ACC = 0;
+
+ public static final Pose2d INITIAL_SWERVE_POSITION = new Pose2d();
+
+ // Auto Drive Constants
+ public static final Gains ANGLE_PID_GAINS = new Gains();
+ public static final Gains TRANSLATION_PID_GAINS = new Gains();
+
+ public static final PIDConstants ANGLE_PID_PP_CONSTANTS = new PIDConstants(0.0, 0.0, 0.0);
+ public static final PIDConstants TRANSLATION_PID_PP_CONSTANTS = new PIDConstants(0.0, 0.0, 0.0);
+
+ // Obstacle Avoidance Constants
+ public static final Circle OBSTACLE = new Circle(0, 0, 1);
+ public static final double DISTANCE_TO_AVOID_OBSTACLE = 2; // m
+ public static final double POSSIBLE_DEVIATION = Math.PI / 18;
+
+ public static final double ROTATIONS_TO_RADIANS = 2 * Math.PI;
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve_example/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve_example/SwerveSubsystem.java
new file mode 100644
index 0000000..a411b70
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve_example/SwerveSubsystem.java
@@ -0,0 +1,264 @@
+package frc.robot.subsystems.swerve_example;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.controller.PIDController;
+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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.excalib.additional_utilities.AllianceUtils;
+import frc.excalib.control.imu.IMU;
+import frc.excalib.control.imu.Pigeon;
+import frc.excalib.control.math.Vector2D;
+import frc.excalib.control.motor.controllers.Motor;
+import frc.excalib.control.motor.controllers.SparkMaxMotor;
+import frc.excalib.control.motor.controllers.TalonFXMotor;
+import frc.excalib.control.motor.motor_specs.DirectionState;
+import frc.excalib.control.motor.motor_specs.IdleState;
+import frc.excalib.swerve.ModulesHolder;
+import frc.excalib.swerve.SwerveMechanism;
+import frc.excalib.swerve.SwerveModule;
+import frc.excalib.swerve.swerve_utils.SwerveConfigurationUtils;
+import frc.excalib.swerve.swerve_utils.SwerveSpecs;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless;
+import static edu.wpi.first.math.geometry.Rotation2d.kPi;
+import static frc.robot.subsystems.swerve_example.Constants.*;
+
+@Logged
+public class SwerveSubsystem extends SubsystemBase {
+ // An array of the absolute encoders that measure the modules' positions
+ private final CANcoder[] m_cancoders;
+
+ // An array of the swerve modules
+ private final SwerveModule[] m_swerveModules;
+
+ // The IMU measures the robot's heading
+ private final IMU m_imu;
+
+ // The swerve mechanism
+ private final SwerveMechanism m_swerveMechanism;
+
+ // PID controllers for the example turnToAngle and pidToPose commands
+ private final PIDController m_angleController;
+ private final PIDController m_xController;
+ private final PIDController m_yController;
+ // setpoint for the example turnToAngle and pidToPose commands
+ private Supplier m_angleSetpoint;
+ private Supplier m_translationSetpoint;
+ // finish trigger for the example pidToPoseCommand.
+ private final Trigger m_atPoseTrigger;
+
+ /**
+ * A constructor that initialize the swerve subsystem
+ */
+ public SwerveSubsystem() {
+ // The drive motors of the modules.
+ // THEY MUST BE ARRANGED IN THE FOLLOWING ORDER: front left, front right, back left, back right
+ Motor[] driveMotors = new Motor[]{
+ new TalonFXMotor(FRONT_LEFT_DRIVE_ID, SWERVE_CANBUS),
+ new TalonFXMotor(FRONT_RIGHT_DRIVE_ID, SWERVE_CANBUS),
+ new TalonFXMotor(BACK_LEFT_DRIVE_ID, SWERVE_CANBUS),
+ new TalonFXMotor(BACK_RIGHT_DRIVE_ID, SWERVE_CANBUS)
+ };
+
+ // The steering motors of the modules.
+ // THEY MUST BE ARRANGED IN THE FOLLOWING ORDER: front left, front right, back left, back right
+ Motor[] steeringMotors = new Motor[]{
+ new SparkMaxMotor(FRONT_LEFT_ROTATION_ID, kBrushless),
+ new SparkMaxMotor(FRONT_RIGHT_ROTATION_ID, kBrushless),
+ new SparkMaxMotor(BACK_LEFT_ROTATION_ID, kBrushless),
+ new SparkMaxMotor(BACK_RIGHT_ROTATION_ID, kBrushless)
+ };
+
+ m_cancoders = new CANcoder[]{
+ new CANcoder(FRONT_LEFT_CANCODER_ID, SWERVE_CANBUS),
+ new CANcoder(FRONT_RIGHT_CANCODER_ID, SWERVE_CANBUS),
+ new CANcoder(BACK_LEFT_CANCODER_ID, SWERVE_CANBUS),
+ new CANcoder(BACK_RIGHT_CANCODER_ID, SWERVE_CANBUS)
+ };
+
+ // An array of DoubleSupplier represents the positions of the modules.
+ // THEY MUST BE ARRANGED IN THE FOLLOWING ORDER: front left, front right, back left, back right
+ DoubleSupplier[] modulesPositionsSuppliers = {
+ () -> m_cancoders[0].getPosition().getValueAsDouble() * ROTATIONS_TO_RADIANS,
+ () -> m_cancoders[1].getPosition().getValueAsDouble() * ROTATIONS_TO_RADIANS,
+ () -> m_cancoders[2].getPosition().getValueAsDouble() * ROTATIONS_TO_RADIANS,
+ () -> m_cancoders[3].getPosition().getValueAsDouble() * ROTATIONS_TO_RADIANS
+ };
+
+ // A function that initializes the swerve modules objects
+ m_swerveModules = SwerveConfigurationUtils.setupSwerveModules(
+ SwerveConfigurationUtils.setupDrivingMechanisms(
+ driveMotors, STALL_DRIVING_MODULE_LIMIT, FREE_DRIVING_MODULE_LIMIT,
+ DirectionState.FORWARD, IdleState.BRAKE, MODULE_TYPE.getModuleConfiguration(),
+ DRIVING_MODULE_GAINS, MAX_MODULE_ACCELERATION, MAX_MODULE_JERK
+ ),
+ SwerveConfigurationUtils.setupSteeringMechanisms(
+ steeringMotors, STALL_STEERING_MODULE_LIMIT, FREE_STEERING_MODULE_LIMIT,
+ DirectionState.FORWARD, IdleState.BRAKE, MODULE_TYPE.getModuleConfiguration(),
+ STEERING_MODULE_GAINS, modulesPositionsSuppliers, STEERING_PID_TOLERANCE
+ ),
+ MODULE_LOCATIONS,
+ MAX_MODULE_VELOCITY //Optionally, you can set the max velocity of each module separately (using an array of doubles)
+ );
+
+ // Initialization of the IMU
+ m_imu = new Pigeon(GYRO_ID, SWERVE_CANBUS, GYRO_OFFSET);
+ m_imu.resetIMU();
+
+ // Initialization of the swerve mechanism
+ m_swerveMechanism = new SwerveMechanism(
+ new ModulesHolder(
+ // MUST BE ARRANGED IN THE FOLLOWING ORDER: front left, front right, back left, back right
+ m_swerveModules[0],
+ m_swerveModules[1],
+ m_swerveModules[2],
+ m_swerveModules[3]
+ ),
+ new SwerveSpecs(MAX_VELOCITY, MAX_OMEGA_RAD_PER_SEC, MAX_ACC, MAX_FRONT_ACC, MAX_SIDE_ACC, MAX_FORWARD_ACC),
+ m_imu,
+ INITIAL_SWERVE_POSITION
+ );
+
+ // Initialization of the PID controllers
+ m_angleController = new PIDController(ANGLE_PID_GAINS.kp, ANGLE_PID_GAINS.ki, ANGLE_PID_GAINS.kd);
+ m_xController = new PIDController(TRANSLATION_PID_GAINS.kp, TRANSLATION_PID_GAINS.ki, TRANSLATION_PID_GAINS.kd);
+ m_yController = new PIDController(TRANSLATION_PID_GAINS.kp, TRANSLATION_PID_GAINS.ki, TRANSLATION_PID_GAINS.kd);
+
+ // Initialization of the setpoints
+ m_angleSetpoint = Rotation2d::new;
+ m_translationSetpoint = Translation2d::new;
+
+ // Initialization of the finish trigger
+ m_atPoseTrigger = new Trigger(m_xController::atSetpoint).and(m_yController::atSetpoint).and(m_angleController::atSetpoint).debounce(0.1);
+
+ // Initialization of the AutoBuilder for pathplanner
+// initAutoBuilder();
+ }
+
+ /**
+ * Takes the drive command from the swerve mechanism.
+ *
+ * @param velocityMPS Supplier for the desired linear velocity in meters per second.
+ * @param omegaRadPerSec Supplier for the desired angular velocity in radians per second.
+ * @param fieldOriented Supplier indicating whether the control is field-oriented.
+ * @return A command that drives the robot.
+ */
+ public Command driveCommand(Supplier velocityMPS,
+ DoubleSupplier omegaRadPerSec,
+ BooleanSupplier fieldOriented) {
+ return m_swerveMechanism.driveCommand(velocityMPS, omegaRadPerSec, fieldOriented, true, this).withName("Drive Command");
+ }
+
+ /**
+ * Turns the robot to a desired angle.
+ *
+ * @param angleSetpoint The desired angle in radians.
+ * @return A command that turns the robot to the wanted angle.
+ */
+ public Command turnToAngleCommand(Supplier velocityMPS, Supplier angleSetpoint) {
+ return new SequentialCommandGroup(
+ new InstantCommand(() -> {
+ m_angleController.calculate(m_swerveMechanism.getRotation2D().getRadians(), angleSetpoint.get().getRadians());
+ m_angleSetpoint = angleSetpoint;
+ }),
+ driveCommand(
+ velocityMPS,
+ () -> m_angleController.calculate(m_swerveMechanism.getRotation2D().getRadians(), angleSetpoint.get().getRadians()),
+ () -> true
+ )
+ ).until(m_angleController::atSetpoint).withName("Turn To Angle Command");
+ }
+
+ /**
+ * Drives the robot to a desired pose.
+ *
+ * @param poseSetpoint The desired pose on the field.
+ * @return A command that turns the robot to the wanted angle.
+ */
+ public Command pidToPoseCommand(Supplier poseSetpoint) {
+ return new SequentialCommandGroup(
+ new InstantCommand(
+ () -> {
+ m_xController.calculate(m_swerveMechanism.getPose2D().getX(), poseSetpoint.get().getX());
+ m_yController.calculate(m_swerveMechanism.getPose2D().getY(), poseSetpoint.get().getY());
+ m_angleController.calculate(m_swerveMechanism.getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians());
+ m_translationSetpoint = () -> poseSetpoint.get().getTranslation();
+ m_angleSetpoint = () -> poseSetpoint.get().getRotation();
+ }
+ ),
+ driveCommand(
+ () -> {
+ Vector2D vel = new Vector2D(
+ m_xController.calculate(m_swerveMechanism.getPose2D().getX(), poseSetpoint.get().getX()),
+ m_yController.calculate(m_swerveMechanism.getPose2D().getY(), poseSetpoint.get().getY())
+ );
+ if (AllianceUtils.isRedAlliance()) return vel.rotate(kPi);
+ return vel;
+ },
+ () -> m_angleController.calculate(m_swerveMechanism.getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()),
+ () -> true
+ )
+ ).until(m_atPoseTrigger).withName("PID To Pose Command");
+ }
+
+ /**
+ * A function that initialize the AutoBuilder for pathplanner.
+ */
+ public void initAutoBuilder() {
+ // Load the RobotConfig from the GUI settings. You should probably
+ // store this in your Constants file
+ RobotConfig config = null;
+ try {
+ config = RobotConfig.fromGUISettings();
+ } catch (Exception e) {
+ // Handle exception as needed
+ e.printStackTrace();
+ }
+
+ // Configure AutoBuilder last
+ AutoBuilder.configure(
+ m_swerveMechanism::getPose2D, // Robot pose supplier
+ m_swerveMechanism::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
+ m_swerveMechanism::getRobotChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+ (speeds, feedforwards) -> m_swerveMechanism.driveRobotRelativeChassisSpeeds(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+ new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+ TRANSLATION_PID_PP_CONSTANTS, // Translation PID constants
+ ANGLE_PID_PP_CONSTANTS // Rotation PID constants
+ ),
+ config, // The robot configuration
+ () -> {
+ // Boolean supplier that controls when the path will be mirrored for the red alliance
+ // This will flip the path being followed to the red side of the field.
+ // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+ var alliance = DriverStation.getAlliance();
+ return alliance.filter(value -> value == DriverStation.Alliance.Red).isPresent();
+ },
+ this // Reference to this subsystem to set requirements
+ );
+ }
+
+ /**
+ * A method that updates the modules positions of all modules.
+ */
+ @Override
+ public void periodic() {
+ m_swerveMechanism.periodic();
+
+ m_swerveMechanism.updateOdometry();
+ }
+}