Need functionality in motion control system to follow Pathfinder trajectory

[RachelLucyshyn]

Robot Code Vision Interface needs functionality in motion control system to follow the trajectory generated by our Pathfinder code. We are thinking of a method such as “followTraj(traj)“ where traj is a Trajectory object generated by Pathfinder. A Trajectory object contains an array of Segment objects where each Segment object has the following double attributes:

traj.segments[i].dt             // time since start of trajectory (sec)
traj.segments[i].x              // x coordinate in robot frame (dist)
traj.segments[i].y              // y coordinate in robot frame (dist)
traj.segments[i].pos            // distance along trajectory (dist)
traj.segments[i].velocity       // velocity along trajectory (dist/sec)
traj.segments[i].acceleration   // acceleration along trajectory (dist/sec^2)
traj.segments[i].jerk           // jerk along trajectory (dist/sec^3)
traj.segments[i].heading        // angle of tangent to trajectory with respect  
                                // to x axis in robot frame (rad)

It the above descriptions of the units of measurement, "dist" represents the
measurement is used for distance (e.g. feet, inches, metres, centimetres, etc.)
which at this moment is TBD. The units will be decided shortly according to
what is most compatible with the motion control system and the vision system.

It is assumed that the robot frame is located at the centre of the robot base
with the y axis is pointing to the front of the robot and the x axis pointing 
to the right of the robot when looking from the back to the front.
It is assumed that this frame is compatible with that used by the motion
control system. (Please advise us if this is not correct.)

The x and y attributes are in the robot frame as positioned and oriented
at the start of the trajectory.

In order to test the trajectory following functionality, it will be easiest to have your test code generate a trajectory using Pathfinder. To install Pathfinder, go to Jaci’s Pathfinder: https://github.com/JacisNonsense/Pathfinder

Here is some sample test code that can be used. It will generate a trajectory that starts from the origin with the robot at 0 degrees to the horizontal and will end up at position (x=3, y=5) at 45 degrees to the horizontal. Note that Pathfinder wants angles in radians so that the Pathfinder.d2r(45) converts 45 degrees to the value in radians.

package frc.robot;

import jaci.pathfinder.Pathfinder; import jaci.pathfinder.Trajectory; import jaci.pathfinder.Waypoint;

public class Basic {

public void testTraj() {
    System.out.println("RCVI: testTraj(): Entered");
    Trajectory.Config config = 
        new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC,
            Trajectory.Config.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0);
    Waypoint[] points = new Waypoint[] {
        new Waypoint(0, 0, 0),
        new Waypoint(3,5,Pathfinder.d2r(45))
    };

    Trajectory traj = Pathfinder.generate(points, config);

    // Here is the method followTraj we are asking you to write
    SomeRobotClass.followTraj(traj)   

}

[ryanlarkin]

Closed by #160