FollowProfile.java

package org.usfirst.frc.team236.robot.commands.autonomous;

import org.usfirst.frc.team236.robot.Robot;
import org.usfirst.frc.team236.robot.RobotMap;
import org.usfirst.frc.team236.robot.Updater;
import org.usfirst.frc.team236.robot.motionProfile.Profile;
import org.usfirst.frc.team236.robot.motionProfile.ProfileFollower;
import org.usfirst.frc.team236.robot.subsystems.Drive;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class FollowProfile extends Command {

	Profile leftProfile, rightProfile;
	Drive d = Robot.drive;
	ProfileFollower left, right;
	boolean invertLeft, invertRight;

	public FollowProfile(Profile leftProfile, Profile rightProfile,
			boolean invertLeft, boolean invertRight) {
		requires(d);
		this.leftProfile = leftProfile;
		this.rightProfile = rightProfile;
		this.invertLeft = invertLeft;
		this.invertRight = invertRight;
		// Use requires() here to declare subsystem dependencies
		// eg. requires(chassis);
	}

	// Called just before this Command runs the first time
	protected void initialize() {
		if(!(leftProfile == null || rightProfile == null)){
			System.out.println("follow profile");
			Robot.drive.zeroEncoders();
			left = new ProfileFollower(leftProfile, d.left, d.left,
					RobotMap.drive_kP, RobotMap.drive_kV, RobotMap.drive_kA,
					invertLeft);
			right = new ProfileFollower(rightProfile, d.right, d.right,
					RobotMap.drive_kP, RobotMap.drive_kV, RobotMap.drive_kA,
					invertRight);
			Updater.getInstance().addThreadedUpdatable(left);
			Updater.getInstance().addThreadedUpdatable(right);
			left.isEnabled = true;
			right.isEnabled = true;
		}else {
			System.out.println("null profile");
		}
		
	}

	// Called repeatedly when this Command is scheduled to run
	protected void execute() {
	}

	// Make this return true when this Command no longer needs to run execute()
	protected boolean isFinished() {
		//System.out.println(left.source.getValue());
		return left.source.getValue() < -16.3;
	}

	// Called once after isFinished returns true
	protected void end() {
		left.isEnabled = false;
		right.isEnabled = false;
	}

	// Called when another command which requires one or more of the same
	// subsystems is scheduled to run
	protected void interrupted() {
		left.isEnabled = false;
		right.isEnabled = false;
	}
}