ProfileGenerator.java

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

import org.usfirst.frc.team236.robot.ProfileParameters;
/**
 * Generate a motion profile.  
 * @author team236
 *
 */
public class ProfileGenerator {
	//dt is resolution of the profile
	//if the generated profile is longer than maxTime, it stops.
	private static final double dt = .02, maxTime = 30;
	
	//different stages of the first half of the profile - see notebook
	private static final int ACC_RAMP_UP = 0, CONSTANT_POSITIVE_ACC = 1, ACC_MIRROR = 2, CONSTANT_VELOCITY = 3;
	
	public static Profile generate(ProfileParameters p){
		return generate(p.distance, p.velocity, p.acceleration, p.jerk);
	}

	public static Profile generate(double displacement, double max_velocity,
			double max_acceleration, double max_jerk) {
		
		//the profile will be a mirror image, so we should find the halfway point to know where to stop
		double midpoint = displacement/2;
		//also need to know the velocity midpoint
		double velocityMidpoint = max_velocity/2;
		
		boolean atMidpoint = false;
		int stage = 0;
		int j = 0; //mirror counter
		boolean jSet = false;
	
		
		//start by adding a (0, 0) element
		Profile p = new Profile();
		Element initialElement = new Element();
		initialElement.acceleration = 0;
		initialElement.jerk = 0;
		initialElement.position = 0;
		initialElement.velocity = 0;
		
		p.addElement(initialElement);
		
		//using max time, calculate the max number of times the generator should run before giving up.
		int maxSteps = (int)(maxTime/dt);
		//loop for generating profile
		for(int i = 0; i < maxSteps; i++){
			//check to see if we've gotten to the midpoint
			if(p.get(i).position > midpoint){
				//if so, remember that we got to the midpoint
				atMidpoint = true;
				//and exit the loop
				break;
			}
			
			//once our acceleration reaches maximum, move on
			if(p.get(i).acceleration >= max_acceleration){
				stage = CONSTANT_POSITIVE_ACC;
			}
			
			//one our velocity reaches halfway, move on
			if(p.get(i).velocity >= velocityMidpoint){
				stage = ACC_MIRROR;
				//configure mirror counter
				if(!jSet){
					j = p.size() - 1;
					jSet = true;
				}
				
			}
			
			//once we've finished mirroring the acc. trapezoid, move on.
			if(j < 0){
				stage = CONSTANT_VELOCITY;
			}
//			
			
//			if(p.get(i).velocity >= max_velocity){
//				stage = CONSTANT_VELOCITY;
//			}
			
			//create empty element to add
			Element e = new Element();
			//check to see if we're in the ramp up acc. stage
			if(stage == ACC_RAMP_UP){
				//if so, do math
				e.jerk = max_jerk;
				e.acceleration = p.get(i).acceleration + max_jerk * dt;
				e.velocity = p.get(i).velocity + e.acceleration * dt;
				e.position = p.get(i).position + e.velocity * dt;
			}
			//check to see if we're in the positive, but constant, acc. stage
			if(stage == CONSTANT_POSITIVE_ACC){
				//if so, do math
				e.jerk = 0;
				e.acceleration = max_acceleration;
				e.velocity = p.get(i).velocity + e.acceleration * dt;
				e.position = p.get(i).position + e.velocity * dt;
			}
			
			//check to see if we're in the process of mirroring acc.
			if(stage == ACC_MIRROR){
				//if so do math
				e.jerk = -p.get(j).jerk;
				e.acceleration = p.get(i).acceleration + e.jerk * dt;
				e.velocity = p.get(i).velocity + e.acceleration * dt;
				e.position = p.get(i).position + e.velocity * dt;
				//decrement mirror counter
				j--;
				//System.out.println(j + "    " + -p.get(j).jerk);
			}
			//check to see if we're in constant velocity
			if(stage == CONSTANT_VELOCITY){
				//if so, do math
				e.jerk = 0;
				e.acceleration = 0;
				e.velocity = p.get(i).velocity;
				e.position = p.get(i).position + e.velocity * dt;
			}
			//add!
			p.addElement(e);
			
			//System.out.println(e.position);
			
		}
		
		//if we break from the loop and we haven't reached the midpoint, something really has gone wrong.
		if(!atMidpoint){
			System.out.println("Profile Generation Failed!");
			return p;
		}
		//mirror everything else
		for(int k = p.size() - 1; k >= 0; k--){
			Element e = new Element();
			//e.jerk = p.get(k).jerk;
			int pSize = p.size() - 1;
			e.acceleration = -p.get(k).acceleration;
			e.velocity = p.get(pSize).velocity + e.acceleration * dt;
			e.position = p.get(pSize).position + e.velocity * dt;
			p.addElement(e);
		}
		
		

		return p;
	}
	
	
}