From dc218fc2dfb318e49baa55db8298290956ee5628 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Fri, 1 Dec 2017 20:09:41 -0500 Subject: [PATCH 1/3] Add c++ code --- Pathfinder-Cpp/include/fit.h | 14 ++ Pathfinder-Cpp/include/followers/distance.h | 19 ++ Pathfinder-Cpp/include/followers/encoder.h | 21 +++ Pathfinder-Cpp/include/generator.h | 20 +++ Pathfinder-Cpp/include/io.h | 26 +++ Pathfinder-Cpp/include/mathutil.h | 18 ++ Pathfinder-Cpp/include/modifiers/swerve.h | 13 ++ Pathfinder-Cpp/include/modifiers/tank.h | 9 + Pathfinder-Cpp/include/spline.h | 19 ++ Pathfinder-Cpp/include/structs.h | 41 +++++ Pathfinder-Cpp/include/trajectory.h | 15 ++ Pathfinder-Cpp/src/fit.cpp | 36 ++++ Pathfinder-Cpp/src/followers/distance.cpp | 33 ++++ Pathfinder-Cpp/src/followers/encoder.cpp | 36 ++++ Pathfinder-Cpp/src/generator.cpp | 110 ++++++++++++ Pathfinder-Cpp/src/io.cpp | 189 ++++++++++++++++++++ Pathfinder-Cpp/src/mathutil.cpp | 15 ++ Pathfinder-Cpp/src/modifiers/swerve.cpp | 37 ++++ Pathfinder-Cpp/src/modifiers/tank.cpp | 50 ++++++ Pathfinder-Cpp/src/spline.cpp | 90 ++++++++++ Pathfinder-Cpp/src/trajectory.cpp | 111 ++++++++++++ 21 files changed, 922 insertions(+) create mode 100644 Pathfinder-Cpp/include/fit.h create mode 100644 Pathfinder-Cpp/include/followers/distance.h create mode 100644 Pathfinder-Cpp/include/followers/encoder.h create mode 100644 Pathfinder-Cpp/include/generator.h create mode 100644 Pathfinder-Cpp/include/io.h create mode 100644 Pathfinder-Cpp/include/mathutil.h create mode 100644 Pathfinder-Cpp/include/modifiers/swerve.h create mode 100644 Pathfinder-Cpp/include/modifiers/tank.h create mode 100644 Pathfinder-Cpp/include/spline.h create mode 100644 Pathfinder-Cpp/include/structs.h create mode 100644 Pathfinder-Cpp/include/trajectory.h create mode 100644 Pathfinder-Cpp/src/fit.cpp create mode 100644 Pathfinder-Cpp/src/followers/distance.cpp create mode 100644 Pathfinder-Cpp/src/followers/encoder.cpp create mode 100644 Pathfinder-Cpp/src/generator.cpp create mode 100644 Pathfinder-Cpp/src/io.cpp create mode 100644 Pathfinder-Cpp/src/mathutil.cpp create mode 100644 Pathfinder-Cpp/src/modifiers/swerve.cpp create mode 100644 Pathfinder-Cpp/src/modifiers/tank.cpp create mode 100644 Pathfinder-Cpp/src/spline.cpp create mode 100644 Pathfinder-Cpp/src/trajectory.cpp diff --git a/Pathfinder-Cpp/include/fit.h b/Pathfinder-Cpp/include/fit.h new file mode 100644 index 0000000..f05746b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/followers/distance.h b/Pathfinder-Cpp/include/followers/distance.h new file mode 100644 index 0000000..d9c195c --- /dev/null +++ b/Pathfinder-Cpp/include/followers/distance.h @@ -0,0 +1,19 @@ +#ifndef PATHFINDER_FOL_DISTANCE_H_DEF +#define PATHFINDER_FOL_DISTANCE_H_DEF + +#include + +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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/followers/encoder.h b/Pathfinder-Cpp/include/followers/encoder.h new file mode 100644 index 0000000..87f7a36 --- /dev/null +++ b/Pathfinder-Cpp/include/followers/encoder.h @@ -0,0 +1,21 @@ +#ifndef PATHFINDER_FOL_ENCODER_H_DEF +#define PATHFINDER_FOL_ENCODER_H_DEF + +#include + +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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/generator.h b/Pathfinder-Cpp/include/generator.h new file mode 100644 index 0000000..f796e27 --- /dev/null +++ b/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 + +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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/io.h b/Pathfinder-Cpp/include/io.h new file mode 100644 index 0000000..7708224 --- /dev/null +++ b/Pathfinder-Cpp/include/io.h @@ -0,0 +1,26 @@ +#ifndef PATHFINDER_IO_H_DEF +#define PATHFINDER_IO_H_DEF + +#include +#include +#include +#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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/mathutil.h b/Pathfinder-Cpp/include/mathutil.h new file mode 100644 index 0000000..db1c8c9 --- /dev/null +++ b/Pathfinder-Cpp/include/mathutil.h @@ -0,0 +1,18 @@ +#ifndef PATHFINDER_MATH_UTIL_H_DEF +#define PATHFINDER_MATH_UTIL_H_DEF + +#include + +#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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/modifiers/swerve.h b/Pathfinder-Cpp/include/modifiers/swerve.h new file mode 100644 index 0000000..1061313 --- /dev/null +++ b/Pathfinder-Cpp/include/modifiers/swerve.h @@ -0,0 +1,13 @@ +#ifndef PATHFINDER_MOD_SWERVE_H_DEF +#define PATHFINDER_MOD_SWERVE_H_DEF + +#include + +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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/modifiers/tank.h b/Pathfinder-Cpp/include/modifiers/tank.h new file mode 100644 index 0000000..f589979 --- /dev/null +++ b/Pathfinder-Cpp/include/modifiers/tank.h @@ -0,0 +1,9 @@ +#ifndef PATHFINDER_MOD_TANK_H_DEF +#define PATHFINDER_MOD_TANK_H_DEF + +#include +#include "mathutil.h" + +void pathfinder_modify_tank(Segment *original, int length, Segment *left, Segment *right, double wheelbase_width); + +#endif \ No newline at end of file diff --git a/Pathfinder-Cpp/include/spline.h b/Pathfinder-Cpp/include/spline.h new file mode 100644 index 0000000..d96ceb4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/structs.h b/Pathfinder-Cpp/include/structs.h new file mode 100644 index 0000000..0daad8c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Pathfinder-Cpp/include/trajectory.h b/Pathfinder-Cpp/include/trajectory.h new file mode 100644 index 0000000..e91fc63 --- /dev/null +++ b/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 + +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 \ No newline at end of file diff --git a/Pathfinder-Cpp/src/fit.cpp b/Pathfinder-Cpp/src/fit.cpp new file mode 100644 index 0000000..e269af2 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/followers/distance.cpp b/Pathfinder-Cpp/src/followers/distance.cpp new file mode 100644 index 0000000..4213cae --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/followers/encoder.cpp b/Pathfinder-Cpp/src/followers/encoder.cpp new file mode 100644 index 0000000..df6a9d1 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/generator.cpp b/Pathfinder-Cpp/src/generator.cpp new file mode 100644 index 0000000..0f83d1f --- /dev/null +++ b/Pathfinder-Cpp/src/generator.cpp @@ -0,0 +1,110 @@ +#include + +TrajectoryCandidate cand_LV; + +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) { + if (path_length < 2) return -1; + + cand->saptr = (Spline *) malloc((path_length - 1) * sizeof(Spline)); + cand->laptr = (double *) malloc((path_length - 1) * sizeof(double)); + double totalLength = 0; + + int i; + for (i = 0; i < path_length-1; i++) { + Spline s; + fit(path[i], path[i+1], &s); + double dist = pf_spline_distance(&s, sample_count); + cand->saptr[i] = s; + cand->laptr[i] = dist; + totalLength += dist; + } + + TrajectoryConfig config = {dt, max_velocity, max_acceleration, max_jerk, 0, path[0].angle, + totalLength, 0, path[0].angle, sample_count}; +// TrajectoryInfo info = pf_trajectory_prepare(config); + TrajectoryInfo info; + info = pf_trajectory_prepare(config); + int trajectory_length = info.length; + + cand->totalLength = totalLength; + cand->length = trajectory_length; + cand->path_length = path_length; + cand->info = info; + cand->config = config; + + return trajectory_length; +} + +/******************************************************************************************** +* LabVIEW memory allocation works different from C and a DLL call requires any memory +* be allocated up front for pointers that are used as outputs (i.e. 'trajectory candidate'). +* For the mode we could probably pass out the function pointer to LabVIEW via a function +* and pass it back in, but for now we can keep it static as cubic. Other option is to +* pass in a flag selecting which path algorithm, and make it an enum in the LabVIEW API. +* +* For the 'trajectory candidate' we don't want to expose it to LabVIEW at all, since we +* would need to know the sizeof 'Spline' and 'double' on the target and pre-allocate it. +* Instead, we keep it in the DLL memory and return a length instead of a status so we +* an allow LabVIEW to create the segments array. +*********************************************************************************************/ +int pathfinder_prepare_LabVIEW(const Waypoint *path, int path_length, int sample_count, double dt, + double max_velocity, double max_acceleration, double max_jerk) +{ + return pathfinder_prepare(path,path_length,FIT_HERMITE_CUBIC,sample_count,dt,max_velocity,max_acceleration,max_jerk,&cand_LV); +} + +int pathfinder_generate(TrajectoryCandidate *c, Segment *segments) { + int trajectory_length = c->length; + int path_length = c->path_length; + double totalLength = c->totalLength; + + Spline *splines = (c->saptr); + double *splineLengths = (c->laptr); + + int trajectory_status = pf_trajectory_create(c->info, c->config, segments); + if (trajectory_status < 0) return trajectory_status; + + int spline_i = 0; + double spline_pos_initial = 0, splines_complete = 0; + + int i; + for (i = 0; i < trajectory_length; ++i) { + double pos = segments[i].position; + + int found = 0; + while (!found) { + double pos_relative = pos - spline_pos_initial; + if (pos_relative <= splineLengths[spline_i]) { + Spline si = splines[spline_i]; + double percentage = pf_spline_progress_for_distance(si, pos_relative, c->config.sample_count); + Coord coords = pf_spline_coords(si, percentage); + segments[i].heading = pf_spline_angle(si, percentage); + segments[i].x = coords.x; + segments[i].y = coords.y; + found = 1; + } else if (spline_i < path_length - 2) { + splines_complete += splineLengths[spline_i]; + spline_pos_initial = splines_complete; + spline_i += 1; + } else { + Spline si = splines[path_length - 2]; + segments[i].heading = pf_spline_angle(si, 1.0); + Coord coords = pf_spline_coords(si, 1.0); + segments[i].x = coords.x; + segments[i].y = coords.y; + found = 1; + } + } + } + + free(c->saptr); + free(c->laptr); + + return trajectory_length; +} + +int pathfinder_generate_LabVIEW(Segment *segments) +{ + return pathfinder_generate(&cand_LV,segments); +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/io.cpp b/Pathfinder-Cpp/src/io.cpp new file mode 100644 index 0000000..fd6f23a --- /dev/null +++ b/Pathfinder-Cpp/src/io.cpp @@ -0,0 +1,189 @@ +#include "io.h" + +// CONVERSIONS // +// We're using the manual stuff instead of fwrite on a double array so these files +// can be preserved on different endianess systems. + +void intToBytes(int n, char *bytes) { + bytes[0] = (n >> 24) & 0xFF; + bytes[1] = (n >> 16) & 0xFF; + bytes[2] = (n >> 8) & 0xFF; + bytes[3] = n & 0xFF; +} + +int bytesToInt(char *bytes) { + int value = 0; + int i; + for (i = 0; i < 4; i++) { + int shift = (4 - 1 - i) * 8; + value += (bytes[i] & 0x000000FF) << shift; + } + return value; +} + +void longToBytes(unsigned long long n, char *bytes) { + bytes[0] = (n >> 56) & 0xFF; + bytes[1] = (n >> 48) & 0xFF; + bytes[2] = (n >> 40) & 0xFF; + bytes[3] = (n >> 32) & 0xFF; + bytes[4] = (n >> 24) & 0xFF; + bytes[5] = (n >> 16) & 0xFF; + bytes[6] = (n >> 8) & 0xFF; + bytes[7] = n & 0xFF; +} + +unsigned long long bytesToLong(char *bytes) { + unsigned long long value = 0; + int i; + for (i = 0; i < 8; i++) { + int shift = (8 - 1 - i) * 8; + value += (unsigned long long)(((unsigned long long)(bytes[i] & 0xFF)) << shift); + } + return value; +} + +double longToDouble(unsigned long long l) { + double result; + memcpy(&result, &l, 8); + return result; +} + +unsigned long long doubleToLong(double d) { + unsigned long long result; + memcpy(&result, &d, 8); + return result; +} + +void doubleToBytes(double n, char *bytes) { + longToBytes(doubleToLong(n), bytes); +} + +double bytesToDouble(char *bytes) { + return longToDouble(bytesToLong(bytes)); +} + +// FUNCTIONS // + +void pathfinder_serialize(FILE *fp, Segment *trajectory, int trajectory_length) { + char buf_1[4]; + intToBytes(trajectory_length, buf_1); + fwrite(buf_1, 1, 4, fp); + + char buf[8]; + int i; + for (i = 0; i < trajectory_length; i++) { + Segment s = trajectory[i]; + + doubleToBytes(s.dt, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.x, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.y, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.position, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.velocity, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.acceleration, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.jerk, buf); + fwrite(buf, 1, 8, fp); + + doubleToBytes(s.heading, buf); + fwrite(buf, 1, 8, fp); + } +} + +int pathfinder_deserialize(FILE *fp, Segment *target) { + char buf_1[4]; + fread(buf_1, 1, 4, fp); + int length = bytesToInt(buf_1); + + char buf[8]; + + int i; + for (i = 0; i < length; i++) { + fread(buf, 1, 8, fp); + double dt = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double x = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double y = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double position = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double velocity = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double acceleration = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double jerk = bytesToDouble(buf); + + fread(buf, 1, 8, fp); + double heading = bytesToDouble(buf); + + Segment s = { dt, x, y, position, velocity, acceleration, jerk, heading }; + target[i] = s; + } + return length; +} + +void pathfinder_serialize_csv(FILE *fp, Segment *trajectory, int trajectory_length) { + fputs(CSV_LEADING_STRING, fp); + + int i; + for (i = 0; i < trajectory_length; i++) { + char buf[1024]; + Segment s = trajectory[i]; + sprintf(buf, "%f,%f,%f,%f,%f,%f,%f,%f\n", s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading); + fputs(buf, fp); + } +} + +int pathfinder_deserialize_csv(FILE *fp, Segment *target) { + char line[1024]; + int line_n = 0; + int seg_n = 0; + while (fgets(line, 1024, fp)) { + char *tmp = strdup(line); + if (line_n == 0) { } // Do nothing, first line specifies the headers + + char *record; + record = strtok(tmp, ","); + double dt = strtod(record, NULL); + record = strtok(NULL, ","); + double x = strtod(record, NULL); + record = strtok(NULL, ","); + double y = strtod(record, NULL); + record = strtok(NULL, ","); + double pos = strtod(record, NULL); + record = strtok(NULL, ","); + double vel = strtod(record, NULL); + record = strtok(NULL, ","); + double acc = strtod(record, NULL); + record = strtok(NULL, ","); + double jerk = strtod(record, NULL); + record = strtok(NULL, ","); + double head = strtod(record, NULL); + + Segment s = { dt, x, y, pos, vel, acc, jerk, head }; + target[seg_n] = s; + + free(tmp); + + if (line_n != 0) seg_n++; + line_n++; + } + return seg_n; +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/mathutil.cpp b/Pathfinder-Cpp/src/mathutil.cpp new file mode 100644 index 0000000..bdebb4d --- /dev/null +++ b/Pathfinder-Cpp/src/mathutil.cpp @@ -0,0 +1,15 @@ +#include "mathutil.h" + +double bound_radians(double angle) { + double newAngle = fmod(angle, TAU); + if (newAngle < 0) newAngle = TAU + newAngle; + return newAngle; +} + +double r2d(double angleInRads) { + return angleInRads * 180 / PI; +} + +double d2r(double angleInDegrees) { + return angleInDegrees * PI / 180; +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/modifiers/swerve.cpp b/Pathfinder-Cpp/src/modifiers/swerve.cpp new file mode 100644 index 0000000..d92bb08 --- /dev/null +++ b/Pathfinder-Cpp/src/modifiers/swerve.cpp @@ -0,0 +1,37 @@ +#include "modifiers/swerve.h" + +void pf_modify_swerve_default(Segment *original, int length, Segment *front_left, Segment *front_right, + Segment *back_left, Segment *back_right, double wheelbase_width, double wheelbase_depth) { + + int i; + for (i = 0; i < length; i++) { + Segment seg = original[i]; + Segment fl = seg; + Segment fr = seg; + Segment bl = seg; + Segment br = seg; + + fl.x = seg.x - wheelbase_width / 2; + fl.y = seg.y + wheelbase_depth / 2; + fr.x = seg.x + wheelbase_width / 2; + fr.y = seg.y + wheelbase_depth / 2; + + bl.x = seg.x - wheelbase_width / 2; + bl.y = seg.y - wheelbase_depth / 2; + br.x = seg.x + wheelbase_width / 2; + br.y = seg.y - wheelbase_depth / 2; + + front_left[i] = fl; + front_right[i] = fr; + back_left[i] = bl; + back_right[i] = br; + } +} + +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) { + + if (mode == SWERVE_DEFAULT) { + pf_modify_swerve_default(original, length, front_left, front_right, back_left, back_right, wheelbase_width, wheelbase_depth); + } +} diff --git a/Pathfinder-Cpp/src/modifiers/tank.cpp b/Pathfinder-Cpp/src/modifiers/tank.cpp new file mode 100644 index 0000000..15a8846 --- /dev/null +++ b/Pathfinder-Cpp/src/modifiers/tank.cpp @@ -0,0 +1,50 @@ +#include "modifiers/tank.h" + +void pathfinder_modify_tank(Segment *original, int length, Segment *left_traj, Segment *right_traj, double wheelbase_width) { + double w = wheelbase_width / 2; + + int i; + for (i = 0; i < length; i++) { + Segment seg = original[i]; + Segment left = seg; + Segment right = seg; + + double cos_angle = cos(seg.heading); + double sin_angle = sin(seg.heading); + + left.x = seg.x - (w * sin_angle); + left.y = seg.y + (w * cos_angle); + + if (i > 0) { + Segment last = left_traj[i - 1]; + double distance = sqrt( + (left.x - last.x) * (left.x - last.x) + + (left.y - last.y) * (left.y - last.y) + ); + + left.position = last.position + distance; + left.velocity = distance / seg.dt; + left.acceleration = (left.velocity - last.velocity) / seg.dt; + left.jerk = (left.acceleration - last.acceleration) / seg.dt; + } + + right.x = seg.x + (w * sin_angle); + right.y = seg.y - (w * cos_angle); + + if (i > 0) { + Segment last = right_traj[i - 1]; + double distance = sqrt( + (right.x - last.x) * (right.x - last.x) + + (right.y - last.y) * (right.y - last.y) + ); + + right.position = last.position + distance; + right.velocity = distance / seg.dt; + right.acceleration = (right.velocity - last.velocity) / seg.dt; + right.jerk = (right.acceleration - last.acceleration) / seg.dt; + } + + left_traj[i] = left; + right_traj[i] = right; + } +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/spline.cpp b/Pathfinder-Cpp/src/spline.cpp new file mode 100644 index 0000000..4056d8f --- /dev/null +++ b/Pathfinder-Cpp/src/spline.cpp @@ -0,0 +1,90 @@ +#include "spline.h" + +Coord pf_spline_coords(Spline s, double percentage) { + percentage = MAX(MIN(percentage, 1), 0); + double x = percentage * s.knot_distance; + double y = (s.a*x + s.b) * (x*x*x*x) + (s.c*x + s.d) * (x*x) + s.e*x; // Heh, sex + + double cos_theta = cos(s.angle_offset); + double sin_theta = sin(s.angle_offset); + + Coord c = { + x * cos_theta - y * sin_theta + s.x_offset, + x * sin_theta + y * cos_theta + s.y_offset + }; + return c; +} + +double pf_spline_deriv(Spline s, double percentage) { + double x = percentage * s.knot_distance; + return (5*s.a*x + 4*s.b) * (x*x*x) + (3*s.c*x + 2*s.d) * x + s.e; +} + +double pf_spline_deriv_2(double a, double b, double c, double d, double e, double k, double p) { + double x = p * k; + return (5*a*x + 4*b) * (x*x*x) + (3*c*x + 2*d) * x + e; +} + +double pf_spline_angle(Spline s, double percentage) { + return bound_radians(atan(pf_spline_deriv(s, percentage)) + s.angle_offset); +} + +double pf_spline_distance(Spline *s, int sample_count) { + double sample_count_d = (double) sample_count; + + double a = s->a; double b = s->b; double c = s->c; + double d = s->d; double e = s->e; double knot = s->knot_distance; + + double arc_length = 0, t = 0, dydt = 0; + + double deriv0 = pf_spline_deriv_2(a, b, c, d, e, knot, 0); + + double integrand = 0; + double last_integrand = sqrt(1 + deriv0*deriv0) / sample_count_d; + + int i; + for (i = 0; i <= sample_count; i = i + 1) { + t = i / sample_count_d; + dydt = pf_spline_deriv_2(a, b, c, d, e, knot, t); + integrand = sqrt(1 + dydt*dydt) / sample_count_d; + arc_length += (integrand + last_integrand) / 2; + last_integrand = integrand; + } + double al = knot * arc_length; + s->arc_length = al; + return al; +} + +double pf_spline_progress_for_distance(Spline s, double distance, int sample_count) { + double sample_count_d = (double) sample_count; + + double a = s.a; double b = s.b; double c = s.c; + double d = s.d; double e = s.e; double knot = s.knot_distance; + + double arc_length = 0, t = 0, dydt = 0, last_arc_length = 0; + + double deriv0 = pf_spline_deriv_2(a, b, c, d, e, knot, 0); + + double integrand = 0; + double last_integrand = sqrt(1 + deriv0*deriv0) / sample_count_d; + + distance /= knot; + + int i; + for (i = 0; i <= sample_count; i = i + 1) { + t = i / sample_count_d; + dydt = pf_spline_deriv_2(a, b, c, d, e, knot, t); + integrand = sqrt(1 + dydt*dydt) / sample_count_d; + arc_length += (integrand + last_integrand) / 2; + if (arc_length > distance) break; + last_integrand = integrand; + last_arc_length = arc_length; + } + + double interpolated = t; + if (arc_length != last_arc_length) { + interpolated += ((distance - last_arc_length) + / (arc_length - last_arc_length) - 1) / sample_count_d; + } + return interpolated; +} \ No newline at end of file diff --git a/Pathfinder-Cpp/src/trajectory.cpp b/Pathfinder-Cpp/src/trajectory.cpp new file mode 100644 index 0000000..b9c4e20 --- /dev/null +++ b/Pathfinder-Cpp/src/trajectory.cpp @@ -0,0 +1,111 @@ +#include "trajectory.h" + +void pf_trajectory_copy(Segment *src, Segment *dest, int length) { + int i; + for (i = 0; i < length; i++) { + Segment s = src[i]; + Segment d; + + d.dt = s.dt; + d.x = s.x; + d.y = s.y; + d.position = s.position; + d.velocity = s.velocity; + d.acceleration = s.acceleration; + d.jerk = s.jerk; + d.heading = s.heading; + + dest[i] = d; + } +} + +TrajectoryInfo pf_trajectory_prepare(TrajectoryConfig c) { + double max_a2 = c.max_a * c.max_a; + double max_j2 = c.max_j * c.max_j; + + double checked_max_v = MIN(c.max_v, + (-(max_a2) + sqrt(max_a2 * max_a2 + 4 * (max_j2 * c.max_a * c.dest_pos))) + / (2 * c.max_j) + ); + + int filter1 = (int) ceil((checked_max_v / c.max_a) / c.dt); + int filter2 = (int) ceil((c.max_a / c.max_j) / c.dt); + + double impulse = (c.dest_pos / checked_max_v) / c.dt; + int time = (int) ceil(filter1 + filter2 + impulse); + + TrajectoryInfo info = { filter1, filter2, time, c.dt, 0, checked_max_v, impulse }; + return info; +} + +int pf_trajectory_create(TrajectoryInfo info, TrajectoryConfig c, Segment *seg) { + int ret = pf_trajectory_fromSecondOrderFilter(info.filter1, info.filter2, info.dt, info.u, info.v, info.impulse, info.length, seg); + + if (ret < 0) { + return ret; + } + + double d_theta = c.dest_theta - c.src_theta; + int i; + for (i = 0; i < info.length; i++) { + seg[i].heading = c.src_theta + d_theta * (seg[i].position) / (seg[info.length - 1].position); + } + return 0; +} + +int pf_trajectory_fromSecondOrderFilter(int filter_1_l, int filter_2_l, + double dt, double u, double v, double impulse, int len, Segment *t) { + Segment last_section = {dt, 0, 0, 0, u, 0, 0}; + + if (len < 0) { + // Error + return -1; + } + + // double f1_buffer[len]; + double *f1_buffer = (double *) malloc(len * sizeof(double)); // VS doesn't support VLAs + f1_buffer[0] = (u / v) * filter_1_l; + double f2; + + int i; + for (i = 0; i < len; i++) { + double input = MIN(impulse, 1); + if (input < 1) { + input -= 1; + impulse = 0; + } else { + impulse -= input; + } + + double f1_last; + + if (i > 0) f1_last = f1_buffer[i - 1]; + else f1_last = f1_buffer[0]; + + f1_buffer[i] = MAX(0.0, MIN(filter_1_l, f1_last + input)); + + f2 = 0; + int j; + for (j = 0; j < filter_2_l; ++j) { + if (i - j < 0) break; + + f2 += f1_buffer[i - j]; + } + f2 = f2 / filter_1_l; + + t[i].velocity = f2 / filter_2_l * v; + + t[i].position = (last_section.velocity + t[i].velocity) / 2.0 * dt + last_section.position; + + t[i].x = t[i].position; + t[i].y = 0; + + t[i].acceleration = (t[i].velocity - last_section.velocity) / dt; + t[i].jerk = (t[i].acceleration - last_section.acceleration) / dt; + t[i].dt = dt; + + last_section = t[i]; + } + free(f1_buffer); + return 0; +} \ No newline at end of file From 2b8075f8052e5e6b642486c8d75ea8c883d2a615 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Fri, 1 Dec 2017 20:16:10 -0500 Subject: [PATCH 2/3] Update readmes --- Pathfinder-Cpp/README.md | 6 ++++++ README.md | 1 + 2 files changed, 7 insertions(+) create mode 100644 Pathfinder-Cpp/README.md diff --git a/Pathfinder-Cpp/README.md b/Pathfinder-Cpp/README.md new file mode 100644 index 0000000..c68da40 --- /dev/null +++ b/Pathfinder-Cpp/README.md @@ -0,0 +1,6 @@ +# Pathfinder C++ Port +This is a port of the C core libraries and uses the same API. 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. \ No newline at end of file diff --git a/README.md b/README.md index 0831e10..a6c2ab3 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ Cross-Platform, Multi-Use Motion Profiling and Trajectory Generation. Pathfinder is a library for generating Motion Profiles, a way to smoothly fit and follow a trajectory based upon given waypoints. Currently, both a C and Java API are available, but can be applied to almost any application. +There also exists a C++ port which can be used by copying the files into your C++ project. An example profile is given below, with the waypoints: 1) X = -4, Y = -1, Angle = -45 degrees From 2f994c25d1975046657ca284d195ab9634eb4eff Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Fri, 1 Dec 2017 20:18:39 -0500 Subject: [PATCH 3/3] Add difference between cpp and c libraries --- Pathfinder-Cpp/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Pathfinder-Cpp/README.md b/Pathfinder-Cpp/README.md index c68da40..e5a63f9 100644 --- a/Pathfinder-Cpp/README.md +++ b/Pathfinder-Cpp/README.md @@ -1,5 +1,6 @@ # Pathfinder C++ Port -This is a port of the C core libraries and uses the same API. The purpose +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