[Glitch159]

First of all, that entire document has been read in it's entirety, and we made sure that we added in everything that was specified that we would need. Second of all, here is the code:

package org.usfirst.frc.team839.robot.commands;

import org.usfirst.frc.team839.robot.instrumentation;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.command.Command;

public class GoodMotionProfileCommand extends Command
{

	CANTalon _talon = new CANTalon(5);
	CANTalon _talonSlave = new CANTalon(4);
	
	//MotionProfileExample _example = new MotionProfileExample(_talon);
	private CANTalon.MotionProfileStatus _status = new CANTalon.MotionProfileStatus();

	boolean isFinished = false;
	double[][] profile;
	public GoodMotionProfileCommand(double[][] points)
	{
		System.out.println("GoodMotionProfileCommand constructor");
		profile = points;
	}

	@Override
	protected void initialize()
	{

		System.out.println("GoodMotionProfileCommand initialize");

		_talon.clearMotionProfileTrajectories();
		_talon.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
		_talon.reverseSensor(false);
		
		_talon.changeControlMode(TalonControlMode.MotionProfile);
//		_talonSlave.changeControlMode(TalonControlMode.Follower);
//		_talonSlave.set(5);
		
		_talon.set(CANTalon.SetValueMotionProfile.Disable.value);
		_talon.changeMotionControlFramePeriod(10);
		
		//feed Trajectory Points
		fill(profile,profile.length);
		_talon.set(CANTalon.SetValueMotionProfile.Enable.value);
//		_talon.processMotionProfileBuffer();
	}

	private void fill(double[][] profile, int totalCnt) {

		System.out.println("GoodMotionProfileCommand fill");
		/* create an empty point */
		CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint();

		for (int i = 0; i < totalCnt; ++i) {
			/* for each point, fill our structure and pass it to API */
			point.position = profile[i][0];
			point.velocity = profile[i][1];
			point.timeDurMs = (int) profile[i][2];
			point.profileSlotSelect = 0; /* which set of gains would you like to use? */
			point.velocityOnly = true; /* set true to not do any position
										 * servo, just velocity feedforward
										 */
			point.zeroPos = false;
			if (i == 0)
				point.zeroPos = true; /* set this to true on the first point */

			point.isLastPoint = false;
			if ((i + 1) == totalCnt)
				point.isLastPoint = true; /* set this to true on the last point  */

			_talon.pushMotionProfileTrajectory(point);
		}
	}
	

	
	@Override
	protected void execute()
	{
		_talon.getMotionProfileStatus(_status);

		if(_status.hasUnderrun)
		{
			System.out.println("GoodMotionProfileCommand _status.hasUnderrun");

			//load points
			_talon.clearMotionProfileHasUnderrun();;
			_talon.processMotionProfileBuffer();
		}
		
		if (_status.activePointValid && _status.activePoint.isLastPoint)
		{
			System.out.println("GoodMotionProfileCommand  _status.activePoint.isLastPoint");
			_talon.set(CANTalon.SetValueMotionProfile.Hold.value);
			isFinished = true;

		}
		else
		{
			instrumentation.process(_status);
			_talon.set(CANTalon.SetValueMotionProfile.Enable.value);
			_talon.processMotionProfileBuffer();
		}

	}
	@Override
	protected boolean isFinished()
	{
		return isFinished;
	}

	@Override
	protected void end()
	{
		System.out.println("GoodMotionProfileCommand end");
		_talon.set(CANTalon.SetValueMotionProfile.Hold.value);
		_talon.clearMotionProfileTrajectories();
		_talon.set(CANTalon.SetValueMotionProfile.Disable.value);
		_talon.changeControlMode(TalonControlMode.Voltage);
	}

	@Override
	protected void interrupted()
	{
		
	}
}