nrg948 / nrgrobot2019 Goto Github PK

We have a start on PathFinder/PathWeaver integration, but the code is commented out. To finish it out and get it a point where we can test it, we need to make some small changes, add a new command, FollowTrajectory, and derive two classes from it: FollowPathWeaverFile and FollowWaypoints.

1. Uncomment the code in Drive, i.e. lines 16-21, 46-47 & 109 - 142.
2. In Drive.followTrajectoryInit:
   a. Change the method to take a Trajectory parameter instead of a String. The method signature should now be public void followTrajectoryInit(Trajectory trajectory).
   b. Delete lines 110-111, but save them someplace because well use the same statements in the FollowTrajectory command to load the PathWeaver-generated file.
   c. Rename the variable pathTrajectory.
3. Create a new command, FollowTrajectory:
   a. The constructor will take a Trajectory object as a parameter and store it in a member variable named trajectory.
   b. The initialize method will call the Drive.followTrajectoryInit method passing trajectory as its parameter,
   c. The execute method will call the Drive.followTrajectoryExecute method.
   d. The isFinished method will return the result of the Drive.followTrajectoryIsFinished method.
   e. The end method will call the Drive.followTrajectoryEnd method.
   f. The interrupted method will call the end method as usual.
4. Create a new command, FollowPathWeaverFile derived from FollowTrajectory
   a. Create a private static Trajectory LoadTrajectoryFromCsvFile(String filename) method that loads the PathWeaver file, and creates and returns Trajectory object (using the Drive.java lines 110-111 that we removed previously) .
   b. The constructor will take a String parameter which is the name of a PathWeaver file that we'll create and place in the deploy directory of the project.
   c. The constructor will call LoadTrajectoryFromCsvFile passing the returned Trajectory object to base class constructor (accessible through the super keyword).
5. Create a new command, FollowWaypoints:
   a. Create a private static Trajectory CreateTrajectory(Waypoint[] waypoints) method that creates and returns a Trajectory object from the array of jaci.pathfinder.Waypoint object. See Pathfinder Java Wrapper for an example. We'll need to create constants for the time step, max velocity, max acceleration and max jerk parameters. Make a best guess now, and we'll work them out empirically with the robot later. Store the Trajectory object in a trajectory member variable.
   b. The constructor will take an array of jaci.pathfinder.Waypoint object.
   c. The constructor will call LoadTrajectoryFromCsvFile passing the returned Trajectory object to base class constructor (accessible through the super keyword).
6. Create and instances of the two commands derived from FollowTrajectory and attach them to buttons or put them to the SmartDashboard.

We've calibrated the drive encoders to inches, so keep that in mind when creating the Waypoint array and PathWeaver files.