Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive

[Ben Wolsieffer]

I was in a hurry when I wrote my last post (time restricted unbag periods are mixed blessing), so I'll try to explain the problem I am running into better now.

I was generating this waypoint sequence:

public static final Waypoint[] LOW_BAR_AUTONOMOUS_WAYPOINTS =
            { new Waypoint(0.5, 0.5, 0), new Waypoint(1, 0.5, Math.toRadians(45)) };

using this config (acceleration and max velocity are very low for testing):

public static final Trajectory.Config AUTONOMOUS_TRAJECTORY_CONFIG = new Config(Trajectory.FitMethod.HERMITE_CUBIC,
            Trajectory.Config.SAMPLES_HIGH, DRIVE_SUBSYSTEM_TRAJECTORY_PERIOD, 0.2, 0.5, 40);

When the trajectory was generated, it seemed to ignore the max acceleration and velocity values. The left and right trajectories accelerate extremely quickly, reaching 25 m/s within 0.01 seconds (also, jerk was around 24000 ). The speed maxes out at 25 m/s. I never checked the source trajectory, so I don't know if the it had the correct velocity values.

I just realized that I was not using the real wheelbase width, only a dummy value (0.5 m), but this is still reasonable and should not have caused this problem.