Add C++ Port

Pathfinder-Cpp/README.md

@@ -0,0 +1,7 @@
+# Pathfinder C++ Port
+This is a port of the C core libraries and uses the same API, except for imports. There is no
+ equivalent pathfinder.h, each header file must be imported as needed. of The purpose
+of this port is for C++ projects that will be run across multiple architectures
+or where you want to compile it yourself for some reason. Note that this is NOT
+for use in C++ robot applications, for that you always have the same target architecture
+(the RoboRIO). For that, add the compiled binaries per the instructions in the main page.

Pathfinder-Cpp/include/fit.h

@@ -0,0 +1,14 @@
+#ifndef PATHFINDER_FIT_H_DEF
+#define PATHFINDER_FIT_H_DEF
+
+#include "structs.h"
+#include "mathutil.h"
+
+void pf_fit_hermite_pre(Waypoint a, Waypoint b, Spline *s);
+void pf_fit_hermite_cubic(Waypoint a, Waypoint b, Spline *s);
+void pf_fit_hermite_quintic(Waypoint a, Waypoint b, Spline *s);
+
+#define FIT_HERMITE_CUBIC   &pf_fit_hermite_cubic
+#define FIT_HERMITE_QUINTIC &pf_fit_hermite_quintic
+
+#endif

Pathfinder-Cpp/include/followers/distance.h

@@ -0,0 +1,19 @@
+#ifndef PATHFINDER_FOL_DISTANCE_H_DEF
+#define PATHFINDER_FOL_DISTANCE_H_DEF
+
+#include <structs.h>
+
+typedef struct {
+    double kp, ki, kd, kv, ka;
+} FollowerConfig;
+
+typedef struct {
+    double last_error, heading, output;
+    int segment, finished;
+} DistanceFollower;
+
+double pathfinder_follow_distance(FollowerConfig c, DistanceFollower *follower, Segment *trajectory, int trajectory_length, double distance);
+
+double pathfinder_follow_distance2(FollowerConfig c, DistanceFollower *follower, Segment segment, int trajectory_length, double distance);
+
+#endif

Pathfinder-Cpp/include/followers/encoder.h

@@ -0,0 +1,21 @@
+#ifndef PATHFINDER_FOL_ENCODER_H_DEF
+#define PATHFINDER_FOL_ENCODER_H_DEF
+
+#include <structs.h>
+
+typedef struct {
+    int initial_position, ticks_per_revolution;
+    double wheel_circumference;
+    double kp, ki, kd, kv, ka;
+} EncoderConfig;
+
+typedef struct {
+    double last_error, heading, output;
+    int segment, finished;
+} EncoderFollower;
+
+double pathfinder_follow_encoder(EncoderConfig c, EncoderFollower *follower, Segment *trajectory, int trajectory_length, int encoder_tick);
+
+double pathfinder_follow_encoder2(EncoderConfig c, EncoderFollower *follower, Segment segment, int trajectory_length, int encoder_tick);
+
+#endif

Pathfinder-Cpp/include/generator.h

@@ -0,0 +1,20 @@
+#ifndef PATHFINDER_GENERATOR_H_DEF
+#define PATHFINDER_GENERATOR_H_DEF
+
+#include "trajectory.h"
+#include "structs.h"
+#include "spline.h"
+#include "fit.h"
+#include <stdlib.h>
+
+int pathfinder_prepare(const Waypoint *path, int path_length, void (*fit)(Waypoint,Waypoint,Spline*), int sample_count, double dt,
+        double max_velocity, double max_acceleration, double max_jerk, TrajectoryCandidate *cand);
+
+int pathfinder_prepare_LabVIEW(const Waypoint *path, int path_length, int sample_count, double dt,
+        double max_velocity, double max_acceleration, double max_jerk);
+
+int pathfinder_generate_LabVIEW(Segment *segments);
+
+int pathfinder_generate(TrajectoryCandidate *c, Segment *segments);
+
+#endif

Pathfinder-Cpp/include/io.h

@@ -0,0 +1,26 @@
+#ifndef PATHFINDER_IO_H_DEF
+#define PATHFINDER_IO_H_DEF
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "structs.h"
+
+#define CSV_LEADING_STRING "dt,x,y,position,velocity,acceleration,jerk,heading\n"
+
+void intToBytes(int n, char *bytes);
+int bytesToInt(char *bytes);
+void longToBytes(unsigned long long n, char *bytes);
+unsigned long long bytesToLong(char *bytes);
+double longToDouble(unsigned long long l);
+unsigned long long doubleToLong(double d);
+void doubleToBytes(double n, char *bytes);
+double bytesToDouble(char *bytes);
+
+void pathfinder_serialize(FILE *fp, Segment *trajectory, int trajectory_length);
+int pathfinder_deserialize(FILE *fp, Segment *target);
+
+void pathfinder_serialize_csv(FILE *fp, Segment *trajectory, int trajectory_length);
+int pathfinder_deserialize_csv(FILE *fp, Segment *target);
+
+#endif

