Swerve Bézier Trajectory Command for Vision

src/main/java/xbot/common/subsystems/drive/SwerveBezierTrajectoryCommand.java

@@ -0,0 +1,98 @@
+package xbot.common.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import xbot.common.logging.RobotAssertionManager;
+import xbot.common.properties.PropertyFactory;
+import xbot.common.subsystems.drive.control_logic.HeadingModule;
+import xbot.common.subsystems.pose.BasePoseSubsystem;
+import xbot.common.trajectory.XbotSwervePoint;
+
+import javax.inject.Inject;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * A "not-so-simple" SwerveSimpleTrajectoryCommand that does a Bézier curve!~~
+ * TO USE: call setBezierConfiguration and pass it the needed params
+ * If you are EXTENDING this because you need some logic to compute control points:
+ * Remember to call setBezierConfiguration and super.initialize()
+ */
+public class SwerveBezierTrajectoryCommand extends SwerveSimpleTrajectoryCommand {
+
+    List<Translation2d> controlPoints;
+    Pose2d endPoint;
+    int steps;
+
+    @Inject
+    public SwerveBezierTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf,
+                                         HeadingModule.HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) {
+        super(drive, pose, pf, headingModuleFactory, assertionManager);
+    }
+
+    @Override
+    public void initialize() {
+        setBezierCurve(controlPoints, endPoint, steps);
+        super.initialize();
+    }
+
+    /**
+     * Set the configuration of the Bézier curve, we can't generate our curve immediately as our pose will change
+     * @param controlPoints of the Bézier curve
+     * @param endPoint of our command
+     * @param steps to split our Bézier curve into
+     */
+    public void setBezierConfiguration(List<Translation2d> controlPoints, Pose2d endPoint, int steps) {
+        this.controlPoints = controlPoints;
+        this.steps = steps;
+        this.endPoint = endPoint;
+    }
+
+    /**
+     * Generates a Bézier curve for our command!
+     */
+    private void setBezierCurve(List<Translation2d> controlPoints, Pose2d endPoint, int steps) {
+        List<XbotSwervePoint> bezierPoints = new ArrayList<>();
+        List<Translation2d> allPoints = new ArrayList<>();
+        allPoints.add(pose.getCurrentPose2d().getTranslation());
+        allPoints.addAll(controlPoints);
+        allPoints.add(endPoint.getTranslation());
+
+        Rotation2d startingRotation = pose.getCurrentPose2d().getRotation();
+        for (int i = 1; i <= steps; i++) {
+            double lerpFraction = i / (double) steps;
+            XbotSwervePoint point = new XbotSwervePoint(
+                    deCasteljauIterative(allPoints, lerpFraction),
+                    startingRotation.plus((endPoint.getRotation().minus(startingRotation)).times(lerpFraction)),
+                    10
+            );
+            bezierPoints.add(point);
+        }
+
+        logic.setKeyPoints(bezierPoints);
+    }
+
+    /**
+     * Generated by some A.I., this tells us where we SHOULD be at while taking in consideration control points
+     * Apparently, "Iterative" potentially (not too-sure).
+     * @param points include: start, control points, end
+     * @param lerpFraction is our completion percentage
+     * @return our position during lerpFraction (progress of operation completion)
+     */
+    private Translation2d deCasteljauIterative(List<Translation2d> points, double lerpFraction) {
+        int n = points.size();
+        List<Translation2d> temp = new ArrayList<>(points);
+
+        // Compute the position using de Casteljau's algorithm (all we know is that it works)
+        for (int level = 1; level < n; level++) {
+            for (int i = 0; i < n - level; i++) {
+                double x = (1 - lerpFraction) * temp.get(i).getX() + lerpFraction * temp.get(i + 1).getX();
+                double y = (1 - lerpFraction) * temp.get(i).getY() + lerpFraction * temp.get(i + 1).getY();
+                temp.set(i, new Translation2d(x, y));
+            }
+        }
+
+        return temp.get(0);
+    }
+}