Creating Path Spline and Path Segment logic

src/v5_hal/firmware/include/pathing/PathSegment.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "api.h"
+#include "eigen/Eigen/Dense"
+#include "pathing/Waypoint.h"
+
+class PathSegment {
+private:
+    Waypoint m_start_waypoint;
+    Waypoint m_end_waypoint;
+    float m_length_m;
+    float m_duration_ms;
+
+public:
+    PathSegment(Waypoint start_waypoint, Waypoint end_waypoint);
+
+    void initialize();
+
+    float getLength();
+
+    float getDuration();
+};

src/v5_hal/firmware/include/pathing/PathSpline.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <vector>
+
+#include "api.h"
+#include "eigen/Eigen/Dense"
+#include "pathing/Waypoint.h"
+
+class PathSpline {
+private:
+    Waypoint m_start_waypoint;
+    Waypoint m_end_waypoint;
+    float m_length_m;
+    float m_duration_ms;
+
+    Rotation2Dd m_path_start_angle;
+    Rotation2Dd m_path_end_angle;
+
+    std::vector<Waypoint> m_calculated_waypoints;
+
+    void m_generateSpline();
+
+public:
+    PathSpline(Waypoint start_waypoint, Waypoint end_waypoint);
+
+    void calculate();
+
+    float getLength();
+
+    float getDuration();
+
+    Waypoint getWaypointAtPercentage(float percentage);
+
+    Waypoint getWaypointAtTime(float time);
+};

src/v5_hal/firmware/include/pathing/Waypoint.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "api.h"
+#include "eigen/Eigen/Dense"
+#include "math/Pose.h"
+
+class Waypoint {
+private:
+    Pose m_position;
+    Vector2d m_velocity;
+
+public:
+    Waypoint(Pose position, Vector2d velocity);
+
+    Pose getPosition();
+
+    Vector2d getVelocity();
+};

src/v5_hal/firmware/src/pathing/PathSegment.cpp

@@ -0,0 +1,22 @@
+#include "pathing/PathSegment.h"
+
+PathSegment::PathSegment(Waypoint start_waypoint, Waypoint end_waypoint) : 
+        m_start_waypoint(start_waypoint), 
+        m_end_waypoint(end_waypoint) {
+
+}
+
+void PathSegment::initialize() {
+    m_length_m = m_end_waypoint.getPosition().position(1) - m_start_waypoint.getPosition().position(1) / 
+                 m_end_waypoint.getPosition().position(0) - m_start_waypoint.getPosition().position(0);
+
+    // TODO calculate duration
+}
+
+float PathSegment::getLength() {
+    return m_length_m;
+}
+
+float PathSegment::getDuration() {
+    return m_duration_ms;
+}

src/v5_hal/firmware/src/pathing/PathSpline.cpp

@@ -0,0 +1,50 @@
+#include "pathing/PathSpline.h"
+
+PathSpline::PathSpline(Waypoint start_waypoint, Waypoint end_waypoint) : 
+        m_start_waypoint(start_waypoint), 
+        m_end_waypoint(end_waypoint) {
+
+}
+
+void PathSpline::m_generateSpline() {
+
+}
+
+void PathSpline::calculate() {
+    // Generate the spline
+    // Get the start and end angle of the path
+
+    // Get the distance between the two waypoints
+
+    // Get the change of tangent angle between the last and first point of the path
+
+    // 
+
+    // Calculate waypoints in between the start and end
+    // should be length parameterized in order to have a set distance between points
+
+    // https://github.com/msoe-vex/WebDashboard/blob/5c112bcf74bd4b63ad7efa2d1fb9093cc9a7cb3f/www/path.js#L112
+    int sample_points = 1;
+
+    do {
+
+    } while (true);
+
+
+}
+
+float PathSpline::getLength() {
+    return m_length_m;
+}
+
+float PathSpline::getDuration() {
+    return m_duration_ms;
+}
+
+Waypoint PathSpline::getWaypointAtPercentage(float percentage) {
+
+}
+
+Waypoint PathSpline::getWaypointAtTime(float time) {
+
+}

src/v5_hal/firmware/src/pathing/Waypoint.cpp

@@ -0,0 +1,15 @@
+#include "pathing/Waypoint.h"
+
+Waypoint::Waypoint(Pose position, Vector2d velocity) :
+        m_position(position), 
+        m_velocity(velocity) {
+
+}
+
+Pose Waypoint::getPosition() {
+    return m_position;
+}
+
+Vector2d Waypoint::getVelocity() {
+    return m_velocity;
+}