Work on pure pursuit

include/lemlib/movements/purepursuit.hpp

@@ -4,30 +4,9 @@
 #include <vector>
 #include "lemlib/pose.hpp"
 #include "lemlib/asset.hpp"
-#include "lemlib/chassis/structs.hpp"
 #include "lemlib/movements/movement.hpp"
 
 namespace lemlib {
-/**
- * @brief Waypoint class. Derived from Pose. Has speed field
- */
-class Waypoint : public Pose {
-    public:
-        /**
-         * @brief Construct a new Waypoint
-         *
-         * @param x
-         * @param y
-         * @param theta
-         * @param speed
-         */
-        Waypoint(float x, float y, float theta, float speed)
-            : Pose(x, y, theta),
-              speed(speed) {}
-
-        float speed;
-};
-
 /**
  * @brief Pure Pursuit class. Derived from Movement
  */
@@ -36,16 +15,15 @@ class PurePursuit : public Movement {
         /**
          * @brief Construct a new Pure Pursuit movement
          *
-         * @param drive drivetrain settings
+         * @param trackWidth the width of the chassis. Units in inches
          * @param path const reference to the asset containing the path
-         * @param lookahead the lookahead distance. Units in inches. Recommended value is 15, but can be changed if
+         * @param lookaheadDist the lookahead distance. Units in inches. Recommended value is 15, but can be changed if
          * needed
          * @param timeout the maximum time the robot can spend moving
          * @param forwards whether the chassis should move forwards or backwards. True by default
          * @param maxSpeed the maximum speed the robot can move at. 127 by default
          */
-        PurePursuit(Drivetrain_t drive, const asset& path, float lookahead, int timeout, bool forwards = true,
-                    int maxSpeed = 127);
+        PurePursuit(float trackWidth, const asset& path, float lookaheadDist, int timeout, bool forwards, int maxSpeed);
 
         /**
          * @brief Update the movement
@@ -66,16 +44,18 @@ class PurePursuit : public Movement {
          */
         float getDist() override;
     private:
-        Drivetrain_t drive;
         std::vector<Waypoint> path;
+        Pose prevPose = Pose(0, 0, 0);
+        Waypoint prevLookahead = Waypoint(0, 0);
+        float trackWidth;
         int startTime;
-        float lookahead;
+        float lookaheadDist;
         int timeout;
         bool forwards;
         int maxSpeed;
 
         int compState;
-        int state = 0; // 0 = in progress, 1 = settling, 2 = done
+        int state = 0; // 0 = in progress, 1 = done
         float dist = 0;
 };
 }; // namespace lemlib

include/lemlib/pose.hpp

@@ -62,6 +62,20 @@ class Pose {
          * @return Pose
          */
         Pose operator/(const float& other);
+        /**
+         * @brief Check if two poses are equal
+         *
+         * @param other the other pose
+         * @return bool
+         */
+        bool operator==(const Pose& other);
+        /**
+         * @brief Check if two poses are not equal
+         *
+         * @param other the other pose
+         * @return bool
+         */
+        bool operator!=(const Pose& other);
         /**
          * @brief Linearly interpolate between two poses
          *
@@ -92,4 +106,27 @@ class Pose {
          */
         Pose rotate(float angle);
 };
+
+/**
+ * @brief Waypoint class. Derived from Pose. Has speed field
+ */
+class Waypoint : public Pose {
+    public:
+        /**
+         * @brief Construct a new Waypoint
+         *
+         * @param x
+         * @param y
+         * @param theta
+         * @param speed
+         */
+        Waypoint(float x, float y, float theta = 0, float speed = 0, int index = 0)
+            : Pose(x, y, theta),
+              speed(speed),
+              index(index) {}
+
+        float speed;
+        int index;
+};
+
 } // namespace lemlib

include/lemlib/util.hpp

@@ -12,6 +12,7 @@
 #pragma once
 
 #include <vector>
+#include <string>
 #include "lemlib/pose.hpp"
 
 namespace lemlib {
@@ -91,4 +92,35 @@ float ema(float current, float previous, float smooth);
  * @return float curvature
  */
 float getCurvature(Pose pose, Pose other);
+
+/**
+ * @brief function that returns elements in a string separated by a delimeter
+ *
+ * @param input the raw string
+ * @param delimeter string separating the elements in the input string
+ * @return std::vector<std::string> vector of elements read from the file
+ */
+std::vector<std::string> splitString(const std::string& input, const std::string& delimiter);
+
+/**
+ * @brief Finds the closest waypoint in a vector of waypoints to a target pose
+ *
+ * @param poses the vector of waypoints
+ * @param target the target pose
+ * @return Pose
+ */
+Waypoint closestWaypoint(const std::vector<Waypoint>& waypoints, const Pose& target);
+
+/**
+ * @brief Find the intersection between a circle and a line
+ *
+ *
+ * @param p1 the start of the line
+ * @param p2 the end of the line
+ * @param center the center of the circle
+ * @param radius the radius of the circle
+ * @return Pose the intersection point. If no intersection is found, the circle center will be returned. If there are
+ * multiple intersections, the first one will be returned
+ */
+Pose circleLineIntersect(Pose p1, Pose p2, Pose center, float radius);
 } // namespace lemlib