ProfileFollower.java

package org.usfirst.frc.team236.robot.motionProfile;

import org.usfirst.frc.team236.robot.PID;
import org.usfirst.frc.team236.robot.Updatable;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
 * Control loop which applies a profile to a motor, using proportional position feedback
 *  and velocity + acceleration feedforward control.
 * @author team236
 *
 */
public class ProfileFollower implements Updatable{
	//profile which will be follower
	Profile p;
	//which step of the profile the robot is currently at
	private int i = 0;
	//source for feedback
	public ProfileFollowerSource source;
	//output for calculated motor power
	ProfileFollowerOutput output;
	//feedback + feedforward constants
	double kV, kA, kP, endPosition;
	public volatile boolean isEnabled;
	public volatile boolean onTarget;
	//indicates if the robot should drive backward
	private boolean invert;
	public volatile boolean isOld = false;
	public ProfileFollower(Profile p, ProfileFollowerSource source, ProfileFollowerOutput output, double kP, double kV, double kA, boolean invert){
		this.p = p;
		this.source = source;
		this.output = output;
		this.kV = kV;
		this.kA = kA;
		this.kP = kP;
		this.invert = invert;
		//set end position inverted if needed.
		endPosition = p.get(p.size() - 1).position * (invert ? -1 : 1);
	}
	
	@Override
	public void update() {
		i = (int)PID.coerce(i, 0, p.size() - 1);
		int invertFactor = invert ? -1 : 1;
		double position = p.get(i).position * invertFactor;
		//see if the robot is at its final target by comparing the current position 
		//to the end position
		onTarget = Math.abs(endPosition + source.getValue()) < .1;
		//System.out.println(endPosition + " " + source.getValue());
		//make sure the step of the profile we're at is within the length of the profile.
		
		if(isEnabled){
			//calculate acceleration and velocity feedforward
			double a = kA * p.get(i).acceleration * invertFactor;
			double v = kV * p.get(i).velocity * invertFactor;
			//calculate proportional position correction 
			double correction = kP * (-position - source.getValue());
			output.setValue((a + v - correction));
			SmartDashboard.putNumber("sp", position);
			i++;
		}
		
		
	}
	@Override
	public boolean isOld(){
		return isOld;
	}
	
}