Pathfinder-Cpp/include/mathutil.h

@@ -0,0 +1,18 @@
+#ifndef PATHFINDER_MATH_UTIL_H_DEF
+#define PATHFINDER_MATH_UTIL_H_DEF
+
+#include <math.h>
+
+#define PI 3.14159265358979323846
+#define TAU PI*2
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+double bound_radians(double angle);
+
+double r2d(double angleInRads);
+
+double d2r(double angleInDegrees);
+
+#endif

Pathfinder-Cpp/include/modifiers/swerve.h

@@ -0,0 +1,13 @@
+#ifndef PATHFINDER_MOD_SWERVE_H_DEF
+#define PATHFINDER_MOD_SWERVE_H_DEF
+
+#include <structs.h>
+
+typedef enum {
+    SWERVE_DEFAULT
+} SWERVE_MODE;
+
+void pathfinder_modify_swerve(Segment *original, int length, Segment *front_left, Segment *front_right,
+        Segment *back_left, Segment *back_right, double wheelbase_width, double wheelbase_depth, SWERVE_MODE mode);
+
+#endif

Pathfinder-Cpp/include/modifiers/tank.h

@@ -0,0 +1,9 @@
+#ifndef PATHFINDER_MOD_TANK_H_DEF
+#define PATHFINDER_MOD_TANK_H_DEF
+
+#include <structs.h>
+#include "mathutil.h"
+
+void pathfinder_modify_tank(Segment *original, int length, Segment *left, Segment *right, double wheelbase_width);
+
+#endif

Pathfinder-Cpp/include/spline.h

@@ -0,0 +1,19 @@
+#ifndef PATHFINDER_SPLINE_H_DEF
+#define PATHFINDER_SPLINE_H_DEF
+
+#include "structs.h"
+#include "mathutil.h"
+
+#define PATHFINDER_SAMPLES_FAST (int)1000
+#define PATHFINDER_SAMPLES_LOW  (int)PATHFINDER_SAMPLES_FAST*10
+#define PATHFINDER_SAMPLES_HIGH (int)PATHFINDER_SAMPLES_LOW*10
+
+Coord pf_spline_coords(Spline s, double percentage);
+double pf_spline_deriv(Spline s, double percentage);
+double pf_spline_deriv_2(double a, double b, double c, double d, double e, double k, double p);
+double pf_spline_angle(Spline s, double percentage);
+
+double pf_spline_distance(Spline *s, int sample_count);
+double pf_spline_progress_for_distance(Spline s, double distance, int sample_count);
+
+#endif

Pathfinder-Cpp/include/structs.h

@@ -0,0 +1,41 @@
+#ifndef PATHFINDER_STRUCT_H_DEF
+#define PATHFINDER_STRUCT_H_DEF
+
+typedef struct {
+    double x, y, angle;
+} Waypoint;
+
+typedef struct {
+    double a, b, c, d, e;
+    double x_offset, y_offset, angle_offset, knot_distance, arc_length;
+} Spline;
+
+typedef struct {
+    double x, y;
+} Coord;
+
+typedef struct {
+    double dt, x, y, position, velocity, acceleration, jerk, heading;
+} Segment;
+
+typedef struct {
+    double dt, max_v, max_a, max_j, src_v, src_theta, dest_pos, dest_v, dest_theta;
+    int sample_count;
+} TrajectoryConfig;
+
+typedef struct {
+    int filter1, filter2, length;
+    double dt, u, v, impulse;
+} TrajectoryInfo;
+
+typedef struct {
+    Spline *saptr;
+    double *laptr;
+    double totalLength;
+    int length;
+    int path_length;
+    TrajectoryInfo info;
+    TrajectoryConfig config;
+} TrajectoryCandidate;
+
+#endif

Pathfinder-Cpp/include/trajectory.h

@@ -0,0 +1,15 @@
+#ifndef PATHFINDER_TRAJECTORY_H_DEF
+#define PATHFINDER_TRAJECTORY_H_DEF
+
+#include "structs.h"
+#include "mathutil.h"
+#include <stdlib.h>
+
+void pf_trajectory_copy(Segment *src, Segment *dest, int length);
+
+TrajectoryInfo pf_trajectory_prepare(TrajectoryConfig c);
+int pf_trajectory_create(TrajectoryInfo info, TrajectoryConfig c, Segment *seg);
+int pf_trajectory_fromSecondOrderFilter(int filter_1_l, int filter_2_l,
+        double dt, double u, double v, double impulse, int len, Segment *t);
+
+#endif

Pathfinder-Cpp/src/fit.cpp

@@ -0,0 +1,36 @@
+#include "fit.h"
+
+void pf_fit_hermite_cubic(Waypoint a, Waypoint b, Spline *s) {
+    pf_fit_hermite_pre(a, b, s);
+
+    double a0_delta = tan(bound_radians(a.angle - (*s).angle_offset));
+    double a1_delta = tan(bound_radians(b.angle - (*s).angle_offset));
+
+    (*s).a = 0; (*s).b = 0;
+    (*s).c = (a0_delta + a1_delta) / ((*s).knot_distance * (*s).knot_distance);
+    (*s).d = -(2 * a0_delta + a1_delta) / (*s).knot_distance;
+    (*s).e = a0_delta;
+}
+
+void pf_fit_hermite_quintic(Waypoint a, Waypoint b, Spline *s) {
+    pf_fit_hermite_pre(a, b, s);
+
+    double a0_delta = tan(bound_radians(a.angle - (*s).angle_offset));
+    double a1_delta = tan(bound_radians(b.angle - (*s).angle_offset));
+
+    double d = (*s).knot_distance;
+
+    (*s).a = -(3 * (a0_delta + a1_delta)) / (d*d*d*d);
+    (*s).b = (8 * a0_delta + 7 * a1_delta) / (d*d*d);
+    (*s).c = -(6 * a0_delta + 4 * a1_delta) / (d*d);
+    (*s).d = 0; (*s).e = a0_delta;
+}
+
+void pf_fit_hermite_pre(Waypoint a, Waypoint b, Spline *s) {
+    (*s).x_offset = a.x;
+    (*s).y_offset = a.y;
+
+    double delta = sqrt( (b.x - a.x)*(b.x - a.x) + (b.y - a.y)*(b.y - a.y) );
+    (*s).knot_distance = delta;
+    (*s).angle_offset = atan2(b.y - a.y, b.x - a.x);
+}

Pathfinder-Cpp/src/followers/distance.cpp

@@ -0,0 +1,33 @@
+#include "followers/distance.h"
+
+double pathfinder_follow_distance(FollowerConfig c, DistanceFollower *follower, Segment *trajectory, int trajectory_length, double distance) {
+    int segment = follower->segment;
+    if (segment >= trajectory_length) {
+        follower->finished = 1;
+        follower->output = 0.0;
+        Segment last = trajectory[trajectory_length - 1];
+        follower->heading = last.heading;
+        return 0.0;
+    } else {
+        return pathfinder_follow_distance2(c, follower, trajectory[segment], trajectory_length, distance);
+    }
+}
+
+double pathfinder_follow_distance2(FollowerConfig c, DistanceFollower *follower, Segment s, int trajectory_length, double distance) {
+    if (follower->segment < trajectory_length) {
+        follower->finished = 0;
+        double error = s.position - distance;
+        double calculated_value = c.kp * error +
+                                  c.kd * ((error - follower->last_error) / s.dt) +
+                                  (c.kv * s.velocity + c.ka * s.acceleration);
+
+        follower->last_error = error;
+        follower->heading = s.heading;
+        follower->output = calculated_value;
+        follower->segment = follower->segment + 1;
+        return calculated_value;
+    } else {
+        follower->finished = 1;
+        return 0.0;
+    }
+}

Pathfinder-Cpp/src/followers/encoder.cpp

@@ -0,0 +1,36 @@
+#include "followers/encoder.h"
+
+double pathfinder_follow_encoder(EncoderConfig c, EncoderFollower *follower, Segment *trajectory, int trajectory_length, int encoder_tick) {
+    int segment = follower->segment;
+    if (segment >= trajectory_length) {
+        follower->finished = 1;
+        follower->output = 0.0;
+        Segment last = trajectory[trajectory_length - 1];
+        follower->heading = last.heading;
+        return 0.0;
+    } else {
+        return pathfinder_follow_encoder2(c, follower, trajectory[segment], trajectory_length, encoder_tick);
+    }
+}
+
+double pathfinder_follow_encoder2(EncoderConfig c, EncoderFollower *follower, Segment s, int trajectory_length, int encoder_tick) {
+    double distance_covered = ((double)encoder_tick - (double)c.initial_position) /  ((double)c.ticks_per_revolution);
+    distance_covered = distance_covered * c.wheel_circumference;
+
+    if (follower->segment < trajectory_length) {
+        follower->finished = 0;
+        double error = s.position - distance_covered;
+        double calculated_value = c.kp * error +
+                                  c.kd * ((error - follower->last_error) / s.dt) +
+                                  (c.kv * s.velocity + c.ka * s.acceleration);
+
+        follower->last_error = error;
+        follower->heading = s.heading;
+        follower->output = calculated_value;
+        follower->segment = follower->segment + 1;
+        return calculated_value;
+    } else {
+        follower->finished = 1;
+        return 0.0;
+    }
+}