diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java
index 96a6f89..d86f53c 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java
@@ -8,7 +8,7 @@ package org.usfirst.frc4388.robot;
  */
 
 public class Constants {
-
+    public static double kLooperDt = 0.01;
     public static double kDriveWheelDiameterInches = 6.04; 
     public static double kTrackLengthInches = 10;
     public static double kTrackWidthInches = 26.5;
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
index 4e26054..1f3669c 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
@@ -32,7 +32,6 @@ public class Robot extends IterativeRobot
 
 
  
-	public static final Elevator elevator = new Elevator();
 	public static final BallIntake ballIntake = new BallIntake();
     
 
@@ -58,7 +57,6 @@ public class Robot extends IterativeRobot
 		oi = OI.getInstance();
 		
     	controlLoop.addLoopable(drive);
-    	controlLoop.addLoopable(elevator);
 			
 
         operationModeChooser = new SendableChooser<OperationMode>();
@@ -144,7 +142,6 @@ public class Robot extends IterativeRobot
     
     public void updateStatus() {
     	drive.updateStatus(operationMode);
-    	elevator.updateStatus(operationMode);
 
    }
 
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java
deleted file mode 100644
index e205818..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java
+++ /dev/null
@@ -1,60 +0,0 @@
-// RobotBuilder Version: 2.0
-//
-// This file was generated by RobotBuilder. It contains sections of
-// code that are automatically generated and assigned by robotbuilder.
-// These sections will be updated in the future when you export to
-// Java from RobotBuilder. Do not put any code or make any change in
-// the blocks indicating autogenerated code or it will be lost on an
-// update. Deleting the comments indicating the section will prevent
-// it from being updated in the future.
-
-
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-
-public class DriveStraightMP extends Command {
-    private double m_distanceInches;
-    private double m_maxVelocityInchesPerSec;
-    private boolean m_useGyroLock;
-    private boolean m_useAbsolute;
-    private double m_desiredAbsoluteAngle;
- 
-    public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
-        requires(Robot.drive);
-        m_distanceInches = distanceInches;
-        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
-        m_useGyroLock = useGyroLock;
-        m_useAbsolute = useAbsolute;
-        m_desiredAbsoluteAngle = desiredAbsoluteAngle;
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
-		Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle);
-    }
-
-    // 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() {
-		return Robot.drive.isFinished(); 
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    	end();
-    }
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java
index 82223d9..a74f364 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java
@@ -4,7 +4,7 @@ import org.usfirst.frc4388.robot.Constants;
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
 import org.usfirst.frc4388.utility.BHRMathUtils;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.TalonSRXEncoder;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java
deleted file mode 100644
index df2b718..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java
+++ /dev/null
@@ -1,99 +0,0 @@
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.robot.Constants;
-import org.usfirst.frc4388.robot.Robot;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-
-public class ElevatorBasic extends Command {
-    private double m_targetHeightInchesAboveFloor;
-    private double m_speed;
-    private boolean m_goingUp;
-    private double m_commandInitTimestamp;
-	private double m_lastCommandExecuteTimestamp = 0.0;
-	private double m_lastCommandExecutePeriod = 0.0;
-	public static boolean isfinishedElevatorBasic;
- 
-    public ElevatorBasic(double targetHeightInchesAboveFloor) {
-        requires(Robot.elevator);
-        m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
-   }
-
-    // Called just before this Command runs the first time
-    protected void initialize() 
-    {
-    	Robot.elevator.setControlMode(DriveControlMode.RAW);
-    	
-    	double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
-    	// start out at half speed, as crude way to reduce slippage
-    	m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight);
-System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN"));
-    	if (m_goingUp) {
-    		m_speed = Constants.kElevatorBasicPercentOutputUp;
-    	}
-    	else {
-    		m_speed = Constants.kElevatorBasicPercentOutputDown;
-    	}
-		m_commandInitTimestamp = Timer.getFPGATimestamp();
-		
-    }
-
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
-		// Measure *actual* update period
-		double currentTimestamp = Timer.getFPGATimestamp();
-		if (m_lastCommandExecuteTimestamp > 0.001)	{	// ie, if this is NOT the first time
-			m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
-		}
-		m_lastCommandExecuteTimestamp = currentTimestamp;
-    	Robot.elevator.rawSetOutput(m_speed);
-		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
-    }
-
-    // Make this return true when this Command no longer needs to run execute()
-    protected boolean isFinished() {
-    	boolean finished=false;
-    	double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
-    	double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0);
-System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor);
-    	if (remaining < 0.0) {
-    		finished = true;
-    		
-    	}
-    	/*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
-    		velocity = m_maxVelocityInchesPerSec / 4.0;		// quarter speed
-    	} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) {		// last 300 ms
-    		velocity = m_maxVelocityInchesPerSec / 2.0;		// half speed
-    	}*/
-    		
-    		
-    	if (!finished) {
-    		SmartDashboard.putNumber("EB Dist", currentHeight);
-    	} else {
-    		SmartDashboard.putNumber("EB finDist", currentHeight);
-    	}
-		return finished;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-		double currentTimestamp = Timer.getFPGATimestamp();
-    	SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp);
-    	
-    	isfinishedElevatorBasic = isFinished();
-    	
-    	Robot.elevator.rawSetOutput(0.0);
-    	
-		Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    	end();
-    }
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java
index fd9ea62..483cc62 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java
@@ -3,8 +3,8 @@ package org.usfirst.frc4388.robot.subsystems;
 import org.usfirst.frc4388.robot.RobotMap;
 import org.usfirst.frc4388.robot.commands.*;
 import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
-import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.Looper;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
 import org.usfirst.frc4388.robot.OI;
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java
index 7adcb4c..6a54225 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java
@@ -12,10 +12,10 @@ import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.RobotMap;
 import org.usfirst.frc4388.robot.commands.*;
 import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
-import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.Looper;
 import org.usfirst.frc4388.utility.PIDParams;
-import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
+import org.usfirst.frc4388.utility.SoftwarePIDController;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
index 47711c4..5535671 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
@@ -8,11 +8,11 @@ import org.usfirst.frc4388.robot.Constants;
 import org.usfirst.frc4388.robot.OI;
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.RobotMap;
-import org.usfirst.frc4388.utility.AdaptivePurePursuitController;
+import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController;
 import org.usfirst.frc4388.utility.BHRMathUtils;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
+//import org.usfirst.frc4388.utility.CANTalonEncoder;
 import org.usfirst.frc4388.utility.ControlLoopable;
-import org.usfirst.frc4388.utility.Kinematics;
+import org.usfirst.frc4388.utility.control.Kinematics;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 import org.usfirst.frc4388.utility.MPTalonPIDController;
@@ -22,12 +22,12 @@ import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController;
 import org.usfirst.frc4388.utility.MotionProfilePoint;
 //import org.usfirst.frc4388.utility.MotionProfileBoxCar;
 import org.usfirst.frc4388.utility.PIDParams;
-import org.usfirst.frc4388.utility.Path;
-import org.usfirst.frc4388.utility.PathGenerator;
-import org.usfirst.frc4388.utility.RigidTransform2d;
-import org.usfirst.frc4388.utility.Rotation2d;
+//import org.usfirst.frc4388.utility.Path;
+//import org.usfirst.frc4388.utility.PathGenerator;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
 import org.usfirst.frc4388.utility.SoftwarePIDController;
-import org.usfirst.frc4388.utility.Translation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.ControlMode;
@@ -373,12 +373,33 @@ public class Drive extends Subsystem implements ControlLoopable
 	//	}
 	//}
 	
+
+
+
+
+
+
+
+
+
+
+	/*
 	public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
 		double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
 		mpStraightController.setPID(mpStraightPIDParams);
 		mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); 
 		setControlMode(DriveControlMode.MP_STRAIGHT);
 	}
+*/
+
+
+
+
+
+
+
+
+
 		//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
 	//	double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
 	//	mpStraightController.setPID(mpStraightPIDParams);
@@ -416,7 +437,7 @@ public class Drive extends Subsystem implements ControlLoopable
 		pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
 		setControlMode(DriveControlMode.PID_TURN);
 	}
-
+/*
 	public void setPathMP(PathGenerator path) {
 		mpPathController.setPID(mpPathPIDParams);
 		mpPathController.setMPPathTarget(path); 
@@ -438,7 +459,7 @@ public class Drive extends Subsystem implements ControlLoopable
     	adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); 
 		setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
     }
-
+*/
     public void setDriveHold(boolean status) {
 		if (status) {
 			setControlMode(DriveControlMode.HOLD);
@@ -469,13 +490,15 @@ public class Drive extends Subsystem implements ControlLoopable
      * 
      * @return Set of Strings with Path Markers that the robot has crossed.
      */
+
+	 /*
     public synchronized Set<String> getPathMarkersCrossed() {
         if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) {
             return null;
         } else {
             return adaptivePursuitController.getMarkersCrossed();
         }
-    }
+    }*/
 
     public synchronized void setControlMode(DriveControlMode controlMode) {
  		this.controlMode = controlMode;
@@ -497,6 +520,16 @@ public class Drive extends Subsystem implements ControlLoopable
 			leftDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
 			rightDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
 		}
+
+
+
+
+
+
+
+
+
+		/*
 		else if (controlMode == DriveControlMode.HOLD) {
 			mpStraightController.setPID(mpHoldPIDParams);
 			//leftDrive1.changeControlMode(TalonControlMode.Position);
@@ -509,7 +542,7 @@ public class Drive extends Subsystem implements ControlLoopable
 			leftDrive1.set(0);
 			//rightDrive1.setSelectedSensorPosition(0, 0, 0);	//not needed for spark?  TODO: verify want 0="Primary closed-loop", with no timeout
 			rightDrive1.set(0);
-		}
+		}*/
 		isFinished = false;
 	}
 	
@@ -538,13 +571,13 @@ public class Drive extends Subsystem implements ControlLoopable
 			else if (controlMode == DriveControlMode.MP_PATH) {
 				isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); 
 			}
-			else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
+			/*else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
 				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
 			}
 			else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
 				updatePose();
 				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
-			}
+			}*/
 		}
 	}
 	
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java
deleted file mode 100644
index 933ed93..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java
+++ /dev/null
@@ -1,450 +0,0 @@
-package org.usfirst.frc4388.robot.subsystems;
-
-import java.util.ArrayList;
-
-import org.usfirst.frc4388.controller.XboxController;
-import org.usfirst.frc4388.robot.Constants;
-import org.usfirst.frc4388.robot.Robot;
-import org.usfirst.frc4388.robot.RobotMap;
-import org.usfirst.frc4388.robot.commands.*;
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
-import org.usfirst.frc4388.utility.ControlLoopable;
-import org.usfirst.frc4388.utility.PIDParams;
-import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
-import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SensorCollection;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
-public class Elevator extends Subsystem implements ControlLoopable
-{
-	//PID encoder and motor
-	private CANTalonEncoder elevatorRight;
-	private WPI_TalonSRX elevatorLeft;
-
-	//PID controller Max Scale
-	private SoftwarePIDPositionController pidPositionControllerMaxScale;
-	//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
-	private PIDParams PositionPMaxScale;
-	
-	//PID controller Max Scale
-	private SoftwarePIDPositionController pidPositionControllerLowScale;
-	//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
-	private PIDParams PositionPLowScale;
-	
-	//PID controller Max Scale
-	private SoftwarePIDPositionController pidPositionControllerSwitch;
-	//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
-	private PIDParams PositionPSwitch;
-	
-	//PID controller Max Scale
-	private SoftwarePIDPositionController pidPositionControllerLowest;
-	//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
-	private PIDParams PositionPLowest;
-
-	//PID target
-	private double targetPPosition;
-	
-	//Encoder ticks to inches for encoders
-	public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch;
-	
-	//control mode for joystick control
-	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
-	
-	private double periodMs;
-	
-	//Non Linear Joystick
-	public static final double STICK_DEADBAND = 0.02;
-	public static final double MOVE_NON_LINEARITY = 1.0;
-	private int moveNonLinear = 0;
-	private double moveScale = 1.0;
-	private double moveTrim = 0.0;
-	
-	private boolean isFinished;
-	private DifferentialDrive m_drive;
-	
-	//private LimitSwitchSource limitSwitch = new DigitalInput(1);
-	LimitSwitchSource limitSwitchSource;
-	
-	public boolean pressed;
-	SensorCollection isPressed;
-	
-	public boolean elevatorControlMode = false;
-	// Motor controllers
-	//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
-    
-    public Elevator()
-    {    	
-    	try
-    	{
-			//PID elevator encoder and talon
-			elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
-			elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
-			
-    		elevatorRight.setInverted(false);
-
-			//Setting left elevator motor as follower
-    		elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
-    		elevatorLeft.setInverted(false);
-    		elevatorLeft.setNeutralMode(NeutralMode.Brake);
-    		elevatorRight.setNeutralMode(NeutralMode.Brake);
-    		elevatorRight.setSensorPhase(true);
-    		//Limit Switch Left
-    		//elevatorLeft.overrideLimitSwitchesEnable(true);
-    		elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-    		elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-    		
-    		//Limit Switch Right
-    		//elevatorRight.overrideLimitSwitchesEnable(true);
-			//elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-    		//elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-    		
-    		
-    		//Change This boi
-    		
-    	//	elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here
-    		//elevatorRight.configReverseSoftLimitThreshold(5, 0);
-    		//elevatorRight.configForwardSoftLimitEnable(true, 0);
-    		//elevatorRight.configReverseSoftLimitEnable(true, 0);
-    		
-    		//sos
-    		//elevatorRight.enableLimitSwitch(true, true);
-
-    		
-    		
-    		
-    		
-    	}
-    	catch(Exception e)
-    	{
-    		System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor");
-    	}
-    }
-    
-    private double nonlinearStickCalcPositive(double move, double moveNonLinearity) 
-    {
-		return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
-	}
-    
-    private double nonlinearStickCalcNegative(double move, double moveNonLinearity) 
-    {
-		return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
-	}
-    
-    private boolean inDeadZone(double input) 
-    {
-		boolean inDeadZone;
-		if (Math.abs(input) < STICK_DEADBAND)
-		{
-			inDeadZone = true;
-		} 
-		else 
-		{
-			inDeadZone = false;
-		}
-		
-		return inDeadZone;
-	}
-    
-    private double limitValue(double value) 
-    {
-		if (value > 1.0)
-		{
-			value = 1.0;
-		}
-		else if (value < -1.0)
-		{
-			value = -1.0;
-		}
-		return value;
-	}
-    
-    public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) 
-    {
-		if (inDeadZone(move))
-		{
-			return 0;
-		}
-
-		move += trim;
-		move *= scale;
-		move = limitValue(move);
-
-		int iterations = Math.abs(nonLinearFactor);
-		for (int i = 0; i < iterations; i++)
-		{
-			if (nonLinearFactor > 0)
-			{
-				move = nonlinearStickCalcPositive(move, wheelNonLinearity);
-			} 
-			else
-			{
-				move = nonlinearStickCalcNegative(move, wheelNonLinearity);
-			}
-		}
-		return move;
-	}
-    
-    public void setElevatorMode()
-    {
-    	setControlMode(DriveControlMode.JOYSTICK);
-    }
-    
-    public void resetElevatorEncoder()
-    {
-    	elevatorRight.setSelectedSensorPosition(0, 0, 0);
-    }
-    
-    public void moveElevatorXbox()
-    {
-    	double moveElevatorInput;
-    	double elevatorSafeZone = 15; 
-    	
-    	double elevatorPos = getEncoderElevatorPosition();
-    	
-    	//moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
-    	
-    	//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
-    	
-    	boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
-    	boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
-    	
-    	if(elevatorTuningPressed == true)
-      	{     		
-     		//elevatorRight.set(moveElevatorInput * 0.5);
-      	}
-     	else if(elevatorTuningPressed == false)
-     	{
-     		//elevatorRight.set(moveElevatorInput);
-     	}
-     		
-     		/*
-     		if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
-     		{
-     			elevatorRight.set(moveElevatorInput);
-     		}
-     		else if(elevatorPos > elevatorSafeZone)
-     		{
-     			elevatorRight.set(moveElevatorInput * 0.65);
-     			
-     			
-     			if(holdButtonPressed == true)
-             	{
-             		elevatorRight.set(-0.43 * (0.2));
-             	}
-             	else if(holdButtonPressed == false)
-             	{
-             		elevatorRight.set(moveElevatorInput * 0.75);
-             	}
-             	
-     		}
-     		
-     		else if(elevatorPos < 0)
-     		{
-             	elevatorRight.set(moveElevatorInput * 0.75);
-     		}
-     		*/
-     	}
-
-     	
-//     	System.out.println(elevatorPos);		//-6.9 to 1.9   total: 8.8 range
-	
-    
-	//PID encoder position
-	public double getEncoderElevatorPosition()
-	{
-		return elevatorRight.getPositionWorld();
-	}
-	
-	public double getElevatorHeightInchesAboveFloor()
-	{
-		return elevatorRight.getPositionWorld();
-	}
-
-	public synchronized void setControlMode(DriveControlMode controlMode) 
-	{
- 		this.controlMode = controlMode;
- 		
- 		isFinished = false;
-	}
-	/*
-	public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError)
-	{
-		double elevatorTargetPos = 0;
-		this.targetPPosition = elevatorTargetPos;
-		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);      ///////TARGET POSITION WHERE??
-		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
-	}
-	
-	public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError)
-	{
-		double elevatorTargetPos = 0;
-		this.targetPPosition = elevatorTargetPos;
-		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
-		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
-	}
-	
-	public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError)
-	{
-		double elevatorTargetPos = 0;
-		this.targetPPosition = elevatorTargetPos;
-		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
-		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
-	}
-	
-	public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError)
-	{
-		double elevatorTargetPos = 0;
-		this.targetPPosition = elevatorTargetPos;
-		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
-		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
-	}
-	*/
-	public void rawSetOutput(double output){
-		elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
-	}
-	
-	public void holdInPos()
-	{
-		elevatorRight.set(-0.43 * 0.2);
-	}
-	
-	public void stopMotors()
-	{
-		elevatorRight.set(0);
-	}
-	
-	public void isSwitchPressed()
-	{
-		pressed = false;
-		isPressed = elevatorRight.getSensorCollection();
-		
-		if(isPressed.isFwdLimitSwitchClosed() == true)
-		{
-			if (controlMode == DriveControlMode.JOYSTICK) {
-				Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);	
-			}
-			pressed = true;
-		}
-		else
-		{
-			if 	(controlMode == DriveControlMode.STOP_MOTORS){
-				{
-				Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
-				}
-			
-			pressed = false;
-			}
-		}
-
-	}
-		
-		//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
-	
-	
-	
-
-
-	@Override
-	public void controlLoopUpdate() 
-	{
-		if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) 
-		{
-			moveElevatorXbox();
-		}
-		else if (!isFinished)
-		{
-			//PID control mode
-			if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
-			{
-				isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition());
-			}
-			else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
-			{
-				isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition());
-			}
-			else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
-			{
-				isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition());
-			}
-			else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
-			{
-				isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
-			}
-			/*
-			else if(controlMode == DriveControlMode.RAW)
-			{
-				isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
-			}
-			*/
-		}
-	}
-
-	@Override
-    public void initDefaultCommand()
-    {
-    }
-
-    @Override
-    public void periodic() 
-    {
-    //	isSwitchPressed();
-    }
-    
-	@Override
-	public void setPeriodMs(long periodMs)
-	{
-		//PID controller
-		pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight);
-		pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight);
-		pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight);
-		pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight);
-		
-		this.periodMs = periodMs;
-	}
-	
-	public synchronized boolean isFinished() 
-	{
-		return isFinished;
-	}
-	
-	public double getPeriodMs()
-	{
-		return periodMs;
-	}
-	
-	public void updateStatus(Robot.OperationMode operationMode) 
-	{
-		if (operationMode == Robot.OperationMode.TEST) 
-		{
-			try 
-			{
-				SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
-				SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
-				//SmartDashboard.putData(pressed);
-			}
-			catch (Exception e) 
-			{
-				System.err.println("Drivetrain update status error");
-			}
-		}
-	}
-
-	
-}
-
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java
deleted file mode 100644
index c217c81..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java
+++ /dev/null
@@ -1,228 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import java.util.ArrayList;
-import java.util.Optional;
-import java.util.Set;
-
-import org.usfirst.frc4388.robot.Constants;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-
-import edu.wpi.first.wpilibj.Timer;
-
-/**
- * Implements an adaptive pure pursuit controller. See:
- * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4
- * .pdf
- * 
- * Basically, we find a spot on the path we'd like to follow and calculate the
- * wheel speeds necessary to make us land on that spot. The target spot is a
- * specified distance ahead of us, and we look further ahead the greater our
- * tracking error.
- */
-public class AdaptivePurePursuitController {
-    private static final double kEpsilon = 1E-9;
-
-    double mFixedLookahead;
-    Path mPath;
-    RigidTransform2d.Delta mLastCommand;
-    double mLastTime;
-    double mMaxAccel;
-    double mDt;
-    boolean mReversed;
-    double mPathCompletionTolerance;
-
-	protected ArrayList<CANTalonEncoder> motorControllers;	
-	protected long periodMs;
-	protected PIDParams pidParams;	
-	protected double startGyroAngle;
-	protected double targetGyroAngle;
-	protected double trackDistance;
-	
-	public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
-	{
-		this.motorControllers = motorControllers;
-		this.periodMs = periodMs;
-		setPID(pidParams);
-	}
-    
-	public void setPID(PIDParams pidParams) {
-		this.pidParams = pidParams;
-		
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
-			//motorController.setF(pidParams.kF);
-			//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
-			//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
-			//motorController.setProfile(0);
-			motorController.config_kP(0, pidParams.kP, 0);
-			motorController.config_kI(0, pidParams.kI, 0);
-			motorController.config_kD(0, pidParams.kD, 0);
-			motorController.config_kF(0, pidParams.kF, 0);
-			motorController.configNominalOutputForward(+0.0f, 0);
-			motorController.configNominalOutputReverse(-0.0f, 0);
-			motorController.configPeakOutputForward(+1.0f, 0);
-			motorController.configPeakOutputReverse(-1.0f, 0);
-			motorController.selectProfileSlot(0, 0);
-		}
-	}
-	
-    public void setPath(double fixed_lookahead, double max_accel, Path path,
-            boolean reversed, double path_completion_tolerance) {
-        mFixedLookahead = fixed_lookahead;
-        mMaxAccel = max_accel;
-        mPath = path;
-        mDt = periodMs;
-        mLastCommand = null;
-        mReversed = reversed;
-        mPathCompletionTolerance = path_completion_tolerance;
-
-        // Set up the motion profile 
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Speed);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			motorController.set(ControlMode.Velocity, 0);
-		}
-    }
-
-    public boolean isDone() {
-        double remainingLength = mPath.getRemainingLength();
-        return remainingLength <= mPathCompletionTolerance;
-    }
-
-	public boolean controlLoopUpdate(RigidTransform2d robot_pose) {
-        RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp());
-        Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command);
-
-        // Scale the command to respect the max velocity limits
-        double max_vel = 0.0;
-        max_vel = Math.max(max_vel, Math.abs(setpoint.left));
-        max_vel = Math.max(max_vel, Math.abs(setpoint.right));
-        if (max_vel > Constants.kPathFollowingMaxVel) {
-            double scaling = Constants.kPathFollowingMaxVel / max_vel;
-            setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling);
-        }
-
-        // Update the controllers Kf and set point.
-		for (CANTalonEncoder motorController : motorControllers) {
-			if (motorController.isRight()) {
-				motorController.setVelocityWorld(-setpoint.right);
-			}
-			else {
-				motorController.setVelocityWorld(-setpoint.left);
-			}
-		}
-		
-		return isDone();
-	}
-
-	public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) {
-        RigidTransform2d pose = robot_pose;
-        if (mReversed) {
-            pose = new RigidTransform2d(robot_pose.getTranslation(),
-                    robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
-        }
-
-        double distance_from_path = mPath.update(robot_pose.getTranslation());
-        if (this.isDone()) {
-            return new RigidTransform2d.Delta(0, 0, 0);
-        }
-
-        PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(),
-                distance_from_path + mFixedLookahead);
-        Optional<Circle> circle = joinPath(pose, lookahead_point.translation);
-
-        double speed = lookahead_point.speed;
-        if (mReversed) {
-            speed *= -1;
-        }
-        // Ensure we don't accelerate too fast from the previous command
-        double dt = now - mLastTime;
-        if (mLastCommand == null) {
-            mLastCommand = new RigidTransform2d.Delta(0, 0, 0);
-            dt = mDt;
-        }
-        double accel = (speed - mLastCommand.dx) / dt;
-        if (accel < -mMaxAccel) {
-            speed = mLastCommand.dx - mMaxAccel * dt;
-        } else if (accel > mMaxAccel) {
-            speed = mLastCommand.dx + mMaxAccel * dt;
-        }
-
-        // Ensure we slow down in time to stop
-        // vf^2 = v^2 + 2*a*d
-        // 0 = v^2 + 2*a*d
-        double remaining_distance = mPath.getRemainingLength();
-        double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance);
-        if (Math.abs(speed) > max_allowed_speed) {
-            speed = max_allowed_speed * Math.signum(speed);
-        }
-        final double kMinSpeed = 4.0;
-        if (Math.abs(speed) < kMinSpeed) {
-            // Hack for dealing with problems tracking very low speeds with
-            // Talons
-            speed = kMinSpeed * Math.signum(speed);
-        }
-
-        RigidTransform2d.Delta rv;
-        if (circle.isPresent()) {
-            rv = new RigidTransform2d.Delta(speed, 0,
-                    (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius);
-        } else {
-            rv = new RigidTransform2d.Delta(speed, 0, 0);
-        }
-        mLastTime = now;
-        mLastCommand = rv;
-        return rv;
-    }
-
-    public Set<String> getMarkersCrossed() {
-        return mPath.getMarkersCrossed();
-    }
-
-    public static class Circle {
-        public final Translation2d center;
-        public final double radius;
-        public final boolean turn_right;
-
-        public Circle(Translation2d center, double radius, boolean turn_right) {
-            this.center = center;
-            this.radius = radius;
-            this.turn_right = turn_right;
-        }
-    }
-
-    public static Optional<Circle> joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) {
-        double x1 = robot_pose.getTranslation().getX();
-        double y1 = robot_pose.getTranslation().getY();
-        double x2 = lookahead_point.getX();
-        double y2 = lookahead_point.getY();
-
-        Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point);
-        double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin()
-                - pose_to_lookahead.getY() * robot_pose.getRotation().cos();
-        if (Math.abs(cross_product) < kEpsilon) {
-            return Optional.empty();
-        }
-
-        double dx = x1 - x2;
-        double dy = y1 - y2;
-        double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos();
-        double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin();
-
-        double cross_term = mx * dx + my * dy;
-
-        if (Math.abs(cross_term) < kEpsilon) {
-            // Points are colinear
-            return Optional.empty();
-        }
-
-        return Optional.of(new Circle(
-                new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term),
-                        (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)),
-                .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0));
-    }
-
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java
deleted file mode 100644
index 52088f7..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java
+++ /dev/null
@@ -1,65 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
-public class CANTalonEncoder extends WPI_TalonSRX
-{
-	private double encoderTicksToWorld;
-	private boolean isRight = true;
-	
-
-	
-	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
-		this(deviceNumber, encoderTicksToWorld, false, feedbackDevice);
-	}
-
-	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
-		super(deviceNumber);
-		//this.setFeedbackDevice(feedbackDevice);
-		this.configSelectedFeedbackSensor(feedbackDevice, 0, 0);
-		this.encoderTicksToWorld = encoderTicksToWorld;
-		this.isRight = isRight;
-	}
-
-    public boolean isRight() {
-		return isRight;
-	}
-
-	public void setRight(boolean isRight) {
-		this.isRight = isRight;
-	}
-
-	public double convertEncoderTicksToWorld(double encoderTicks) {
-    	return encoderTicks / encoderTicksToWorld;
-    }
-
-    public double convertEncoderWorldToTicks(double worldValue) {
-    	return worldValue * encoderTicksToWorld;
-    }
-    
-    public void setWorld(double worldValue) {
-    	//this.set(convertEncoderWorldToTicks(worldValue));
-    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue));		//TODO: change to explicit mode set?
-    }
-    
-    public void setPositionWorld(double worldValue) {
-    	//this.setPosition(convertEncoderWorldToTicks(worldValue));
-    	this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0);	//TODO: verify
-    }
-    
-    public double getPositionWorld() {
-    	//return convertEncoderTicksToWorld(this.getPosition());
-    	return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0));	//TODO: verify want 0="Primary closed-loop"
-    }
-    
-    public void setVelocityWorld(double worldValue) {
-    	//this.set(convertEncoderWorldToTicks(worldValue) * 0.1);
-    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1);	//TODO: change to explicit mode set?
-    }
-    
-    public double getVelocityWorld() {
-    	//return convertEncoderTicksToWorld(this.getSpeed() / 0.1);
-    	return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1);	//TODO: verify want 0="Primary closed-loop"
-    }
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java
index 135bb97..65dfc5f 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java
@@ -6,6 +6,8 @@ import java.io.FileWriter;
 import java.io.IOException;
 import java.lang.reflect.Field;
 import java.math.BigDecimal;
+import java.nio.file.Files;
+import java.nio.file.StandardOpenOption;
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.HashMap;
@@ -19,8 +21,8 @@ import org.json.simple.parser.ParseException;
 /**
  * ConstantsBase
  * 
- * Base class for storing robot constants. Anything stored as a public static
- * field will be reflected and be able to set externally
+ * Base class for storing robot constants. Anything stored as a public static field will be reflected and be able to set
+ * externally
  */
 public abstract class ConstantsBase {
     HashMap<String, Boolean> modifiedKeys = new HashMap<String, Boolean>();
@@ -54,6 +56,18 @@ public abstract class ConstantsBase {
         return new File(filePath);
     }
 
+    public boolean truncateUserConstants() {
+        try {
+            Files.write(getFile().toPath(), new byte[0], StandardOpenOption.TRUNCATE_EXISTING);
+            loadFromFile();
+            return true;
+        } catch (IOException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+            return false;
+        }
+    }
+
     public boolean setConstant(String name, Double value) {
         return setConstantRaw(name, value);
     }
@@ -77,6 +91,9 @@ public abstract class ConstantsBase {
                     success = true;
                     if (!value.equals(current)) {
                         modifiedKeys.put(name, true);
+                        System.out.println("Constant Modified:" + field.getName());
+                    } else {
+                        System.out.println("Constant Not Modified:" + field.getName());
                     }
                 } catch (IllegalArgumentException | IllegalAccessException e) {
                     System.out.println("Could not set field: " + name);
@@ -116,7 +133,7 @@ public abstract class ConstantsBase {
 
     public Collection<Constant> getConstants() {
         List<Constant> constants = (List<Constant>) getAllConstants();
-        int stop = constants.size() - 1;
+        int stop = constants.size();
         for (int i = 0; i < constants.size(); ++i) {
             Constant c = constants.get(i);
             if ("kEndEditableArea".equals(c.name)) {
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java
new file mode 100644
index 0000000..0e797d4
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc4388.utility;
+
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.Date;
+import java.util.UUID;
+
+/**
+ * Tracks start-up and caught crash events, logging them to a file which dosn't roll over
+ */
+public class CrashTracker {
+
+    private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID();
+
+    public static void logRobotStartup() {
+        logMarker("robot startup");
+    }
+
+    public static void logRobotConstruction() {
+        logMarker("robot startup");
+    }
+
+    public static void logRobotInit() {
+        logMarker("robot init");
+    }
+
+    public static void logTeleopInit() {
+        logMarker("teleop init");
+    }
+
+    public static void logAutoInit() {
+        logMarker("auto init");
+    }
+
+    public static void logDisabledInit() {
+        logMarker("disabled init");
+    }
+
+    public static void logThrowableCrash(Throwable throwable) {
+        logMarker("Exception", throwable);
+    }
+
+    private static void logMarker(String mark) {
+        logMarker(mark, null);
+    }
+
+    private static void logMarker(String mark, Throwable nullableException) {
+
+        try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) {
+            writer.print(RUN_INSTANCE_UUID.toString());
+            writer.print(", ");
+            writer.print(mark);
+            writer.print(", ");
+            writer.print(new Date().toString());
+
+            if (nullableException != null) {
+                writer.print(", ");
+                nullableException.printStackTrace(writer);
+            }
+
+            writer.println();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java
new file mode 100644
index 0000000..8f9b6e6
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java
@@ -0,0 +1,19 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * Runnable class with reports all uncaught throws to CrashTracker
+ */
+public abstract class CrashTrackingRunnable implements Runnable {
+
+    @Override
+    public final void run() {
+        try {
+            runCrashTracked();
+        } catch (Throwable t) {
+            CrashTracker.logThrowableCrash(t);
+            throw t;
+        }
+    }
+
+    public abstract void runCrashTracked();
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java
deleted file mode 100644
index ea3af27..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java
+++ /dev/null
@@ -1,894 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import java.awt.AWTException;
-import java.awt.BasicStroke;
-import java.awt.Color;
-import java.awt.Dimension;
-import java.awt.FontMetrics;
-import java.awt.Graphics;
-import java.awt.Graphics2D;
-import java.awt.Image;
-import java.awt.Rectangle;
-import java.awt.RenderingHints;
-import java.awt.Robot;
-import java.awt.Stroke;
-import java.awt.Toolkit;
-import java.awt.datatransfer.Clipboard;
-import java.awt.datatransfer.ClipboardOwner;
-import java.awt.datatransfer.DataFlavor;
-import java.awt.datatransfer.Transferable;
-import java.awt.datatransfer.UnsupportedFlavorException;
-import java.awt.event.ActionEvent;
-import java.awt.event.ActionListener;
-import java.awt.event.MouseAdapter;
-import java.awt.event.MouseEvent;
-import java.awt.geom.AffineTransform;
-import java.awt.geom.Ellipse2D;
-import java.awt.geom.Line2D;
-import java.awt.image.BufferedImage;
-import java.io.IOException;
-import java.text.DecimalFormat;
-import java.util.LinkedList;
-
-import javax.swing.JFrame;
-import javax.swing.JMenuItem;
-import javax.swing.JPanel;
-import javax.swing.JPopupMenu;
-
-/**
- * This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user 
- * to plot multiple graphs on one figure, control axis dimensions, and specify colors.
- * 
- * This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If
- * a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this
- * class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user
- * control over as much as possible, so they can generate the perfect chart.
- * 
- * The plotter also features the ability to capture screen shots directly from the right-click menu, this allows
- * the user to copy and paste plots into reports or other documents rather quickly.
- * 
- * This class holds an interface similar to that of Matlab. 
- * 
- * This class currently only supports scatterd line charts.
- * 
- * @author Kevin Harrilal
- * @email kevin@team2168.org
- * @version 1
- * @date 9 Sept 2014
- *
- */
- 
-class FalconLinePlot extends JPanel implements ClipboardOwner{
-  
-    
-	private static final long serialVersionUID = 3205256608145459434L;
-	private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge
-    private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge
-    
-    private double upperXtic;
-    private double lowerXtic;
-    private double upperYtic;
-    private double lowerYtic;
-    private boolean yGrid;
-    private boolean xGrid;
-    
-    private double yMax;
-    private double yMin;
-    private double xMax;
-    private double xMin;
-    
-    private int yticCount;
-    private int xticCount;
-    private double xTicStepSize;
-    private double yTicStepSize;
-    
-    boolean userSetYTic;
-    boolean userSetXTic;
-    
-    private String xLabel;
-    private String yLabel;
-    private String titleLabel;
-    protected static int count = 0;
-    
-    JPopupMenu menu = new JPopupMenu("Popup");
-    
-    //Link List to hold all different plots on one graph.
-    private LinkedList<xyNode> link; 
-    
-
-    /**
-     * Constructor which Plots only Y-axis data.
-     * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
-     */
-    public FalconLinePlot(double[] yData)
-    {
-    	this(null,yData,Color.red);
-    }
-    
-    public FalconLinePlot(double[] yData,Color lineColor, Color marker)
-    {
-    	this(null,yData,lineColor,marker);
-    }
-    
-    /**
-     * Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length.
-     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
-     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
-     */
-    public FalconLinePlot(double[] xData, double[] yData)
-    {
-    	this(xData,yData,Color.red,null);
-    }
-    
-    /**
-     * Constructor which Plots chart based on provided x and y axis data. 
-     * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
-     * is the second dimension.
-     */
-    public FalconLinePlot(double[][] data)
-    {
-    	this(getXVector(data),getYVector(data),Color.red,null);
-    }
-    
-/**
- * Constructor which plots charts based on provided x and y axis data in a single two dimensional array.
- * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
- * is the second dimension.
- * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
- * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
- * not have datapoint markers.
- */
-    public FalconLinePlot(double[][] data, Color lineColor, Color markerColor)
-    {
-    	this(getXVector(data),getYVector(data),lineColor,markerColor);
-    }
-    
-    /**
-     * Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line.
-     * Data markers are not displayed.
-     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
-     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
-     * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
-     */
-    public FalconLinePlot(double[] xData, double[] yData,Color lineColor)
-    {
-    	this(xData,yData,lineColor,null);
-    }
-    
-
-    
-    /**
-     * Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user 
-     * can also specify the color of the adjoining line and the color of the datapoint maker.
-     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
-     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
-     * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
-     * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
-     * not have datapoint markers.
-     */
-    public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor)
-    {
-    	xLabel = "X axis";
-    	yLabel = "Y axis";
-    	titleLabel = "Title";
-    	
-       upperXtic = -Double.MAX_VALUE;
-       lowerXtic = Double.MAX_VALUE;
-       upperYtic = -Double.MAX_VALUE;
-       lowerYtic = Double.MAX_VALUE;
-       xticCount = -Integer.MAX_VALUE;
-       
-       this.userSetXTic = false;
-       this.userSetYTic = false;
-    	
-    	link = new LinkedList<xyNode>();
-    	
-    	addData(xData, yData, lineColor,markerColor);
-    	
-    	count ++;
-    	JFrame g = new JFrame("Figure " + count);
-        g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        g.add(this);
-        g.setSize(600,400);
-        g.setLocationByPlatform(true);
-        g.setVisible(true);
-         
-        menu(g,this);	
-    }
-    
-    /**
-     * Adds a plot to an existing figure.  
-     * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
-     * @param color is the color the user wishes to be displayed for the line connecting each datapoint
-     */
-    
-    public void addData(double[] y, Color lineColor)
-    {
-    	addData(y, lineColor, null);
-    }
-    
-    public void addData(double[] y, Color lineColor, Color marker)
-    {
-    	//cant add y only data unless all other data is y only data
-    	for(xyNode data: link)
-    		if(data.x != null)
-    			throw new Error ("All previous chart series need to have only Y data arrays");
-    	
-    	addData(null,y,lineColor, marker);
-    }
-    
-    public void addData(double[] x, double[] y, Color lineColor)
-    {
-    	addData(x,y,lineColor,null);
-    }
-    
-    
-    public void addData(double[][] data, Color lineColor)
-    {
-    	addData(getXVector(data),getYVector(data),lineColor,null);
-    }
-    
-    public void addData(double[][] data, Color lineColor, Color marker)
-    {
-    	addData(getXVector(data),getYVector(data),lineColor,marker);
-    }
-    
-    public void addData(double[] x, double[] y, Color lineColor, Color marker)
-    { 	
-    	xyNode Data = new xyNode();
-    	    	
-    	//copy y array into node
-    	Data.y = new double[y.length];
-    	Data.lineColor = lineColor;
-    	
-    	if(marker == null)
-    		Data.lineMarker = false;
-    	else
-    	{
-    		Data.lineMarker = true;
-    		Data.markerColor = marker;
-    	}
-    	for(int i=0; i<y.length; i++)
-    		Data.y[i] = y[i];
-    	
-    	//if X is not null, copy x
-    	if(x != null)
-    	{
-        	//cant add x, and y data unless all other data has x and y data
-        	
-        	for(xyNode data: link)
-        		if(data.x == null)
-        			throw new Error ("All previous chart series need to have both X and Y data arrays");
-    		
-    		if(x.length != y.length)
-    			throw new Error("X dimension must match Y dimension");
-    		
-    		Data.x = new double[x.length];
-    		
-    		for(int i=0; i<x.length; i++)
-        		Data.x[i] = x[i];
-    		
-    	}
-    	link.add(Data);
-    }
- 
-    /**
-     * Main method which paints the panel and shows the figure.
-     * 
-     */
-    @Override
-    protected void paintComponent(Graphics g) {
-        super.paintComponent(g);
-        Graphics2D g2 =  (Graphics2D)g;
-        g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
-        
-        int w = getWidth();
-        int h = getHeight();
-    
-        // Draw X and Y lines axis.
-        Line2D yaxis = new Line2D.Double(xPAD, yPAD, xPAD, h-yPAD);
-        Line2D.Double xaxis = new Line2D.Double(xPAD, h-yPAD, w-xPAD, h-yPAD);
-        g2.draw(yaxis); 
-        g2.draw(xaxis);
-        
-        //find Max Y limits
-        getMinMax(link);
-        
-        //draw ticks
-        drawYTickRange(g2, yaxis, 15, yMax, yMin);
-        drawXTickRange(g2, xaxis, 15, xMax, xMin);
-        
-        //plot all data
-        plot(g2);
-        
-        //draw x and y labels
-        setXLabel(g2, xLabel);
-        setYLabel(g2, yLabel);
-        setTitle(g2, titleLabel);
-        
-    }
-    
-    void setXTic(double lowerBound, double upperBound, double stepSize)
-    {
-    	this.userSetXTic = true;
-    	
-    	this.upperXtic=upperBound;
-    	this.lowerXtic=lowerBound;
-    	this.xTicStepSize = stepSize;
-    }
-    
-    public void setYTic(double lowerBound, double upperBound, double stepSize)
-    {
-    	this.userSetYTic = true;
-    	
-    	this.upperYtic=upperBound;
-    	this.lowerYtic=lowerBound;
-    	this.yTicStepSize = stepSize;
-    }
-    
-    private void plot(Graphics2D g2)
-    {
-    	
-    	int w = super.getWidth();
-    	int h = super.getHeight();
-    	
-    	Color tempC = g2.getColor();
-    	
-    	//loop through list and plot each
-    	for(int i=0; i<link.size(); i++)
-    	{
-	        // Draw lines.
-	        double xScale = (double)(w - 2*xPAD)/(upperXtic-lowerXtic);
-	        double yScale = (double)(h - 2*yPAD)/(upperYtic-lowerYtic);
-	        
-	        for(int j = 0; j < link.get(i).y.length-1; j++) 
-	        {
-	        	double x1;
-	        	double x2;
-	        	
-	            if(link.get(i).x==null)
-	            {
-	            	x1 = xPAD + j*xScale;
-	            	x2 = xPAD + (j+1)*xScale;
-	            }
-	            else
-	            {
-	            	x1 = xPAD + xScale*link.get(i).x[j] + lowerXtic*xScale;
-	            	x2 = xPAD + xScale*link.get(i).x[j+1] + lowerXtic*xScale;
-	            }
-	            
-	            double y1 = h - yPAD - yScale*link.get(i).y[j] + lowerYtic*yScale;
-	            
-	            double y2 = h - yPAD - yScale*link.get(i).y[j+1] + lowerYtic*yScale;
-	            g2.setPaint(link.get(i).lineColor);
-	            g2.draw(new Line2D.Double(x1, y1, x2, y2));
-	            
-	            if(link.get(i).lineMarker)
-	            {
-	            	g2.setPaint(link.get(i).markerColor);
-	            	g2.fill(new Ellipse2D.Double(x1-2, y1-2, 4, 4));
-	            	g2.fill(new Ellipse2D.Double(x2-2, y2-2, 4, 4));
-	            }
-	      
-	        }
-	        	        	
-    	}
-    	
-    	g2.setColor(tempC);
-    }
-    
-    /**
-     * need to optimize for loops
-     * @param list
-     */
-    private void getMinMax(LinkedList<xyNode> list)
-    {
-    	for(xyNode node: list)
-    	{
-    		double tempYMax = getMax(node.y);
-    		double tempYMin = getMin(node.y);
-    		
-    		if(tempYMin<yMin)
-    			yMin = tempYMin;
-    		
-    		if(tempYMax>yMax)
-    			yMax=tempYMax;
-    		
-    		if(xticCount < node.y.length)
-				xticCount = node.y.length;
-    		
-    		
-    		if(node.x != null)
-    		{
-        		double tempXMax = getMax(node.x);
-        		double tempXMin = getMin(node.x);
-        		
-        		if(tempXMin<xMin)
-        			xMin = tempXMin;
-        		
-        		if(tempXMax>xMax)
-        			xMax=tempXMax;
-        		
-    		}
-    		else
-    		{
-    			xMax=node.y.length-1;
-    			xMin=0;
-    			
-    		}
-    		
-    	}
-    	
-    }
- 
-    private double getMax(double[] data) {
-        double max = -Double.MAX_VALUE;
-        for(int i = 0; i < data.length; i++) {
-            if(data[i] > max)
-                max = data[i];
-        }
-        return max;
-    }
-    
-    private double getMin(double[] data) {
-        double min = Double.MAX_VALUE;
-        for(int i = 0; i < data.length; i++) {
-            if(data[i] < min)
-                min = data[i];
-        }
-        return min;
-    }
-    
-    public void setYLabel(String s)
-    {
-    	yLabel = s;
-    }
-    
-    public void setXLabel(String s)
-    {
-    	xLabel = s;
-    }
-    
-    public void setTitle(String s)
-    {
-    	titleLabel = s;
-    }
-    
-    private void setYLabel(Graphics2D g2, String s)
-    {
-    	FontMetrics fm = getFontMetrics(getFont());
-    	int width = fm.stringWidth(s);
-    	
-    	AffineTransform temp = g2.getTransform();
-    	
-    	    AffineTransform at = new AffineTransform();
-    	    at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2);
-    	    g2.setTransform(at);
-    	    
-    	    //draw string in center of y axis
-    	    g2.drawString(s, 10, 7+getHeight()/2+width/2);
-
-        g2.setTransform(temp);
-    	
-    }
-    
-    private void setXLabel(Graphics2D g2, String s)
-    {
-    	FontMetrics fm = getFontMetrics(getFont());
-    	int width = fm.stringWidth(s);
-    	
-    	g2.drawString(s, getWidth()/2-(width/2), getHeight()-10);
-    }
-    
-    private void setTitle(Graphics2D g2, String s)
-    {
-    	FontMetrics fm = getFontMetrics(getFont());
-    	
-    	String[] line = s.split("\n");
-    	
-    	int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2);
-    	
-    	for (int i=0; i<line.length; i++)
-    	{
-    		
-    		int width = fm.stringWidth(line[i]);
-            g2.drawString(line[i], getWidth()/2-(width/2),  height);
-            height +=fm.getHeight();
-            
-    	}
-   
-    }
-
-    
-    public void yGridOn()
-    {
-    	yGrid=true;
-    	//super.repaint();
-    }
-    
-    public void yGridOff()
-    {
-    	yGrid=false;
-    	//super.repaint();
-    }
-    
-    public void xGridOn()
-    {
-    	xGrid=true;
-    	//super.repaint();
-    }
-    
-    public void xGridOff()
-    {
-    	xGrid=false;
-    	//super.repaint();
-    }
-    
-    private void drawYTickRange(Graphics2D g2, Line2D yaxis, int tickCount, double Max, double Min)
-    {
-    	if(!userSetYTic)
-    	{
-    	double range = Max - Min;
-    	
-    	
-    	//calculate max Y and min Y tic Range
-    	double unroundedTickSize = range/(tickCount-1);
-    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
-    	double pow10x = Math.pow(10, x);
-    	yTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
-    	
-    	//determine min and max tick label 
-        if(Min<0)
-        	lowerYtic = yTicStepSize * Math.floor(Min/yTicStepSize);
-        else
-        	lowerYtic = yTicStepSize * Math.ceil(Min/yTicStepSize);
-        
-        if(Max<0)
-        	upperYtic = yTicStepSize * Math.floor(1+Max/yTicStepSize);
-        else
-        	upperYtic = yTicStepSize * Math.ceil(1+Max/yTicStepSize);
-        
-    	}
-    	
-        
-        double x0 = yaxis.getX1();
-        double y0 = yaxis.getY1();
-        double xf = yaxis.getX2();
-        double yf = yaxis.getY2();
-        
-        //calculate stepsize between ticks and length of Y axis using distance formula
-        int roundedTicks = (int) ((upperYtic - lowerYtic) / yTicStepSize);
-        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
-        
-        double upper = upperYtic;
-        for (int i = 0; i<=roundedTicks; i++)
-        {
-        	double newY = y0;
-        	
-        	//calculate width of number for proper drawing
-        	String number = new DecimalFormat("#.#").format(upper);
-        	FontMetrics fm = getFontMetrics(getFont());
-        	int width = fm.stringWidth(number);
-   
-        	g2.draw(new Line2D.Double(x0,newY, x0-10,newY));
-        	g2.drawString(number, (float)x0-15-width, (float)newY+5); 
-        	 
-        	//add grid lines to chart
-        	if(yGrid && i!=roundedTicks)
-        	{
-
-        		Stroke tempS = g2.getStroke();
-        		Color tempC = g2.getColor();
-        		
-        		g2.setColor (Color.lightGray);
-          	    g2.setStroke (new BasicStroke(
-          	      1f, 
-          	      BasicStroke.CAP_ROUND, 
-          	      BasicStroke.JOIN_ROUND, 
-          	      1f, 
-          	      new float[] {5f}, 
-          	      0f));
-          	    
-          	    g2.draw(new Line2D.Double(xPAD, newY, getWidth()-xPAD, newY));
-        		
-          	    g2.setColor(tempC);
-          	    g2.setStroke(tempS);
-        		
-        	}
-        	
-        	  upper = upper - yTicStepSize;
-        	y0 = newY + distance;
-        	
-        }
-    }
-    
-    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min)
-    {
-    	drawXTickRange(g2, xaxis, tickCount, Max, Min, 1);
-    }
-    
-    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min, double skip)
-    {
-    	if(!userSetXTic)
-    	{
-    	double range = Max - Min;
-    	
-    	//calculate max Y and min Y tic Range
-    	double unroundedTickSize = range/(tickCount-1);
-    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
-    	double pow10x = Math.pow(10, x);
-    	xTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
-    	
-    	//determine min and max tick label 
-        if(Min<0)
-        	lowerXtic = xTicStepSize * Math.floor(Min/xTicStepSize);
-        else
-        	lowerXtic = xTicStepSize * Math.ceil(Min/xTicStepSize);
-        
-        if(Max<0)
-        	upperXtic = xTicStepSize * Math.floor(1+Max/xTicStepSize);
-        else
-        	upperXtic = xTicStepSize * Math.ceil(1+Max/xTicStepSize);
-    	}
-        
-        
-        double x0 = xaxis.getX1();
-        double y0 = xaxis.getY1();
-        double xf = xaxis.getX2();
-        double yf = xaxis.getY2();
-        
-        //calculate stepsize between ticks and length of Y axis using distance formula
-        int roundedTicks = (int) ((upperXtic - lowerXtic) / xTicStepSize);
-        
-        
-        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
-        
-        double lower = lowerXtic;
-        for (int i = 0; i<=roundedTicks; i++)
-        {
-        	double newX = x0;
-        	
-        	//calculate width of number for proper drawing
-        	String number = new DecimalFormat("#.#").format(lower);
-        	FontMetrics fm = getFontMetrics( getFont() );
-        	int width = fm.stringWidth(number);
-        	
-   
-        	g2.draw(new Line2D.Double(newX,yf, newX,yf+10));
-        	
-        	//dont label every x tic to prevent clutter
-        	if(i%skip==0)
-       		g2.drawString(number, (float)(newX-(width/2.0)), (float)yf+25); 
-        	
-        	//add grid lines to chart
-        	if(xGrid && i!=0)
-        	{
-        		Stroke tempS = g2.getStroke();
-        		Color tempC = g2.getColor();
-        		
-        		g2.setColor (Color.lightGray);
-          	    g2.setStroke (new BasicStroke(
-          	      1f, 
-          	      BasicStroke.CAP_ROUND, 
-          	      BasicStroke.JOIN_ROUND, 
-          	      1f, 
-          	      new float[] {5f}, 
-          	      0f));
-          	    
-          	    g2.draw(new Line2D.Double(newX, yPAD, newX, getHeight()-yPAD));
-        		
-          	    g2.setColor(tempC);
-          	    g2.setStroke(tempS);
-        		
-        	}
-        	
-        	 
-        	lower = lower + xTicStepSize;
-        	x0 = newX + distance;
-        }
-    }
-    
-    public void updateData(int series, double[][] data)
-    {
-    	//add Data to link list
-    	addData(data,null,null);
-    	
-    	//copy data from new to old and line styles from list to new list.
-    	
-    	link.get(series).x = link.getLast().x.clone();
-    	link.get(series).y = link.getLast().y.clone();
-    	
-    	//remove last data
-    	link.removeLast();
-    
-    	
-    	
-    }
-    
-	public static double[] getXVector(double[][] arr)
-	{
-		double[] temp = new double[arr.length];
-
-		for(int i=0; i<temp.length; i++)
-			temp[i] = arr[i][0];
-
-		return temp;		
-	}
-
-	public static double[] getYVector(double[][] arr)
-	{
-		double[] temp = new double[arr.length];
-
-		for(int i=0; i<temp.length; i++)
-			temp[i] = arr[i][1];
-
-		return temp;		
-	}
-	
-	/**********Class for Linked List************/
-    private class xyNode
-    {
-    	double[] x;
-    	double[] y;
-    	Color lineColor;
-    	
-    	boolean lineMarker;
-    	Color markerColor;
-    	
-    	public xyNode()
-    	{
-    		x=null;
-    		y=null;
-    		
-    		lineMarker = false;
-    	}
-    }
-	
-    /****Methods to Support Right Click Menu****/
-    @Override
-	public void lostOwnership(Clipboard    clip, Transferable transferable) 
-	{
-		//We must keep the object we placed on the system clipboard
-		//until this method is called.
-	}
-
-    
-    
-    private void menu(JFrame g, final FalconLinePlot p )
-    {
-
-    	g.addMouseListener(new PopupTriggerListener());
-    	
-    	JMenuItem item = new JMenuItem("Copy Figure");
-
-        item.addActionListener(new ActionListener() 
-        {
-          public void actionPerformed(ActionEvent e) 
-          {
-            
-            BufferedImage i = new BufferedImage(p.getSize().width, p.getSize().height,BufferedImage.TRANSLUCENT);
-            p.setOpaque(false);
-            p.paint(i.createGraphics()); 
-            TransferableImage trans = new TransferableImage( i );
-			Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
-			c.setContents( trans, p);
-          }
-        });
-        
-        menu.add(item);
-    	
-        item = new JMenuItem("Desktop ScreenShot");
-        item.addActionListener(new ActionListener() 
-        {
-          public void actionPerformed(ActionEvent e) 
-          {
-            System.out.println("Copy files to clipboard");
-            
-            try {
-                Robot robot = new Robot();
-                Dimension screenSize  = Toolkit.getDefaultToolkit().getScreenSize();
-                Rectangle screen = new Rectangle( screenSize );
-                BufferedImage i = robot.createScreenCapture( screen );
-                TransferableImage trans = new TransferableImage( i );
-                Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
-                c.setContents( trans, p);
-            }
-            catch ( AWTException x ) {
-                x.printStackTrace();
-                System.exit( 1 );
-            }
-            
-          }
-        });
-        
-        menu.add(item);
-    }
-
-    class PopupTriggerListener extends MouseAdapter {
-    	public void mousePressed(MouseEvent ev) {
-    		if (ev.isPopupTrigger()) {
-    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
-    		}
-    	}
-
-    	public void mouseReleased(MouseEvent ev) {
-    		if (ev.isPopupTrigger()) {
-    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
-    		}
-    	}
-
-    	public void mouseClicked(MouseEvent ev) {
-
-    	}
-    }
-
-    private class TransferableImage implements Transferable {
-
-        Image i;
-
-        public TransferableImage( Image i ) {
-            this.i = i;
-        }
-
-        public Object getTransferData( DataFlavor flavor )
-        throws UnsupportedFlavorException, IOException {
-            if ( flavor.equals( DataFlavor.imageFlavor ) && i != null ) {
-                return i;
-            }
-            else {
-                throw new UnsupportedFlavorException( flavor );
-            }
-        }
-
-        public DataFlavor[] getTransferDataFlavors() {
-            DataFlavor[] flavors = new DataFlavor[ 1 ];
-            flavors[ 0 ] = DataFlavor.imageFlavor;
-            return flavors;
-        }
-
-        public boolean isDataFlavorSupported( DataFlavor flavor ) {
-            DataFlavor[] flavors = getTransferDataFlavors();
-            for ( int i = 0; i < flavors.length; i++ ) {
-                if ( flavor.equals( flavors[ i ] ) ) {
-                    return true;
-                }
-            }
-
-            return false;
-        }
-    }
-    
-    /******TEST MAIN METHOD*******/
-    public static void main(String[] args) {
-    	
-  	  double[] data = {
-  		        -235, 14, 18, 03, 60, 150, 74, 87, 54, 77,
-  		        61, 55, 48, 60, 49, 36, 38, 27, 20, 18,5
-  		    };
-  	  
-  	  double[] data2 = {
-		        -4, 124, 128, 33, -1, 1, 74, 87, 54, 77,
-		        61, 55, 48, 60, 40, 36, 38, 27, 20, 18,5
-		    };
-  	  
-  	  double[] test = {0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 2.0,
-  			  2.2,2.4,2.6,2.8,3.0,3.2,3.4,3.6,3.8,4.0,4.2
-  	  };
-  	
-  	
-  	
-  	FalconLinePlot fig2 = new FalconLinePlot(data,Color.red, Color.blue);
-  	
-  	fig2.yGridOn();
-  	fig2.xGridOn();
-  	fig2.setYLabel("This is a new");
-  	
-  	fig2.addData(data2, Color.blue);
-  	
-  	FalconLinePlot fig1 =  new FalconLinePlot(test,data);
-  	
-  }
-    
-    
-}
-   
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java
deleted file mode 100644
index e3c3910..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java
+++ /dev/null
@@ -1,59 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import org.usfirst.frc4388.robot.Constants;
-
-/**
- * Provides forward and inverse kinematics equations for the robot modeling the
- * wheelbase as a differential drive (with a corrective factor to account for
- * the inherent skidding of the center 4 wheels quasi-kinematically).
- */
-
-public class Kinematics {
-    private static final double kEpsilon = 1E-9;
-
-    /**
-     * Forward kinematics using only encoders, rotation is implicit (less
-     * accurate than below, but useful for predicting motion)
-     */
-    public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) {
-        double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2;
-        double delta_v = (right_wheel_delta - left_wheel_delta) / 2;
-        double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter;
-        return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation);
-    }
-
-    /**
-     * Forward kinematics using encoders and explicitly measured rotation (ex.
-     * from gyro)
-     */
-    public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta,
-            double delta_rotation_rads) {
-        return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads);
-    }
-
-    /** Append the result of forward kinematics to a previous pose. */
-    public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta,
-            double right_wheel_delta, Rotation2d current_heading) {
-        RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta,
-                current_pose.getRotation().inverse().rotateBy(current_heading).getRadians());
-        return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro));
-    }
-
-    public static class DriveVelocity {
-        public final double left;
-        public final double right;
-
-        public DriveVelocity(double left, double right) {
-            this.left = left;
-            this.right = right;
-        }
-    }
-
-    public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) {
-        if (Math.abs(velocity.dtheta) < kEpsilon) {
-            return new DriveVelocity(velocity.dx, velocity.dx);
-        }
-        double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor);
-        return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v);
-    }
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java
new file mode 100644
index 0000000..792bd07
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java
@@ -0,0 +1,14 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * Interface for loops, which are routine that run periodically in the robot code (such as periodic gyroscope
+ * calibration, etc.)
+ */
+public interface Loop {
+
+    public void onStart(double timestamp);
+
+    public void onLoop(double timestamp);
+
+    public void onStop(double timestamp);
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java
new file mode 100644
index 0000000..823131f
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java
@@ -0,0 +1,89 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.usfirst.frc4388.robot.Constants;
+
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This code runs all of the robot's loops. Loop objects are stored in a List object. They are started when the robot
+ * powers up and stopped after the match.
+ */
+public class Looper {
+    public final double kPeriod = Constants.kLooperDt;
+
+    private boolean running_;
+
+    private final Notifier notifier_;
+    private final List<Loop> loops_;
+    private final Object taskRunningLock_ = new Object();
+    private double timestamp_ = 0;
+    private double dt_ = 0;
+
+    private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() {
+        @Override
+        public void runCrashTracked() {
+            synchronized (taskRunningLock_) {
+                if (running_) {
+                    double now = Timer.getFPGATimestamp();
+
+                    for (Loop loop : loops_) {
+                        loop.onLoop(now);
+                    }
+
+                    dt_ = now - timestamp_;
+                    timestamp_ = now;
+                }
+            }
+        }
+    };
+
+    public Looper() {
+        notifier_ = new Notifier(runnable_);
+        running_ = false;
+        loops_ = new ArrayList<>();
+    }
+
+    public synchronized void register(Loop loop) {
+        synchronized (taskRunningLock_) {
+            loops_.add(loop);
+        }
+    }
+
+    public synchronized void start() {
+        if (!running_) {
+            System.out.println("Starting loops");
+            synchronized (taskRunningLock_) {
+                timestamp_ = Timer.getFPGATimestamp();
+                for (Loop loop : loops_) {
+                    loop.onStart(timestamp_);
+                }
+                running_ = true;
+            }
+            notifier_.startPeriodic(kPeriod);
+        }
+    }
+
+    public synchronized void stop() {
+        if (running_) {
+            System.out.println("Stopping loops");
+            notifier_.stop();
+            synchronized (taskRunningLock_) {
+                running_ = false;
+                timestamp_ = Timer.getFPGATimestamp();
+                for (Loop loop : loops_) {
+                    System.out.println("Stopping " + loop);
+                    loop.onStop(timestamp_);
+                }
+            }
+        }
+    }
+
+    public void outputToSmartDashboard() {
+        SmartDashboard.putNumber("looper_dt", dt_);
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java
index 28dced9..c3be5a6 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java
@@ -2,9 +2,9 @@ package org.usfirst.frc4388.utility;
 
 import java.util.ArrayList;
 
-import org.usfirst.frc4388.robot.Constants;
 import org.usfirst.frc4388.robot.subsystems.Drive;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
 public class MMTalonPIDController
@@ -12,7 +12,7 @@ public class MMTalonPIDController
 	protected static enum MMControlMode { STRAIGHT, TURN };
 	public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
 
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected long periodMs;
 	protected PIDParams pidParams;	
 	protected boolean useGyroLock;
@@ -23,73 +23,54 @@ public class MMTalonPIDController
 	protected MMTalonTurnType turnType;
 	protected double targetValue;
 	
-	public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		this.periodMs = periodMs;
 		setPID(pidParams);
 	}
-	
-	private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) {
-		return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10);
-	}
     
 	public void setPID(PIDParams pidParams) {
 		this.pidParams = pidParams;
 		
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
-			//motorController.setF(pidParams.kF);
-			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kF(0, pidParams.kF, 0);	//TODO: verify want parameter slot 0, with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF);
 		}
 	}
 	
-	public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
+	public void setMMStraightTarget(double startValue, double targetValue, double maxVelocity, double maxAcceleration, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
 		controlMode = MMControlMode.STRAIGHT;
 		this.startGyroAngle = desiredAngle;
 		this.useGyroLock = useGyroLock;
 		this.targetValue = targetValue;
 		
 		// Set up the motion profile 
-		for (CANTalonEncoder motorController : motorControllers) {
+		for (TalonSRXEncoder motorController : motorControllers) {
 			if (resetEncoder) {
-				//motorController.setPosition(0);
-				motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+				motorController.setPosition(0);
 			}
-			//motorController.setMotionMagicCruiseVelocity(maxVelocity);
-			//motorController.setMotionMagicAcceleration(maxAcceleration);
-			//motorController.set(targetValue);
-			//motorController.changeControlMode(TalonControlMode.MotionMagic);
-			motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0);
-			motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0);
+			motorController.configMotionCruiseVelocity((int)maxVelocity, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.configMotionAcceleration((int)maxAcceleration, TalonSRXEncoder.TIMEOUT_MS);
 			motorController.set(ControlMode.MotionMagic, targetValue);
 		}
 	}
 			
 	public void setZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			motorController.set(ControlMode.Position, 0);
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.MotionMagic, targetValue);
 		}
 	}
 
 	public void resetZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 		}
 	}
 
 	public void resetZeroPosition(double angle) {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(angle);
-			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
 		}
 	}
 
@@ -120,14 +101,12 @@ public class MMTalonPIDController
 			leftTarget = targetValue - deltaDistance;
 			
 			// Update the controllers with updated set points.
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(rightTarget);
-			    	motorController.set(motorController.getControlMode(), rightTarget);	//TODO: change to explicit mode set?
+					motorController.set(ControlMode.MotionMagic, rightTarget);
 				}
 				else {
-					//motorController.set(leftTarget);
-			    	motorController.set(motorController.getControlMode(), leftTarget);	//TODO: change to explicit mode set?
+					motorController.set(ControlMode.MotionMagic, leftTarget);
 				}
 			}
 		}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java
index b91fc31..ca093b5 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java
@@ -2,13 +2,14 @@ package org.usfirst.frc4388.utility;
 
 import java.util.ArrayList;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
 public class MPSoftwarePIDController
 {	
 	public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC };
 
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected long periodMs;
 	protected PIDParams pidParams;	
 	protected MotionProfileBoxCar mp;
@@ -21,7 +22,7 @@ public class MPSoftwarePIDController
 	protected double prevError = 0.0; // the prior error (used to compute velocity)
 	protected double totalError = 0.0; // the sum of the errors for use in the integral calc
 	
-	public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		this.periodMs = periodMs;
@@ -39,32 +40,23 @@ public class MPSoftwarePIDController
 		this.useGyroLock = true;
 		
 		// Set up the motion profile 
-		mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2);
-		// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
-		// for (CANTalonEncoder motorController : motorControllers) {
-		//     motorController.changeControlMode(TalonControlMode.PercentVbus);
-		// }
-		
+		mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2);		
 		prevError = 0.0;
 		totalError = 0.0;
 	}
 	
-	//public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) {
-	//	this.turnType = turnType;
-	//	this.useGyroLock = true;
-	//	
-	//	// Set up the motion profile 
-	//	mp = MotionProfileCache.getInstance().getMP(key);
-	//	this.startGyroAngle = mp.getStartDistance();
-	//	this.targetGyroAngle = mp.getTargetDistance();
-	//	
-	//	for (CANTalonEncoder motorController : motorControllers) {
-	//		motorController.changeControlMode(TalonControlMode.PercentVbus);
-	//	}
-	//	
-	//	prevError = 0.0;
-	//	totalError = 0.0;
-	//}
+	public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) {
+		this.turnType = turnType;
+		this.useGyroLock = true;
+		
+		// Set up the motion profile 
+		mp = MotionProfileCache.getInstance().getMP(key);
+		this.startGyroAngle = mp.getStartDistance();
+		this.targetGyroAngle = mp.getTargetDistance();
+				
+		prevError = 0.0;
+		totalError = 0.0;
+	}
 	
 	public boolean controlLoopUpdate() {
 		return controlLoopUpdate(0);
@@ -93,55 +85,51 @@ public class MPSoftwarePIDController
 			
 		// Update the controllers set point.
 		if (turnType == MPSoftwareTurnType.TANK) {
-			for (CANTalonEncoder motorController : motorControllers) {
-				//motorController.set(output);
-				motorController.set(ControlMode.PercentOutput, output);
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, -output);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, output);					
+				}
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(0);
 					motorController.set(ControlMode.PercentOutput, 0);
 				}
 				else {
-					//motorController.set(2.0 * output);					
-					motorController.set(ControlMode.PercentOutput, 2.0 * output);
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
 				}
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(2.0 * output);
 					motorController.set(ControlMode.PercentOutput, 2.0 * output);
 				}
 				else {
-					//motorController.set(0);					
-					motorController.set(ControlMode.PercentOutput, 0);
+					motorController.set(ControlMode.PercentOutput, 0);					
 				}
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.LEFT_ARC) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(1.0 * output);
 					motorController.set(ControlMode.PercentOutput, 1.0 * output);
 				}
 				else {
-					//motorController.set(2.0 * output);					
 					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
 				}
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.RIGHT_ARC) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(2.0 * output);
 					motorController.set(ControlMode.PercentOutput, 2.0 * output);
 				}
 				else {
-					//motorController.set(1.0 * output);					
 					motorController.set(ControlMode.PercentOutput, 1.0 * output);					
 				}
 			}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
index 9a9d760..1461505 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
@@ -1,16 +1,15 @@
-
 package org.usfirst.frc4388.utility;
 
 import java.util.ArrayList;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
 public class MPTalonPIDController
 {	
 	protected static enum MPControlMode { STRAIGHT, TURN };
 	public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
-
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected long periodMs;
 	protected PIDParams pidParams;	
 	protected MotionProfileBoxCar mp;
@@ -21,22 +20,26 @@ public class MPTalonPIDController
 	protected double trackDistance;
 	protected MPControlMode controlMode;
 	protected MPTalonTurnType turnType;
+	protected int pidSlot;
 	
-	public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public MPTalonPIDController(long periodMs, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		this.periodMs = periodMs;
-		setPID(pidParams);
 	}
     
-	public void setPID(PIDParams pidParams) {
+	public void setPID(PIDParams pidParams, int slot) {
 		this.pidParams = pidParams;
 		
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
-			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPID(slot, pidParams.kP, pidParams.kI, pidParams.kD);
+		}
+	}
+	
+	public void setPIDSlot(int slot) {
+		this.pidSlot = slot;
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.selectProfileSlot(slot, 0);
 		}
 	}
 	
@@ -55,32 +58,36 @@ public class MPTalonPIDController
 		
 		// Set up the motion profile 
 		mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2);
-		for (CANTalonEncoder motorController : motorControllers) {
+		for (TalonSRXEncoder motorController : motorControllers) {
 			if (resetEncoder) {
-				//motorController.setPosition(0);
-				motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+				motorController.setPosition(0);
 			}
-			//motorController.set(mp.getStartDistance());
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.set(ControlMode.Position, mp.getStartDistance());
+			motorController.setWorld(ControlMode.Position, mp.getStartDistance());
 		}
 	}
 	
-	//public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
-	//	controlMode = MPControlMode.STRAIGHT;
-	//	this.startGyroAngle = desiredAngle;
-	//	this.useGyroLock = useGyroLock;
-	//	
-	//	// Set up the motion profile 
-	//	mp = MotionProfileCache.getInstance().getMP(key);
-	//	for (CANTalonEncoder motorController : motorControllers) {
-	//		if (resetEncoder) {
-	//			motorController.setPosition(0);
-	//		}
-	//		motorController.set(mp.getStartDistance());
-	//		motorController.changeControlMode(TalonControlMode.Position);
-	//	}
-	//}
+	public double getStartPosition() {
+		return mp != null ? mp.getStartDistance() : 0;
+	}
+	
+	public double getTargetPosition() {
+		return mp != null ? mp.getTargetDistance() : 0;
+	}
+	
+	public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
+		controlMode = MPControlMode.STRAIGHT;
+		this.startGyroAngle = desiredAngle;
+		this.useGyroLock = useGyroLock;
+		
+		// Set up the motion profile 
+		mp = MotionProfileCache.getInstance().getMP(key);
+		for (TalonSRXEncoder motorController : motorControllers) {
+			if (resetEncoder) {
+				motorController.setPosition(0);
+			}
+			motorController.setWorld(ControlMode.Position, mp.getStartDistance());
+		}
+	}
 	
 	public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPTalonTurnType turnType, double trackWidth) {
 		controlMode = MPControlMode.TURN;
@@ -93,11 +100,8 @@ public class MPTalonPIDController
 
 		// Set up the motion profile 
 		mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2);
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 			motorController.set(ControlMode.Position, 0);
 		}
 		
@@ -121,26 +125,21 @@ public class MPTalonPIDController
 	}
 	
 	public void setZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 			motorController.set(ControlMode.Position, 0);
 		}
 	}
 
 	public void resetZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 		}
 	}
 
-	public void resetZeroPosition(double angle) {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(angle);
-			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+	public void resetZeroPosition(double position) {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(position);
 		}
 	}
 
@@ -159,29 +158,24 @@ public class MPTalonPIDController
 		// Calculate the motion profile feed forward and gyro feedback terms
 		double KfLeft = 0.0;
 		double KfRight = 0.0;
-
+		
 		// Update the set points and Kf gains
 		if (controlMode == MPControlMode.STRAIGHT) {
 			double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0;
 			if (Math.abs(mpPoint.position) > 0.001) {
-				//KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
-				//KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position;
-			
-				KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
+				KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
 				KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position;
 			}
 			
 			// Update the controllers Kf and set point.
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.setF(KfRight);
-					motorController.config_kF(0, KfRight, 0);	//TODO: verify
-					motorController.setWorld(-mpPoint.position);
+					motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+					motorController.setWorld(ControlMode.Position, mpPoint.position);
 				}
 				else {
-					//motorController.setF(KfLeft);
-					motorController.config_kF(0, KfLeft, 0);	//TODO: verify
-					motorController.setWorld(mpPoint.position);
+					motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+					motorController.setWorld(ControlMode.Position, mpPoint.position);
 				}
 			}
 		}
@@ -194,31 +188,27 @@ public class MPTalonPIDController
 				KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
 			}
 			
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (turnType == MPTalonTurnType.TANK) {
 					if (motorController.isRight()) {
-						//motorController.setF(KfRight);
-						motorController.config_kF(0, KfRight, 0);	//TODO: verify
-						motorController.setWorld(-mpPoint.position);
+						motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, -mpPoint.position);
 					}
 					else {
-						//motorController.setF(KfLeft);
-						motorController.config_kF(0, KfLeft, 0);	//TODO: verify
-						motorController.setWorld(mpPoint.position);
+						motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, mpPoint.position);
 					}
 				}
 				else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) {
 					if (!motorController.isRight()) {
-						//motorController.setF(KfLeft);
-						motorController.config_kF(0, KfLeft, 0);	//TODO: verify
-						motorController.setWorld(mpPoint.position);
+						motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, mpPoint.position);
 					}
 				}
 				else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) {
 					if (motorController.isRight()) {
-						//motorController.setF(KfRight);
-						motorController.config_kF(0, KfRight, 0);	//TODO: verify
-						motorController.setWorld(-mpPoint.position);
+						motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, -mpPoint.position);
 					}
 				}
 			}
@@ -227,7 +217,17 @@ public class MPTalonPIDController
 		return false;
 	}
 	
+	public void setTarget(double position, double Kf) {
+		// Kf gets multipled by position in the Talon 
+		double KfPerPosition = Math.abs(position) > 0.001 ? Kf / position : 0;
+
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.config_kF(0, KfPerPosition, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.setWorld(ControlMode.Position, position);
+		}
+	}
+	
 	public MotionProfilePoint getCurrentPoint() {
 		return mpPoint;
 	}
-}
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java
index b9e8fca..235bb91 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java
@@ -2,21 +2,20 @@ package org.usfirst.frc4388.utility;
 
 import java.util.ArrayList;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
-import jaci.pathfinder.Trajectory.Segment;
 
 public class MPTalonPIDPathController
 {	
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected long periodMs;
 	protected PIDParams pidParams;	
-	protected PathGenerator path;
 	protected double startGyroAngle;
 	protected double targetGyroAngle;
 	protected double trackDistance;
 	
-	public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		this.periodMs = periodMs;
@@ -26,87 +25,49 @@ public class MPTalonPIDPathController
 	public void setPID(PIDParams pidParams) {
 		this.pidParams = pidParams;
 		
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
-			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
-			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPID(0, pidParams.kP, pidParams.kI, pidParams.kD);
 		}
 	}
-	
-	public void setMPPathTarget(PathGenerator path) {
-		this.path = path;
-		path.resetCounter();
 		
-		// Set up the motion profile 
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			motorController.set(ControlMode.Position, 0);
-		}
-	}
-	
 	public void setZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 			motorController.set(ControlMode.Position, 0);
 		}
 	}
 
 	public void resetZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 		}
 	}
 
 	public void resetZeroPosition(double angle) {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(angle);
-			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
 		}
 	}
 
 	public boolean controlLoopUpdate(double currentGyroAngle) {
-		Segment leftPoint = path.getLeftPoint();
-		Segment rightPoint = path.getRightPoint();
-		
-		// Check if we are finished
-		if (leftPoint == null) {
-			return true;
-		}
 		
 		// Calculate the motion profile feed forward and gyro feedback terms
 		double KfLeft = 0.0;
 		double KfRight = 0.0;
 
 		// Update the set points and Kf gains
-		double gyroDelta = -path.getHeading() - currentGyroAngle;
-		if (Math.abs(leftPoint.position) > 0.001) {
-			KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position;
-			KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position;
-		}
 		
 		// Update the controllers Kf and set point.
-		for (CANTalonEncoder motorController : motorControllers) {
+		for (TalonSRXEncoder motorController : motorControllers) {
 			if (motorController.isRight()) {
-				//motorController.setF(KfRight);
-				motorController.config_kF(0, KfRight, 0);	//TODO: verify
-				motorController.setWorld(rightPoint.position);
+				motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+//				motorController.setWorld(ControlMode.Position, rightPoint.position);
 			}
 			else {
-				//motorController.setF(KfLeft);
-				motorController.config_kF(0, KfLeft, 0);	//TODO: verify
-				motorController.setWorld(leftPoint.position);
+				motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
 			}
 		}
 		
-		path.incrementCounter();
 		
 		return false;
 	}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java
index 9863dc4..35b48cd 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java
@@ -2,21 +2,21 @@ package org.usfirst.frc4388.utility;
 
 import java.util.ArrayList;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
-import jaci.pathfinder.Trajectory.Segment;
 
 public class MPTalonPIDPathVelocityController
 {	
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected long periodMs;
 	protected PIDParams pidParams;	
-	protected PathGenerator path;
+//	protected PathGenerator path;
 	protected double startGyroAngle;
 	protected double targetGyroAngle;
 	protected double trackDistance;
 	
-	public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		this.periodMs = periodMs;
@@ -26,86 +26,46 @@ public class MPTalonPIDPathVelocityController
 	public void setPID(PIDParams pidParams) {
 		this.pidParams = pidParams;
 		
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
-			//motorController.setF(pidParams.kF);
-			//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
-			//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
-			//motorController.setProfile(0);
-			motorController.config_kP(0, pidParams.kP, 0);
-			motorController.config_kI(0, pidParams.kI, 0);
-			motorController.config_kD(0, pidParams.kD, 0);
-			motorController.config_kF(0, pidParams.kF, 0);
-			motorController.configNominalOutputForward(+0.0f, 0);
-			motorController.configNominalOutputReverse(-0.0f, 0);
-			motorController.configPeakOutputForward(+1.0f, 0);
-			motorController.configPeakOutputReverse(-1.0f, 0);
-			motorController.selectProfileSlot(0, 0);
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF);
+			motorController.configVoltageCompSaturation(12.0, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.enableVoltageCompensation(true);
+			motorController.configNominalOutputForward(0.0, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.configNominalOutputReverse(0.0, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.configPeakOutputForward(+1.0f, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.configPeakOutputReverse(-1.0f, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.selectProfileSlot(0, TalonSRXEncoder.PID_IDX);
 		}
 	}
 	
-	public void setMPPathTarget(PathGenerator path) {
-		this.path = path;
-		path.resetCounter();
-		
-		// Set up the motion profile 
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Speed);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			motorController.set(ControlMode.Velocity, 0);
-		}
-	}
 	
 	public void setZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(0);
-			//motorController.set(0);
-			//motorController.changeControlMode(TalonControlMode.Position);
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 			motorController.set(ControlMode.Position, 0);
 		}
 	}
 
 	public void resetZeroPosition() {
-		for (CANTalonEncoder motorController : motorControllers) {
-			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
 		}
 	}
 
 	public void resetZeroPosition(double angle) {
-		for (CANTalonEncoder motorController : motorControllers) {
-			//motorController.setPosition(angle);
-			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
 		}
-	}
+//	}
 
-	public boolean controlLoopUpdate(double currentGyroAngle) {
-		Segment leftPoint = path.getLeftPoint();
-		Segment rightPoint = path.getRightPoint();
-		
-		// Check if we are finished
-		if (leftPoint == null) {
-			return true;
-		}
-		
-		// Calculate the motion profile feed forward and gyro feedback terms
-		double rightVelocity = rightPoint.velocity;
-		double leftVelocity = leftPoint.velocity;
 		
 		// Update the controllers Kf and set point.
-		for (CANTalonEncoder motorController : motorControllers) {
+		for (TalonSRXEncoder motorController : motorControllers) {
 			if (motorController.isRight()) {
-				motorController.setVelocityWorld(rightVelocity);
 			}
 			else {
-				motorController.setVelocityWorld(leftVelocity);
 			}
 		}
 		
-		path.incrementCounter();
-		
-		return false;
 	}
 }
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java
index eb2367c..9e08b13 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java
@@ -171,7 +171,7 @@ public class MotionProfileBoxCar
 	public static void main(String[] args) {
 		long startTime = System.nanoTime();
 		
-		MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 96, 120, 10, 600, 300);
+		MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 70, 120, 10, 600, 300);
 		System.out.println("Time, Position, Velocity, Acceleration");
 		MotionProfilePoint point = new MotionProfilePoint();
 		while(mp.getNextPoint(point) != null) {
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java
new file mode 100644
index 0000000..8ff5f91
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc4388.utility;
+
+import java.text.DecimalFormat;
+import java.util.Hashtable;
+
+public class MotionProfileCache {
+
+	private Hashtable<String, MotionProfileBoxCar> cache;
+	private DecimalFormat df = new DecimalFormat("#.000"); 
+	private static MotionProfileCache instance;
+
+	private MotionProfileCache() {
+		cache = new Hashtable<String, MotionProfileBoxCar>();
+	}
+	
+	public String addMP(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) {
+		String key = generateKey(startDistance, targetDistance, maxVelocity, itp, t1, t2);
+		
+		if (!cache.containsKey(key)) {
+			MotionProfileBoxCar mp = new MotionProfileBoxCar(startDistance, targetDistance, maxVelocity, itp, t1, t2);
+			this.addMP(key, mp);
+		}
+		
+		return key;
+	}
+	
+	public void addMP(String key, MotionProfileBoxCar mp) {
+		cache.put(key, mp);
+	}
+	
+	public MotionProfileBoxCar getMP(String key) {
+		return cache.get(key);
+	}
+	
+	public static MotionProfileCache getInstance() {
+		if(instance == null) {
+			instance = new MotionProfileCache();
+		}
+		return instance;
+	}
+	
+	public void release() {
+		instance = null;
+	}
+	
+	private String generateKey(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) {
+		return df.format(startDistance) + df.format(targetDistance) + df.format(maxVelocity) + df.format(itp) + df.format(t1) + df.format(t2);
+	}
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java
index c989f65..9daee69 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java
@@ -15,11 +15,6 @@ public class PIDParams
     public PIDParams() {	
     }
     
-    public PIDParams(double kP)
-    {
-    	this.kP = kP;
-    }
-    
     public PIDParams(double kP, double kI, double kD) {
         this.kP = kP;
         this.kI = kI;
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java
deleted file mode 100644
index 163c52d..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java
+++ /dev/null
@@ -1,260 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import java.util.ArrayList;
-import java.util.HashSet;
-import java.util.Iterator;
-import java.util.List;
-import java.util.Optional;
-import java.util.Set;
-
-import org.usfirst.frc4388.utility.Path.Waypoint;
-
-/**
- * A Path is a recording of the path that the robot takes. Path objects consist
- * of a List of Waypoints that the robot passes by. Using multiple Waypoints in
- * a Path object and the robot's current speed, the code can extrapolate future
- * Waypoints and predict the robot's motion. It can also dictate the robot's
- * motion along the set path.
- */
-public class Path {
-    protected static final double kSegmentCompletePercentage = .99;
-
-    protected List<Waypoint> mWaypoints;
-    protected List<PathSegment> mSegments;
-    protected Set<String> mMarkersCrossed;
-
-    /**
-     * A point along the Path, which consists of the location, the speed, and a
-     * string marker (that future code can identify). Paths consist of a List of
-     * Waypoints.
-     */
-    public static class Waypoint {
-        public final Translation2d position;
-        public final double speed;
-        public final Optional<String> marker;
-
-        public Waypoint(Translation2d position, double speed) {
-            this.position = position;
-            this.speed = speed;
-            this.marker = Optional.empty();
-        }
-
-        public Waypoint(Translation2d position, double speed, String marker) {
-            this.position = position;
-            this.speed = speed;
-            this.marker = Optional.of(marker);
-        }
-    }
-
-    public Path(List<Waypoint> waypoints) {
-        mMarkersCrossed = new HashSet<String>();
-        mWaypoints = waypoints;
-        mSegments = new ArrayList<PathSegment>();
-        for (int i = 0; i < waypoints.size() - 1; ++i) {
-            mSegments.add(
-                    new PathSegment(waypoints.get(i).position, waypoints.get(i + 1).position, waypoints.get(i).speed));
-        }
-        // The first waypoint is already complete
-        if (mWaypoints.size() > 0) {
-            Waypoint first_waypoint = mWaypoints.get(0);
-            if (first_waypoint.marker.isPresent()) {
-                mMarkersCrossed.add(first_waypoint.marker.get());
-            }
-            mWaypoints.remove(0);
-        }
-    }
-
-    /**
-     * @param An
-     *            initial position
-     * @return Returns the distance from the position to the first point on the
-     *         path
-     */
-    public double update(Translation2d position) {
-        double rv = 0.0;
-        for (Iterator<PathSegment> it = mSegments.iterator(); it.hasNext();) {
-            PathSegment segment = it.next();
-            PathSegment.ClosestPointReport closest_point_report = segment.getClosestPoint(position);
-            if (closest_point_report.index >= kSegmentCompletePercentage) {
-                it.remove();
-                if (mWaypoints.size() > 0) {
-                    Waypoint waypoint = mWaypoints.get(0);
-                    if (waypoint.marker.isPresent()) {
-                        mMarkersCrossed.add(waypoint.marker.get());
-                    }
-                    mWaypoints.remove(0);
-                }
-            } else {
-                if (closest_point_report.index > 0.0) {
-                    // Can shorten this segment
-                    segment.updateStart(closest_point_report.closest_point);
-                }
-                // We are done
-                rv = closest_point_report.distance;
-                // ...unless the next segment is closer now
-                if (it.hasNext()) {
-                    PathSegment next = it.next();
-                    PathSegment.ClosestPointReport next_closest_point_report = next.getClosestPoint(position);
-                    if (next_closest_point_report.index > 0
-                            && next_closest_point_report.index < kSegmentCompletePercentage
-                            && next_closest_point_report.distance < rv) {
-                        next.updateStart(next_closest_point_report.closest_point);
-                        rv = next_closest_point_report.distance;
-                        mSegments.remove(0);
-                        if (mWaypoints.size() > 0) {
-                            Waypoint waypoint = mWaypoints.get(0);
-                            if (waypoint.marker.isPresent()) {
-                                mMarkersCrossed.add(waypoint.marker.get());
-                            }
-                            mWaypoints.remove(0);
-                        }
-                    }
-                }
-                break;
-            }
-        }
-        return rv;
-    }
-
-    public Set<String> getMarkersCrossed() {
-        return mMarkersCrossed;
-    }
-
-    public double getRemainingLength() {
-        double length = 0.0;
-        for (int i = 0; i < mSegments.size(); ++i) {
-            length += mSegments.get(i).getLength();
-        }
-        return length;
-    }
-
-    /**
-     * @param The
-     *            robot's current position
-     * @param A
-     *            specified distance to predict a future waypoint
-     * @return A segment of the robot's predicted motion with start/end points
-     *         and speed.
-     */
-    public PathSegment.Sample getLookaheadPoint(Translation2d position, double lookahead_distance) {
-        if (mSegments.size() == 0) {
-            return new PathSegment.Sample(new Translation2d(), 0);
-        }
-
-        // Check the distances to the start and end of each segment. As soon as
-        // we find a point > lookahead_distance away, we know the right point
-        // lies somewhere on that segment.
-        Translation2d position_inverse = position.inverse();
-        if (position_inverse.translateBy(mSegments.get(0).getStart()).norm() >= lookahead_distance) {
-            // Special case: Before the first point, so just return the first
-            // point.
-            return new PathSegment.Sample(mSegments.get(0).getStart(), mSegments.get(0).getSpeed());
-        }
-        for (int i = 0; i < mSegments.size(); ++i) {
-            PathSegment segment = mSegments.get(i);
-            double distance = position_inverse.translateBy(segment.getEnd()).norm();
-            if (distance >= lookahead_distance) {
-                // This segment contains the lookahead point
-                Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(segment, position,
-                        lookahead_distance);
-                if (intersection_point.isPresent()) {
-                    return new PathSegment.Sample(intersection_point.get(), segment.getSpeed());
-                } else {
-                    System.out.println("ERROR: No intersection point?");
-                }
-            }
-        }
-        // Special case: After the last point, so extrapolate forward.
-        PathSegment last_segment = mSegments.get(mSegments.size() - 1);
-        PathSegment new_last_segment = new PathSegment(last_segment.getStart(), last_segment.interpolate(10000),
-                last_segment.getSpeed());
-        Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(new_last_segment, position,
-                lookahead_distance);
-        if (intersection_point.isPresent()) {
-            return new PathSegment.Sample(intersection_point.get(), last_segment.getSpeed());
-        } else {
-            System.out.println("ERROR: No intersection point anywhere on line?");
-            return new PathSegment.Sample(last_segment.getEnd(), last_segment.getSpeed());
-        }
-    }
-
-    static Optional<Translation2d> getFirstCircleSegmentIntersection(PathSegment segment, Translation2d center,
-            double radius) {
-        double x1 = segment.getStart().getX() - center.getX();
-        double y1 = segment.getStart().getY() - center.getY();
-        double x2 = segment.getEnd().getX() - center.getX();
-        double y2 = segment.getEnd().getY() - center.getY();
-        double dx = x2 - x1;
-        double dy = y2 - y1;
-        double dr_squared = dx * dx + dy * dy;
-        double det = x1 * y2 - x2 * y1;
-
-        double discriminant = dr_squared * radius * radius - det * det;
-        if (discriminant < 0) {
-            // No intersection
-            return Optional.empty();
-        }
-
-        double sqrt_discriminant = Math.sqrt(discriminant);
-        Translation2d pos_solution = new Translation2d(
-                (det * dy + (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
-                (-det * dx + Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
-        Translation2d neg_solution = new Translation2d(
-                (det * dy - (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
-                (-det * dx - Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
-
-        // Choose the one between start and end that is closest to start
-        double pos_dot_product = segment.dotProduct(pos_solution);
-        double neg_dot_product = segment.dotProduct(neg_solution);
-        if (pos_dot_product < 0 && neg_dot_product >= 0) {
-            return Optional.of(neg_solution);
-        } else if (pos_dot_product >= 0 && neg_dot_product < 0) {
-            return Optional.of(pos_solution);
-        } else {
-            if (Math.abs(pos_dot_product) <= Math.abs(neg_dot_product)) {
-                return Optional.of(pos_solution);
-            } else {
-                return Optional.of(neg_solution);
-            }
-        }
-    }
-    
-    public static void addCircleArc(List<Waypoint> waypoints, double radius, double angleDeg, int numPoints, String endMarker ) {
-    	Waypoint last = waypoints.get(waypoints.size() - 1);
-    	
-    	double centerX = last.position.x_;
-    	double centerY = radius;
-    	
-    	double deltaAngle = angleDeg / numPoints;
-    	double currentAngle = deltaAngle;
-    	for (int i = 0; i < numPoints; i++ ) {
-    		double x = radius * Math.sin(Math.toRadians(currentAngle)) + centerX;
-    		double y = centerY - radius * Math.cos(Math.toRadians(currentAngle));
-    		
-    		if (i == numPoints - 1 && endMarker != null) {
-    			Waypoint point = new Waypoint(new Translation2d(x, y), last.speed, endMarker);
-    			waypoints.add(point);
-    		}
-    		else {
-    			Waypoint point = new Waypoint(new Translation2d(x, y), last.speed);
-    			waypoints.add(point);
-    		}
-    		
-    		currentAngle += deltaAngle;
-    	}
-    }
-    
-	public static void main(String[] args) {
-		
-        List<Waypoint> waypoints = new ArrayList<>();
-        waypoints.add(new Waypoint(new Translation2d(0, 0), 40.0));
-        waypoints.add(new Waypoint(new Translation2d(-35, 0), 40.0));
-        Path.addCircleArc(waypoints, 30.0, -45.0, 10, "hopperSensorOn");
-        waypoints.add(new Waypoint(new Translation2d(-85, 30), 40.0));
-        
-        for (int i = 0; i < waypoints.size(); i++) {
-        	Waypoint curPoint = waypoints.get(i);
-        	System.out.println("x = " + curPoint.position.x_ + ", y = " + curPoint.position.y_);
-        }
-	}
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java
deleted file mode 100644
index f99df73..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java
+++ /dev/null
@@ -1,232 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import java.awt.Color;
-
-import org.usfirst.frc4388.robot.subsystems.Drive;
-
-import jaci.pathfinder.Pathfinder;
-import jaci.pathfinder.Trajectory;
-import jaci.pathfinder.Trajectory.Segment;
-import jaci.pathfinder.Waypoint;
-import jaci.pathfinder.modifiers.TankModifier;
-
-public class PathGenerator {
-	
-	private Segment[] centerPoints;
-	private Segment[] leftPoints;
-	private Segment[] rightPoints;
-	private int curIndex = 0;
-	
-	public PathGenerator(Waypoint[] points, double timeStep, double maxVelocity, double maxAccel, double maxJerk) {
-
-		boolean negativeFlag = false;
-		for (int i = 0; i < points.length; i++) {
-        	if (points[i].x < 0) {
-        		negativeFlag = true;
-        	}
-        }
-		if (negativeFlag == true) {
-			for (int i = 0; i < points.length; i++) {
-	        	points[i].x = -(points[i].x);
-	        }
-		}
-		
-        Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_LOW, timeStep, maxVelocity, maxAccel, maxJerk);
-        Trajectory trajectory = Pathfinder.generate(points, config);
-        centerPoints = trajectory.segments;       
-
-	    TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES);
-	    leftPoints = modifier.getLeftTrajectory().segments;
-	    rightPoints = modifier.getRightTrajectory().segments; 
-
-        if (negativeFlag == true) {
-			for (int i = 0; i < centerPoints.length; i++) {
-				Segment curSeg = centerPoints[i];
-				curSeg.x = -(curSeg.x);
-				curSeg.y = -(curSeg.y);
-				curSeg.jerk = -(curSeg.jerk);
-				curSeg.acceleration = -(curSeg.acceleration);
-				curSeg.velocity = -(curSeg.velocity);
-				curSeg.position = -(curSeg.position);
-				curSeg.heading = -(curSeg.heading);
-				curSeg = leftPoints[i];
-				curSeg.x = -(curSeg.x);
-				curSeg.y = -(curSeg.y);
-				curSeg.jerk = -(curSeg.jerk);
-				curSeg.acceleration = -(curSeg.acceleration);
-				curSeg.velocity = -(curSeg.velocity);
-				curSeg.position = -(curSeg.position);
-				curSeg.heading = -(curSeg.heading);
-				curSeg = rightPoints[i];
-				curSeg.x = -(curSeg.x);
-				curSeg.y = -(curSeg.y);
-				curSeg.jerk = -(curSeg.jerk);
-				curSeg.acceleration = -(curSeg.acceleration);
-				curSeg.velocity = -(curSeg.velocity);
-				curSeg.position = -(curSeg.position);
-				curSeg.heading = -(curSeg.heading);
-			}
-		}
-		
-//      TankModifier2 modifier2 = new TankModifier2();
-//      modifier2.modify(centerPoints, Drive.TRACK_WIDTH_INCHES);
-//      leftPoints = modifier2.getLeftSegments();
-//      rightPoints = modifier2.getRightSegments();
-	}
-	
-	public Segment getLeftPoint() {
-		return (curIndex < leftPoints.length) ? leftPoints[curIndex] : null;
-	}
-	
-	public Segment getRightPoint() {
-		return (curIndex < rightPoints.length) ? rightPoints[curIndex] : null;
-	}
-	
-	public Segment getCenterPoint() {
-		return (curIndex < centerPoints.length) ? centerPoints[curIndex] : null;
-	}
-	
-	public Double getHeading() {
-		if (curIndex < centerPoints.length) {
-			double heading = Math.toDegrees(centerPoints[curIndex].heading);
-			if (heading > 180) {
-				heading -= 360;
-			}
-			else if (heading < -180) {
-				heading += 360;
-			}
-			return heading;
-		}
-		return null;
-	}
-	
-	public void incrementCounter() {
-		curIndex++;
-	}
-	
-	public void resetCounter() {
-		curIndex =0;
-	}
-	
-	public Segment[] getCenterPoints() {
-		return centerPoints;
-	}
-	
-	public Segment[] getLeftPoints() {
-		return leftPoints;
-	}
-	
-	public Segment[] getRightPoints() {
-		return rightPoints;
-	}
-	
-    public static void main(String[] args) {
-        Waypoint[] points = new Waypoint[] {
-                new Waypoint(0, 0, 0),
-                new Waypoint(78, 20, Pathfinder.d2r(32))
-        };
-
-        PathGenerator path = new PathGenerator(points, 0.01, 120, 200.0, 700.0);
-        Segment[] centerSegments = path.getCenterPoints();
-        Segment[] leftSegments = path.getLeftPoints();
-        Segment[] rightSegments = path.getRightPoints();
-       
-        double[] heading = new double[centerSegments.length];
-        double[] centerX = new double[centerSegments.length];
-        double[] centerY = new double[centerSegments.length];
-        double[] centerAccel = new double[centerSegments.length];
-        double[] centerVelocity = new double[centerSegments.length];
-        double[] centerPosition = new double[centerSegments.length];
-        double[] leftX = new double[leftSegments.length];
-        double[] leftY = new double[leftSegments.length];
-        double[] leftAccel = new double[leftSegments.length];
-        double[] leftVelocity = new double[leftSegments.length];
-        double[] leftPosition = new double[leftSegments.length];
-        double[] rightX = new double[rightSegments.length];
-        double[] rightY = new double[rightSegments.length];
-        double[] rightAccel = new double[rightSegments.length];
-        double[] rightVelocity = new double[rightSegments.length];
-        double[] rightPosition = new double[rightSegments.length];
-        
-        path.resetCounter();
-        for (int i = 0; i < centerSegments.length; i++) {
-        	heading[i] = path.getHeading();
-        	centerX[i] = path.getCenterPoint().x;
-        	centerY[i] = path.getCenterPoint().y;
-        	centerAccel[i] = path.getCenterPoint().acceleration;
-        	centerVelocity[i] = path.getCenterPoint().velocity;
-        	centerPosition[i] = path.getCenterPoint().position;
-        	leftX[i] = path.getLeftPoint().x;
-        	leftY[i] = path.getLeftPoint().y;
-        	leftAccel[i] = path.getLeftPoint().acceleration;
-        	leftVelocity[i] = path.getLeftPoint().velocity;
-        	leftPosition[i] = path.getLeftPoint().position;
-        	rightX[i] = path.getRightPoint().x;
-        	rightY[i] = path.getRightPoint().y;
-        	rightAccel[i] = path.getRightPoint().acceleration;
-        	rightVelocity[i] = path.getRightPoint().velocity;
-        	rightPosition[i] = path.getRightPoint().position;
-        	path.incrementCounter();
-        }
-        
-		FalconLinePlot fig4 = new FalconLinePlot(centerX);
-		fig4.yGridOn();
-		fig4.xGridOn();
-		fig4.setYLabel("X (in)");
-		fig4.setXLabel("time");
-		fig4.setTitle("Position Profile X");
-		fig4.addData(rightX, Color.magenta);
-		fig4.addData(leftX, Color.blue);
-
-		FalconLinePlot fig0 = new FalconLinePlot(centerY);
-		fig0.yGridOn();
-		fig0.xGridOn();
-		fig0.setYLabel("Y (in)");
-		fig0.setXLabel("time");
-		fig0.setTitle("Position Profile Y");
-		fig0.addData(rightY, Color.magenta);
-		fig0.addData(leftY, Color.blue);
-
-		FalconLinePlot fig1 = new FalconLinePlot(centerPosition);
-		fig1.yGridOn();
-		fig1.xGridOn();
-		fig1.setYLabel("Position (in)");
-		fig1.setXLabel("time (seconds)");
-		fig1.setTitle("Position Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
-		fig1.addData(rightPosition, Color.magenta);
-		fig1.addData(leftPosition, Color.blue);
-
-		FalconLinePlot fig2 = new FalconLinePlot(centerVelocity, Color.green, Color.green);
-		fig2.yGridOn();
-		fig2.xGridOn();
-		fig2.setYLabel("Velocity (in/sec)");
-		fig2.setXLabel("time (seconds)");
-		fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
-		fig2.addData(rightVelocity, Color.magenta);
-		fig2.addData(leftVelocity, Color.blue);
-
-		FalconLinePlot fig3 = new FalconLinePlot(centerAccel, Color.green, Color.green);
-		fig3.yGridOn();
-		fig3.xGridOn();
-		fig3.setYLabel("Accel (in/sec^2)");
-		fig3.setXLabel("time (seconds)");
-		fig3.setTitle("Accel Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
-		fig3.addData(rightAccel, Color.magenta);
-		fig3.addData(leftAccel, Color.blue);
-
-		FalconLinePlot fig5 = new FalconLinePlot(heading);
-		fig5.yGridOn();
-		fig5.xGridOn();
-		fig5.setYLabel("Heading");
-		fig5.setXLabel("time");
-		fig5.setTitle("Heading Profile");
-
-		FalconLinePlot fig6 = new FalconLinePlot(centerX, centerY);
-		fig6.yGridOn();
-		fig6.xGridOn();
-		fig6.setYLabel("Y");
-		fig6.setXLabel("X");
-		fig6.setTitle("XY Profile");
-  }
-
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java
deleted file mode 100644
index df9499d..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java
+++ /dev/null
@@ -1,89 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-/**
- * A PathSegment consists of two Translation2d objects (the start and end
- * points) as well as the speed of the robot.
- *
- */
-public class PathSegment {
-    protected static final double kEpsilon = 1E-9;
-
-    public static class Sample {
-        public final Translation2d translation;
-        public final double speed;
-
-        public Sample(Translation2d translation, double speed) {
-            this.translation = translation;
-            this.speed = speed;
-        }
-    }
-
-    protected double mSpeed;
-    protected Translation2d mStart;
-    protected Translation2d mEnd;
-    protected Translation2d mStartToEnd; // pre-computed for efficiency
-    protected double mLength; // pre-computed for efficiency
-
-    public static class ClosestPointReport {
-        public double index; // Index of the point on the path segment (not
-                             // clamped to [0, 1])
-        public double clamped_index; // As above, but clamped to [0, 1]
-        public Translation2d closest_point; // The result of
-                                            // interpolate(clamped_index)
-        public double distance; // The distance from closest_point to the query
-                                // point
-    }
-
-    public PathSegment(Translation2d start, Translation2d end, double speed) {
-        mEnd = end;
-        mSpeed = speed;
-        updateStart(start);
-    }
-
-    public void updateStart(Translation2d new_start) {
-        mStart = new_start;
-        mStartToEnd = mStart.inverse().translateBy(mEnd);
-        mLength = mStartToEnd.norm();
-    }
-
-    public double getSpeed() {
-        return mSpeed;
-    }
-
-    public Translation2d getStart() {
-        return mStart;
-    }
-
-    public Translation2d getEnd() {
-        return mEnd;
-    }
-
-    public double getLength() {
-        return mLength;
-    }
-
-    // Index is on [0, 1]
-    public Translation2d interpolate(double index) {
-        return mStart.interpolate(mEnd, index);
-    }
-
-    public double dotProduct(Translation2d other) {
-        Translation2d start_to_other = mStart.inverse().translateBy(other);
-        return mStartToEnd.getX() * start_to_other.getX() + mStartToEnd.getY() * start_to_other.getY();
-    }
-
-    public ClosestPointReport getClosestPoint(Translation2d query_point) {
-        ClosestPointReport rv = new ClosestPointReport();
-        if (mLength > kEpsilon) {
-            double dot_product = dotProduct(query_point);
-            rv.index = dot_product / (mLength * mLength);
-            rv.clamped_index = Math.min(1.0, Math.max(0.0, rv.index));
-            rv.closest_point = interpolate(rv.index);
-        } else {
-            rv.index = rv.clamped_index = 0.0;
-            rv.closest_point = new Translation2d(mStart);
-        }
-        rv.distance = rv.closest_point.inverse().translateBy(query_point).norm();
-        return rv;
-    }
-}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java
new file mode 100644
index 0000000..341dfd0
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java
@@ -0,0 +1,74 @@
+package org.usfirst.frc4388.utility;
+
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.lang.reflect.Field;
+import java.util.concurrent.ConcurrentLinkedDeque;
+
+/**
+ * Writes data to a CSV file
+ */
+public class ReflectingCSVWriter<T> {
+    ConcurrentLinkedDeque<String> mLinesToWrite = new ConcurrentLinkedDeque<>();
+    PrintWriter mOutput = null;
+    Field[] mFields;
+
+    public ReflectingCSVWriter(String fileName, Class<T> typeClass) {
+        mFields = typeClass.getFields();
+        try {
+            mOutput = new PrintWriter(fileName);
+        } catch (FileNotFoundException e) {
+            e.printStackTrace();
+        }
+        // Write field names.
+        StringBuffer line = new StringBuffer();
+        for (Field field : mFields) {
+            if (line.length() != 0) {
+                line.append(", ");
+            }
+            line.append(field.getName());
+        }
+        writeLine(line.toString());
+    }
+
+    public void add(T value) {
+        StringBuffer line = new StringBuffer();
+        for (Field field : mFields) {
+            if (line.length() != 0) {
+                line.append(", ");
+            }
+            try {
+                line.append(field.get(value).toString());
+            } catch (IllegalArgumentException e) {
+                e.printStackTrace();
+            } catch (IllegalAccessException e) {
+                e.printStackTrace();
+            }
+        }
+        mLinesToWrite.add(line.toString());
+    }
+
+    protected synchronized void writeLine(String line) {
+        if (mOutput != null) {
+            mOutput.println(line);
+        }
+    }
+
+    // Call this periodically from any thread to write to disk.
+    public void write() {
+        while (true) {
+            String val = mLinesToWrite.pollFirst();
+            if (val == null) {
+                break;
+            }
+            writeLine(val);
+        }
+    }
+
+    public synchronized void flush() {
+        if (mOutput != null) {
+            write();
+            mOutput.flush();
+        }
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java
index 7bd6abd..3e41b63 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java
@@ -4,12 +4,12 @@ import java.util.ArrayList;
 
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
+//import com.ctre.CANTalon.TalonControlMode;
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 public class SoftwarePIDController
 {	
-	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
 	protected PIDParams pidParams;	
 	protected boolean useGyroLock;
 	protected double targetGyroAngle;
@@ -21,7 +21,7 @@ public class SoftwarePIDController
 	protected double prevError = 0.0; // the prior error (used to compute velocity)
 	protected double totalError = 0.0; // the sum of the errors for use in the integral calc
 	
-	public SoftwarePIDController( PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	public SoftwarePIDController( PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
 	{
 		this.motorControllers = motorControllers;
 		setPID(pidParams);
@@ -31,22 +31,15 @@ public class SoftwarePIDController
 		this.pidParams = pidParams;
 	}
 	
-	
 	public void setPIDTurnTarget(double targetAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
 		this.turnType = turnType;
 		this.targetGyroAngle = targetAngleDeg;
 		this.useGyroLock = true;
 		this.maxError = maxError;
 		this.maxPrevError = maxPrevError;
-		
-		// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
-		// for (CANTalonEncoder motorController : motorControllers) {
-		//     motorController.changeControlMode(TalonControlMode.PercentVbus);
-		// }
-		
+				
 		prevError = 0.0;
 		totalError = 0.0;
-	
 	}
 	
 	public boolean controlLoopUpdate() {
@@ -63,7 +56,6 @@ public class SoftwarePIDController
 			return true;
 		}
 		
-		
 		if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) {
 			totalError += error;
 		}
@@ -79,32 +71,27 @@ public class SoftwarePIDController
 			
 		// Update the controllers set point.
 		if (turnType == MPSoftwareTurnType.TANK) {
-			for (CANTalonEncoder motorController : motorControllers) {
-				//motorController.set(output);
+			for (TalonSRXEncoder motorController : motorControllers) {
 				motorController.set(ControlMode.PercentOutput, output);
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(0);
 					motorController.set(ControlMode.PercentOutput, 0);
 				}
 				else {
-					//motorController.set(2.0 * output);
-					motorController.set(ControlMode.PercentOutput, 2.0 * output);
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
 				}
 			}
 		}
 		else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
-			for (CANTalonEncoder motorController : motorControllers) {
+			for (TalonSRXEncoder motorController : motorControllers) {
 				if (motorController.isRight()) {
-					//motorController.set(2.0 * output);
 					motorController.set(ControlMode.PercentOutput, 2.0 * output);
 				}
 				else {
-					//motorController.set(0);					
-					motorController.set(ControlMode.PercentOutput, 0);
+					motorController.set(ControlMode.PercentOutput, 0);					
 				}
 			}
 		}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java
deleted file mode 100644
index 383acd7..0000000
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java
+++ /dev/null
@@ -1,85 +0,0 @@
-
-package org.usfirst.frc4388.utility;
-
-import java.util.ArrayList;
-
-import org.usfirst.frc4388.robot.Robot;
-import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
-import edu.wpi.first.wpilibj.Timer;
-
-public class SoftwarePIDPositionController
-{	
-	//protected ArrayList<CANTalonEncoder> motorControllers;	
-	protected WPI_TalonSRX motorController;
-	protected PIDParams pidParams;
-	
-	protected PIDParams PValue;
-	
-	protected double targetEncoderPosition;
-	
-	protected double minTurnOutput = 0.002;
-	protected double maxError;
-	protected double minError;
-	protected double maxPrevError; ///??
-	protected double prevError = 0.0; // the prior error (used to compute velocity)
-	protected double totalError = 0.0; // the sum of the errors for use in the integral calc
-	
-	public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) 
-	{
-		this.motorController = elevatorLeft;
-		setP(PValue);
-	}
-    
-	public void setP(PIDParams PValue) 
-	{
-		this.PValue = PValue;
-	}
-	
-	public void setPIDPositionTarget(double targetPosition, double maxError, double minError)
-	{
-		this.targetEncoderPosition = targetPosition;
-		
-		this.maxError = maxError;
-		this.minError = minError;
-		//this.maxPrevError = maxPrevError;
-		
-		prevError = 0.0;
-		totalError = 0.0;
-	}
-	
-	public boolean controlLoopUpdate() 
-	{
-		return controlLoopUpdate(0);
-	}
-	
-	public boolean controlLoopUpdate(double currentEncoderPosition) 
-	{
-		// Calculate the motion profile feed forward and error feedback terms
-		double error = targetEncoderPosition - currentEncoderPosition;
-		//double deltaLastError = (error - prevError);
-		
-		//Updating the error
-		//totalError += error;
-		
-		// Check if we are finished
-		if (Math.abs(error) < maxError && Math.abs(error) > minError) 
-		{
-			//Robot.elevator.holdInPos();
-			
-			return true;
-		}
-		
-		double output =  pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError;
-		
-		prevError = error;
-		
-		// Update the controllers set point.
-		motorController.set(ControlMode.PercentOutput, output);
-		
-		return false;
-	}
-}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java
new file mode 100644
index 0000000..c7ef11a
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java
@@ -0,0 +1,84 @@
+package org.usfirst.frc4388.utility;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class TalonSRXEncoder extends WPI_TalonSRX
+{
+	public static int TIMEOUT_MS = 0;
+	public static int PID_IDX = 0;
+
+	private double encoderTicksToWorld;
+	private boolean isRight = true;
+	
+	public TalonSRXEncoder(int deviceId, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
+		this(deviceId, encoderTicksToWorld, false, feedbackDevice);
+	}
+
+	public TalonSRXEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
+		super(deviceNumber);
+		this.configSelectedFeedbackSensor(feedbackDevice, PID_IDX, TIMEOUT_MS);
+		this.encoderTicksToWorld = encoderTicksToWorld;
+		this.isRight = isRight;
+	}
+
+    public boolean isRight() {
+		return isRight;
+	}
+
+	public void setRight(boolean isRight) {
+		this.isRight = isRight;
+	}
+
+	public void setPID(int slotId, double kP, double kI, double kD) {
+		this.setPIDF(slotId, kP, kI, kD, 0);
+	}
+
+	public void setPIDF(int slotId, double kP, double kI, double kD, double kF) {
+		this.config_kP(slotId, kP, TIMEOUT_MS);
+		this.config_kI(slotId, kI, TIMEOUT_MS);
+		this.config_kD(slotId, kD, TIMEOUT_MS);
+		this.config_kF(slotId, kF, TIMEOUT_MS);
+	}
+	
+	public void setPIDFIZone(int slotId, double kP, double kI, double kD, double kF, int iZone) {
+		this.config_kP(slotId, kP, TIMEOUT_MS);
+		this.config_kI(slotId, kI, TIMEOUT_MS);
+		this.config_kD(slotId, kD, TIMEOUT_MS);
+		this.config_kF(slotId, kF, TIMEOUT_MS);
+		this.config_IntegralZone(slotId, iZone, TIMEOUT_MS);
+	}
+	
+	public double convertEncoderTicksToWorld(double encoderTicks) {
+    	return encoderTicks / encoderTicksToWorld;
+    }
+
+    public int convertEncoderWorldToTicks(double worldValue) {
+    	return (int)(worldValue * encoderTicksToWorld);
+    }
+    
+    public void setWorld(ControlMode mode, double worldValue) {
+    	this.set(mode, convertEncoderWorldToTicks(worldValue));
+    }
+    
+    public void setPosition(int value) {
+    	this.setSelectedSensorPosition(value, PID_IDX, TIMEOUT_MS);
+    }
+    
+    public void setPositionWorld(double worldValue) {
+    	this.setSelectedSensorPosition(convertEncoderWorldToTicks(worldValue), PID_IDX, TIMEOUT_MS);
+    }
+    
+    public double getPositionWorld() {
+    	return convertEncoderTicksToWorld(this.getSelectedSensorPosition(PID_IDX));
+    }
+    
+    public void setVelocityWorld(double worldValue) {
+    	this.set(ControlMode.Velocity, convertEncoderWorldToTicks(worldValue) * 0.1);
+    }
+    
+    public double getVelocityWorld() {
+    	return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1);
+    }
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java
new file mode 100644
index 0000000..64c20d1
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java
@@ -0,0 +1,208 @@
+package org.usfirst.frc4388.utility;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.StatusFrame;
+import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+
+import edu.wpi.first.wpilibj.MotorSafety;
+
+/**
+ * Creates TalonSRX objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor
+ * parameters are not set, as these are expected to be set by the application.
+ */
+public class TalonSRXFactory {
+
+    public static class Configuration {
+        public boolean LIMIT_SWITCH_NORMALLY_OPEN = true;
+        public double MAX_OUTPUT = 1;
+        public double NOMINAL_OUTPUT = 0;
+        public double PEAK_OUTPUT = 12;
+        public NeutralMode ENABLE_BRAKE = NeutralMode.Brake;
+        public boolean ENABLE_CURRENT_LIMIT = false;
+        public boolean ENABLE_SOFT_LIMIT = false;
+        public boolean ENABLE_LIMIT_SWITCH = false;
+        public int CURRENT_LIMIT = 0;
+       // public double EXPIRATION_TIMEOUT_SECONDS = MotorSafety.DEFAULT_SAFETY_EXPIRATION;
+        public double FORWARD_SOFT_LIMIT = 0;
+        public boolean INVERTED = false;
+        public double NOMINAL_CLOSED_LOOP_VOLTAGE = 12;
+        public double REVERSE_SOFT_LIMIT = 0;
+        public boolean SAFETY_ENABLED = false;
+
+        public int CONTROL_FRAME_PERIOD_MS = 5;
+        public int MOTION_CONTROL_FRAME_PERIOD_MS = 100;
+        public int GENERAL_STATUS_FRAME_RATE_MS = 5;
+        public int FEEDBACK_STATUS_FRAME_RATE_MS = 100;
+        public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 100;
+        public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 100;
+        public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 100;
+
+        public VelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = VelocityMeasPeriod.Period_100Ms;
+        public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64;
+
+        public double VOLTAGE_COMPENSATION_RAMP_RATE = 0;
+        public double VOLTAGE_RAMP_RATE = 0;
+    }
+
+    private static final Configuration kDefaultConfiguration = new Configuration();
+    private static final Configuration kSlaveConfiguration = new Configuration();
+
+    static {
+        kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 1000;
+        kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000;
+        kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000;
+        kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000;
+        kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000;
+        kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000;
+        kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000;
+    }
+
+    // Create a TalonSRX with the default (out of the box) configuration.
+    public static TalonSRX createDefaultTalon(int id) {
+        return createTalon(id, kDefaultConfiguration);
+    }
+
+    public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
+    	TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, feedbackDevice);
+//        initializeTalon(talon, kDefaultConfiguration);
+        return talon;
+    }
+
+    public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
+    	TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, isRight, feedbackDevice);
+//        initializeTalon(talon, kDefaultConfiguration);
+        return talon;
+    }
+
+    public static TalonSRX createPermanentSlaveTalon(int id, int master_id) {
+        final TalonSRX talon = createTalon(id, kSlaveConfiguration);
+        talon.set(ControlMode.Follower, master_id);
+        return talon;
+    }
+
+    public static TalonSRX createTalon(int id, Configuration config) {
+        TalonSRX talon = new TalonSRX(id);
+//        initializeTalon(talon, config);
+        return talon;
+    }
+    
+    public static void initializeTalon(TalonSRX talon, Configuration config) {
+//        talon.setControlFramePeriod(config.CONTROL_FRAME_PERIOD_MS, TalonSRXEncoder.TIMEOUT_MS);
+        talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS);
+        talon.setIntegralAccumulator(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS);
+        talon.clearMotionProfileHasUnderrun(TalonSRXEncoder.TIMEOUT_MS);
+        talon.clearMotionProfileTrajectories();
+        talon.clearStickyFaults(TalonSRXEncoder.TIMEOUT_MS);
+        talon.configPeakOutputForward(config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configPeakOutputReverse(-config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configNominalOutputForward(config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configNominalOutputReverse(-config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.setNeutralMode(config.ENABLE_BRAKE);
+        talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT);
+        talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS);
+        talon.setInverted(config.INVERTED);
+        talon.setSensorPhase(false);
+        talon.setSelectedSensorPosition(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS);
+        talon.selectProfileSlot(0, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, TalonSRXEncoder.TIMEOUT_MS);
+        talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, TalonSRXEncoder.TIMEOUT_MS);
+
+        talon.setStatusFramePeriod(StatusFrame.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
+        talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
+        talon.setStatusFramePeriod(StatusFrame.Status_4_AinTempVbat, config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
+
+//        talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.QuadEncoder, config.QUAD_ENCODER_STATUS_FRAME_RATE_MS);
+//        talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.PulseWidth, config.PULSE_WIDTH_STATUS_FRAME_RATE_MS);
+    }
+
+    
+    /**
+     * Run this on a fresh talon to produce good values for the defaults.
+     */
+    public static String getFullTalonInfo(TalonSRX talon) {
+        StringBuilder sb = new StringBuilder().append("isRevLimitSwitchClosed = ");
+//                .append(talon.isRevLimitSwitchClosed()).append("\n").append("getBusVoltage = ")
+//                .append(talon.getBusVoltage()).append("\n").append("isForwardSoftLimitEnabled = ")
+//                .append(talon.isForwardSoftLimitEnabled()).append("\n").append("getFaultRevSoftLim = ")
+//                .append(talon.getFaultRevSoftLim()).append("\n").append("getStickyFaultOverTemp = ")
+//                .append(talon.getStickyFaultOverTemp()).append("\n").append("isZeroSensorPosOnFwdLimitEnabled = ")
+//                .append(talon.isZeroSensorPosOnFwdLimitEnabled()).append("\n")
+//                .append("getMotionProfileTopLevelBufferCount = ").append(talon.getMotionProfileTopLevelBufferCount())
+//                .append("\n").append("getNumberOfQuadIdxRises = ").append(talon.getNumberOfQuadIdxRises()).append("\n")
+//                .append("getInverted = ").append(talon.getInverted()).append("\n")
+//                .append("getPulseWidthRiseToRiseUs = ").append(talon.getPulseWidthRiseToRiseUs()).append("\n")
+//                .append("getError = ").append(talon.getError()).append("\n").append("isSensorPresent = ")
+//                .append(talon.isSensorPresent(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)).append("\n")
+//                .append("isControlEnabled = ").append(talon.isControlEnabled()).append("\n").append("getTable = ")
+//                .append(talon.getTable()).append("\n").append("isEnabled = ").append(talon.isEnabled()).append("\n")
+//                .append("isZeroSensorPosOnRevLimitEnabled = ").append(talon.isZeroSensorPosOnRevLimitEnabled())
+//                .append("\n").append("isSafetyEnabled = ").append(talon.isSafetyEnabled()).append("\n")
+//                .append("getOutputVoltage = ").append(talon.getOutputVoltage()).append("\n").append("getTemperature = ")
+//                .append(talon.getTemperature()).append("\n").append("getSmartDashboardType = ")
+//                .append(talon.getSmartDashboardType()).append("\n").append("getPulseWidthPosition = ")
+//                .append(talon.getPulseWidthPosition()).append("\n").append("getOutputCurrent = ")
+//                .append(talon.getOutputCurrent()).append("\n").append("get = ").append(talon.get()).append("\n")
+//                .append("isZeroSensorPosOnIndexEnabled = ").append(talon.isZeroSensorPosOnIndexEnabled()).append("\n")
+//                .append("getMotionMagicCruiseVelocity = ").append(talon.getMotionMagicCruiseVelocity()).append("\n")
+//                .append("getStickyFaultRevSoftLim = ").append(talon.getStickyFaultRevSoftLim()).append("\n")
+//                .append("getFaultRevLim = ").append(talon.getFaultRevLim()).append("\n").append("getEncPosition = ")
+//                .append(talon.getEncPosition()).append("\n").append("getIZone = ").append(talon.getIZone()).append("\n")
+//                .append("getAnalogInPosition = ").append(talon.getAnalogInPosition()).append("\n")
+//                .append("getFaultUnderVoltage = ").append(talon.getFaultUnderVoltage()).append("\n")
+//                .append("getCloseLoopRampRate = ").append(talon.getCloseLoopRampRate()).append("\n")
+//                .append("toString = ").append(talon.toString()).append("\n")
+//                // .append("getMotionMagicActTrajPosition =
+//                // ").append(talon.getMotionMagicActTrajPosition()).append("\n")
+//                .append("getF = ").append(talon.getF()).append("\n").append("getClass = ").append(talon.getClass())
+//                .append("\n").append("getAnalogInVelocity = ").append(talon.getAnalogInVelocity()).append("\n")
+//                .append("getI = ").append(talon.getI()).append("\n").append("isReverseSoftLimitEnabled = ")
+//                .append(talon.isReverseSoftLimitEnabled()).append("\n").append("getPIDSourceType = ")
+//                .append(talon.getPIDSourceType()).append("\n").append("getEncVelocity = ")
+//                .append(talon.getEncVelocity()).append("\n").append("GetVelocityMeasurementPeriod = ")
+//                .append(talon.GetVelocityMeasurementPeriod()).append("\n").append("getP = ").append(talon.getP())
+//                .append("\n").append("GetVelocityMeasurementWindow = ").append(talon.GetVelocityMeasurementWindow())
+//                .append("\n").append("getDeviceID = ").append(talon.getDeviceID()).append("\n")
+//                .append("getStickyFaultRevLim = ").append(talon.getStickyFaultRevLim()).append("\n")
+//                // .append("getMotionMagicActTrajVelocity =
+//                // ").append(talon.getMotionMagicActTrajVelocity()).append("\n")
+//                .append("getReverseSoftLimit = ").append(talon.getReverseSoftLimit()).append("\n").append("getD = ")
+//                .append(talon.getD()).append("\n").append("getFaultOverTemp = ").append(talon.getFaultOverTemp())
+//                .append("\n").append("getForwardSoftLimit = ").append(talon.getForwardSoftLimit()).append("\n")
+//                .append("GetFirmwareVersion = ").append(talon.GetFirmwareVersion()).append("\n")
+//                .append("getLastError = ").append(talon.getLastError()).append("\n").append("isAlive = ")
+//                .append(talon.isAlive()).append("\n").append("getPinStateQuadIdx = ").append(talon.getPinStateQuadIdx())
+//                .append("\n").append("getAnalogInRaw = ").append(talon.getAnalogInRaw()).append("\n")
+//                .append("getFaultForLim = ").append(talon.getFaultForLim()).append("\n").append("getSpeed = ")
+//                .append(talon.getSpeed()).append("\n").append("getStickyFaultForLim = ")
+//                .append(talon.getStickyFaultForLim()).append("\n").append("getFaultForSoftLim = ")
+//                .append(talon.getFaultForSoftLim()).append("\n").append("getStickyFaultForSoftLim = ")
+//                .append(talon.getStickyFaultForSoftLim()).append("\n").append("getClosedLoopError = ")
+//                .append(talon.getClosedLoopError()).append("\n").append("getSetpoint = ").append(talon.getSetpoint())
+//                .append("\n").append("isMotionProfileTopLevelBufferFull = ")
+//                .append(talon.isMotionProfileTopLevelBufferFull()).append("\n").append("getDescription = ")
+//                .append(talon.getDescription()).append("\n").append("hashCode = ").append(talon.hashCode()).append("\n")
+//                .append("isFwdLimitSwitchClosed = ").append(talon.isFwdLimitSwitchClosed()).append("\n")
+//                .append("getPinStateQuadA = ").append(talon.getPinStateQuadA()).append("\n")
+//                .append("getPinStateQuadB = ").append(talon.getPinStateQuadB()).append("\n").append("GetIaccum = ")
+//                .append(talon.GetIaccum()).append("\n").append("getFaultHardwareFailure = ")
+//                .append(talon.getFaultHardwareFailure()).append("\n").append("pidGet = ").append(talon.pidGet())
+//                .append("\n").append("getBrakeEnableDuringNeutral = ").append(talon.getBrakeEnableDuringNeutral())
+//                .append("\n").append("getStickyFaultUnderVoltage = ").append(talon.getStickyFaultUnderVoltage())
+//                .append("\n").append("getPulseWidthVelocity = ").append(talon.getPulseWidthVelocity()).append("\n")
+//                .append("GetNominalClosedLoopVoltage = ").append(talon.GetNominalClosedLoopVoltage()).append("\n")
+//                .append("getPosition = ").append(talon.getPosition()).append("\n").append("getExpiration = ")
+//                .append(talon.getExpiration()).append("\n").append("getPulseWidthRiseToFallUs = ")
+//                .append(talon.getPulseWidthRiseToFallUs()).append("\n")
+//                // .append("createTableListener = ").append(talon.createTableListener()).append("\n")
+//                .append("getControlMode = ").append(talon.getControlMode()).append("\n")
+//                .append("getMotionMagicAcceleration = ").append(talon.getMotionMagicAcceleration()).append("\n")
+//                .append("getControlMode = ").append(talon.getControlMode());
+
+        return sb.toString();
+    }
+
+ }
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java
new file mode 100644
index 0000000..bcba002
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.List;
+
+/**
+ * Contains basic functions that are used often.
+ */
+public class Util {
+    /** Prevent this class from being instantiated. */
+    private Util() {
+    }
+
+    /**
+     * Limits the given input to the given magnitude.
+     */
+    public static double limit(double v, double maxMagnitude) {
+        return limit(v, -maxMagnitude, maxMagnitude);
+    }
+
+    public static double limit(double v, double min, double max) {
+        return Math.min(max, Math.max(min, v));
+    }
+
+    public static String joinStrings(String delim, List<?> strings) {
+        StringBuilder sb = new StringBuilder();
+        for (int i = 0; i < strings.size(); ++i) {
+            sb.append(strings.get(i).toString());
+            if (i < strings.size() - 1) {
+                sb.append(delim);
+            }
+        }
+        return sb.toString();
+    }
+
+    public static boolean epsilonEquals(double a, double b, double epsilon) {
+        return (a - epsilon <= b) && (a + epsilon >= b);
+    }
+
+    public static boolean allCloseTo(List<Double> list, double value, double epsilon) {
+        boolean result = true;
+        for (Double value_in : list) {
+            result &= epsilonEquals(value_in, value, epsilon);
+        }
+        return result;
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java
new file mode 100644
index 0000000..519e3b0
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java
@@ -0,0 +1,200 @@
+package org.usfirst.frc4388.utility.control;
+
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+import org.usfirst.frc4388.utility.math.Twist2d;
+
+/**
+ * Implements an adaptive pure pursuit controller. See:
+ * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 .pdf
+ * 
+ * Basically, we find a spot on the path we'd like to follow and calculate the arc necessary to make us land on that
+ * spot. The target spot is a specified distance ahead of us, and we look further ahead the greater our tracking error.
+ * We also return the maximum speed we'd like to be going when we reach the target spot.
+ */
+
+public class AdaptivePurePursuitController {
+    private static final double kReallyBigNumber = 1E6;
+
+    public static class Command {
+        public Twist2d delta = Twist2d.identity();
+        public double cross_track_error;
+        public double max_velocity;
+        public double end_velocity;
+        public Translation2d lookahead_point;
+        public double remaining_path_length;
+
+        public Command() {
+        }
+
+        public Command(Twist2d delta, double cross_track_error, double max_velocity, double end_velocity,
+                Translation2d lookahead_point, double remaining_path_length) {
+            this.delta = delta;
+            this.cross_track_error = cross_track_error;
+            this.max_velocity = max_velocity;
+            this.end_velocity = end_velocity;
+            this.lookahead_point = lookahead_point;
+            this.remaining_path_length = remaining_path_length;
+        }
+    }
+
+    Path mPath;
+    boolean mAtEndOfPath = false;
+    final boolean mReversed;
+    final Lookahead mLookahead;
+
+    public AdaptivePurePursuitController(Path path, boolean reversed, Lookahead lookahead) {
+        mPath = path;
+        mReversed = reversed;
+        mLookahead = lookahead;
+    }
+
+    /**
+     * Gives the RigidTransform2d.Delta that the robot should take to follow the path
+     * 
+     * @param pose
+     *            robot pose
+     * @return movement command for the robot to follow
+     */
+    public Command update(RigidTransform2d pose) {
+        if (mReversed) {
+            pose = new RigidTransform2d(pose.getTranslation(),
+                    pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
+        }
+
+        final Path.TargetPointReport report = mPath.getTargetPoint(pose.getTranslation(), mLookahead);
+        
+        if (isFinished()) {
+            // Stop.
+            return new Command(Twist2d.identity(), report.closest_point_distance, report.max_speed, 0.0,
+                    report.lookahead_point, report.remaining_path_distance);
+        }
+
+        final Arc arc = new Arc(pose, report.lookahead_point);
+        double scale_factor = 1.0;
+        // Ensure we don't overshoot the end of the path (once the lookahead speed drops to zero).
+        if (report.lookahead_point_speed < 1E-6 && report.remaining_path_distance < arc.length) {
+            scale_factor = Math.max(0.0, report.remaining_path_distance / arc.length);
+            mAtEndOfPath = true;
+        } else {
+            mAtEndOfPath = false;
+        }
+        if (mReversed) {
+            scale_factor *= -1;
+        }
+
+        return new Command(
+                new Twist2d(scale_factor * arc.length, 0.0,
+                        arc.length * getDirection(pose, report.lookahead_point) * Math.abs(scale_factor) / arc.radius),
+                report.closest_point_distance, report.max_speed,
+                report.lookahead_point_speed * Math.signum(scale_factor), report.lookahead_point,
+                report.remaining_path_distance);
+    }
+
+    public boolean hasPassedMarker(String marker) {
+        return mPath.hasPassedMarker(marker);
+    }
+
+    public static class Arc {
+        public Translation2d center;
+        public double radius;
+        public double length;
+
+        public Arc(RigidTransform2d pose, Translation2d point) {
+            center = getCenter(pose, point);
+            radius = new Translation2d(center, point).norm();
+            length = getLength(pose, point, center, radius);
+        }
+    }
+
+    /**
+     * Gives the center of the circle joining the lookahead point and robot pose
+     * 
+     * @param pose
+     *            robot pose
+     * @param point
+     *            lookahead point
+     * @return center of the circle joining the lookahead point and robot pose
+     */
+    public static Translation2d getCenter(RigidTransform2d pose, Translation2d point) {
+        final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5);
+        final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal();
+        final RigidTransform2d perpendicularBisector = new RigidTransform2d(poseToPointHalfway, normal);
+        final RigidTransform2d normalFromPose = new RigidTransform2d(pose.getTranslation(),
+                pose.getRotation().normal());
+        if (normalFromPose.isColinear(perpendicularBisector.normal())) {
+            // Special case: center is poseToPointHalfway.
+            return poseToPointHalfway;
+        }
+        return normalFromPose.intersection(perpendicularBisector);
+    }
+
+    /**
+     * Gives the radius of the circle joining the lookahead point and robot pose
+     * 
+     * @param pose
+     *            robot pose
+     * @param point
+     *            lookahead point
+     * @return radius of the circle joining the lookahead point and robot pose
+     */
+    public static double getRadius(RigidTransform2d pose, Translation2d point) {
+        Translation2d center = getCenter(pose, point);
+        return new Translation2d(center, point).norm();
+    }
+
+    /**
+     * Gives the length of the arc joining the lookahead point and robot pose (assuming forward motion).
+     * 
+     * @param pose
+     *            robot pose
+     * @param point
+     *            lookahead point
+     * @return the length of the arc joining the lookahead point and robot pose
+     */
+    public static double getLength(RigidTransform2d pose, Translation2d point) {
+        final double radius = getRadius(pose, point);
+        final Translation2d center = getCenter(pose, point);
+        return getLength(pose, point, center, radius);
+    }
+
+    public static double getLength(RigidTransform2d pose, Translation2d point, Translation2d center, double radius) {
+        if (radius < kReallyBigNumber) {
+            final Translation2d centerToPoint = new Translation2d(center, point);
+            final Translation2d centerToPose = new Translation2d(center, pose.getTranslation());
+            // If the point is behind pose, we want the opposite of this angle. To determine if the point is behind,
+            // check the sign of the cross-product between the normal vector and the vector from pose to point.
+            final boolean behind = Math.signum(
+                    Translation2d.cross(pose.getRotation().normal().toTranslation(),
+                            new Translation2d(pose.getTranslation(), point))) > 0.0;
+            final Rotation2d angle = Translation2d.getAngle(centerToPose, centerToPoint);
+            return radius * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians()));
+        } else {
+            return new Translation2d(pose.getTranslation(), point).norm();
+        }
+    }
+
+    /**
+     * Gives the direction the robot should turn to stay on the path
+     * 
+     * @param pose
+     *            robot pose
+     * @param point
+     *            lookahead point
+     * @return the direction the robot should turn: -1 is left, +1 is right
+     */
+    public static int getDirection(RigidTransform2d pose, Translation2d point) {
+        Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point);
+        Translation2d robot = pose.getRotation().toTranslation();
+        double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x();
+        return (cross < 0) ? -1 : 1; // if robot < pose turn left
+    }
+
+    /**
+     * @return has the robot reached the end of the path
+     */
+    public boolean isFinished() {
+        return mAtEndOfPath;
+    }
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java
new file mode 100644
index 0000000..f9acff1
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java
@@ -0,0 +1,83 @@
+package org.usfirst.frc4388.utility.control;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Twist2d;
+
+/**
+ * Provides forward and inverse kinematics equations for the robot modeling the wheelbase as a differential drive (with
+ * a corrective factor to account for skidding).
+ */
+
+public class Kinematics {
+    private static final double kEpsilon = 1E-9;
+
+    /**
+     * Forward kinematics using only encoders, rotation is implicit (less accurate than below, but useful for predicting
+     * motion)
+     */
+    public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta) {
+        double delta_v = (right_wheel_delta - left_wheel_delta) / 2 * Constants.kTrackScrubFactor;
+        double delta_rotation = delta_v * 2 / Constants.kTrackWidthInches;
+        return forwardKinematics(left_wheel_delta, right_wheel_delta, delta_rotation);
+    }
+
+    /**
+     * Forward kinematics using encoders and explicitly measured rotation (ex. from gyro)
+     */
+    public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta,
+            double delta_rotation_rads) {
+        final double dx = (left_wheel_delta + right_wheel_delta) / 2.0;
+        return new Twist2d(dx, 0, delta_rotation_rads);
+    }
+
+    /**
+     * For convenience, forward kinematic with an absolute rotation and previous rotation.
+     */
+    public static Twist2d forwardKinematics(Rotation2d prev_heading, double left_wheel_delta, double right_wheel_delta,
+            Rotation2d current_heading) {
+        return forwardKinematics(left_wheel_delta, right_wheel_delta,
+                prev_heading.inverse().rotateBy(current_heading).getRadians());
+    }
+
+    /** Append the result of forward kinematics to a previous pose. */
+    public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta,
+            double right_wheel_delta, Rotation2d current_heading) {
+        Twist2d with_gyro = forwardKinematics(current_pose.getRotation(), left_wheel_delta, right_wheel_delta,
+                current_heading);
+        return integrateForwardKinematics(current_pose, with_gyro);
+    }
+
+    /**
+     * For convenience, integrate forward kinematics with a Twist2d and previous rotation.
+     */
+    public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose,
+            Twist2d forward_kinematics) {
+        return current_pose.transformBy(RigidTransform2d.exp(forward_kinematics));
+    }
+
+    /**
+     * Class that contains left and right wheel velocities
+     */
+    public static class DriveVelocity {
+        public final double left;
+        public final double right;
+
+        public DriveVelocity(double left, double right) {
+            this.left = left;
+            this.right = right;
+        }
+    }
+
+    /**
+     * Uses inverse kinematics to convert a Twist2d into left and right wheel velocities
+     */
+    public static DriveVelocity inverseKinematics(Twist2d velocity) {
+        if (Math.abs(velocity.dtheta) < kEpsilon) {
+            return new DriveVelocity(velocity.dx, velocity.dx);
+        }
+        double delta_v = Constants.kTrackWidthInches * velocity.dtheta / (2 * Constants.kTrackScrubFactor);
+        return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v);
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java
new file mode 100644
index 0000000..2b91876
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java
@@ -0,0 +1,28 @@
+package org.usfirst.frc4388.utility.control;
+
+/**
+ * A utility class for interpolating lookahead distance based on current speed.
+ */
+public class Lookahead {
+    public final double min_distance;
+    public final double max_distance;
+    public final double min_speed;
+    public final double max_speed;
+
+    protected final double delta_distance;
+    protected final double delta_speed;
+
+    public Lookahead(double min_distance, double max_distance, double min_speed, double max_speed) {
+        this.min_distance = min_distance;
+        this.max_distance = max_distance;
+        this.min_speed = min_speed;
+        this.max_speed = max_speed;
+        delta_distance = max_distance - min_distance;
+        delta_speed = max_speed - min_speed;
+    }
+
+    public double getLookaheadForSpeed(double speed) {
+        double lookahead = delta_distance * (speed - min_speed) / delta_speed + min_distance;
+        return Double.isNaN(lookahead) ? min_distance : Math.max(min_distance, Math.min(max_distance, lookahead));
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java
new file mode 100644
index 0000000..4558d2f
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java
@@ -0,0 +1,235 @@
+package org.usfirst.frc4388.utility.control;
+
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.List;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.utility.math.Translation2d;
+import org.usfirst.frc4388.utility.motion.MotionState;
+
+/**
+ * Class representing the robot's autonomous path.
+ * 
+ * Field Coordinate System: Uses a right hand coordinate system. Positive x is right, positive y is up, and the origin
+ * is at the bottom left corner of the field. For angles, 0 degrees is facing right (1, 0) and angles increase as you
+ * turn counter clockwise.
+ * 
+ *  +Y
+ *   ^
+ *   |
+ *   |------------------------------   
+ *   |                              | <--- FRC Field Boundary
+ *   |                              |  
+ *   |                              |
+ *   |      -----                   |
+ *   |     |robot|--> 0 deg         |
+ *   |      -----                   |
+ *   ----------------------------------> +X 
+ * (0,0)
+ * 
+ *  Notes:
+ *  1) The starting point needs to match the first point (x,y) with the proper starting rotation
+ *  2) When chaining together paths, keep all coordinates for both paths in the reference frame above.  
+ *     The second path should start where the first stopped.
+ *  3) When moving in reverse, you need to set the isReversed flag on the path.
+ *  
+ */
+
+public class Path {
+    List<PathSegment> segments;
+    PathSegment prevSegment;
+    HashSet<String> mMarkersCrossed = new HashSet<String>();
+
+    public void extrapolateLast() {
+        PathSegment last = segments.get(segments.size() - 1);
+        last.extrapolateLookahead(true);
+    }
+
+    public Translation2d getEndPosition() {
+        return segments.get(segments.size() - 1).getEnd();
+    }
+
+    public Path() {
+        segments = new ArrayList<PathSegment>();
+    }
+
+    /**
+     * add a segment to the Path
+     * 
+     * @param segment
+     *            the segment to add
+     */
+    public void addSegment(PathSegment segment) {
+        segments.add(segment);
+    }
+
+    /**
+     * @return the last MotionState in the path
+     */
+    public MotionState getLastMotionState() {
+        if (segments.size() > 0) {
+            MotionState endState = segments.get(segments.size() - 1).getEndState();
+            return new MotionState(0.0, 0.0, endState.vel(), endState.acc());
+        } else {
+            return new MotionState(0, 0, 0, 0);
+        }
+    }
+
+    /**
+     * get the remaining distance left for the robot to travel on the current segment
+     * 
+     * @param robotPos
+     *            robot position
+     * @return remaining distance on current segment
+     */
+    public double getSegmentRemainingDist(Translation2d robotPos) {
+        PathSegment currentSegment = segments.get(0);
+        return currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos));
+    }
+
+    /**
+     * @return the length of the current segment
+     */
+    public double getSegmentLength() {
+        PathSegment currentSegment = segments.get(0);
+        return currentSegment.getLength();
+    }
+
+    public static class TargetPointReport {
+        public Translation2d closest_point;
+        public double closest_point_distance;
+        public double closest_point_speed;
+        public Translation2d lookahead_point;
+        public double max_speed;
+        public double lookahead_point_speed;
+        public double remaining_segment_distance;
+        public double remaining_path_distance;
+
+        public TargetPointReport() {
+        }
+    }
+
+    /**
+     * Gives the position of the lookahead point (and removes any segments prior to this point).
+     * 
+     * @param robot
+     *            Translation of the current robot pose.
+     * @return report containing everything we might want to know about the target point.
+     */
+    public TargetPointReport getTargetPoint(Translation2d robot, Lookahead lookahead) {
+        TargetPointReport rv = new TargetPointReport();
+        PathSegment currentSegment = segments.get(0);
+        rv.closest_point = currentSegment.getClosestPoint(robot);
+        rv.closest_point_distance = new Translation2d(robot, rv.closest_point).norm();
+        /*
+         * if (segments.size() > 1) { // Check next segment to see if it is closer. final Translation2d
+         * next_segment_closest_point = segments.get(1).getClosestPoint(robot); final double
+         * next_segment_closest_point_distance = new Translation2d(robot, next_segment_closest_point) .norm(); if
+         * (next_segment_closest_point_distance < rv.closest_point_distance) { rv.closest_point =
+         * next_segment_closest_point; rv.closest_point_distance = next_segment_closest_point_distance;
+         * removeCurrentSegment(); currentSegment = segments.get(0); } }
+         */
+        rv.remaining_segment_distance = currentSegment.getRemainingDistance(rv.closest_point);
+        rv.remaining_path_distance = rv.remaining_segment_distance;
+        for (int i = 1; i < segments.size(); ++i) {
+            rv.remaining_path_distance += segments.get(i).getLength();
+        }
+        rv.closest_point_speed = currentSegment
+                .getSpeedByDistance(currentSegment.getLength() - rv.remaining_segment_distance);
+        double lookahead_distance = lookahead.getLookaheadForSpeed(rv.closest_point_speed) + rv.closest_point_distance;
+        if (rv.remaining_segment_distance < lookahead_distance && segments.size() > 1) {
+            lookahead_distance -= rv.remaining_segment_distance;
+            for (int i = 1; i < segments.size(); ++i) {
+                currentSegment = segments.get(i);
+                final double length = currentSegment.getLength();
+                if (length < lookahead_distance && i < segments.size() - 1) {
+                    lookahead_distance -= length;
+                } else {
+                    break;
+                }
+            }
+        } else {
+            lookahead_distance += (currentSegment.getLength() - rv.remaining_segment_distance);
+        }
+        rv.max_speed = currentSegment.getMaxSpeed();
+        rv.lookahead_point = currentSegment.getPointByDistance(lookahead_distance);
+        rv.lookahead_point_speed = currentSegment.getSpeedByDistance(lookahead_distance);
+        checkSegmentDone(rv.closest_point);
+        return rv;
+    }
+
+    /**
+     * Gives the speed the robot should be traveling at the given position
+     * 
+     * @param robotPos
+     *            position of the robot
+     * @return speed robot should be traveling
+     */
+    public double getSpeed(Translation2d robotPos) {
+        PathSegment currentSegment = segments.get(0);
+        return currentSegment.getSpeedByClosestPoint(robotPos);
+    }
+
+    /**
+     * Checks if the robot has finished traveling along the current segment then removes it from the path if it has
+     * 
+     * @param robotPos
+     *            robot position
+     */
+    public void checkSegmentDone(Translation2d robotPos) {
+        PathSegment currentSegment = segments.get(0);
+        double remainingDist = currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos));
+        if (remainingDist < 0.1) {
+//            System.out.println("Removed segment from path: " + currentSegment);
+            removeCurrentSegment();
+        }
+    }
+
+    public void removeCurrentSegment() {
+        prevSegment = segments.remove(0);
+        String marker = prevSegment.getMarker();
+        if (marker != null)
+            mMarkersCrossed.add(marker);
+    }
+
+    /**
+     * Ensures that all speeds in the path are attainable and robot can slow down in time
+     */
+    public void verifySpeeds() {
+        double maxStartSpeed = 0.0;
+        double[] startSpeeds = new double[segments.size() + 1];
+        startSpeeds[segments.size()] = 0.0;
+        for (int i = segments.size() - 1; i >= 0; i--) {
+            PathSegment segment = segments.get(i);
+            maxStartSpeed += Math
+                    .sqrt(maxStartSpeed * maxStartSpeed + 2 * Constants.kPathFollowingMaxAccel * segment.getLength());
+            startSpeeds[i] = segment.getStartState().vel();
+            // System.out.println(maxStartSpeed + ", " + startSpeeds[i]);
+            if (startSpeeds[i] > maxStartSpeed) {
+                startSpeeds[i] = maxStartSpeed;
+                // System.out.println("Segment starting speed is too high!");
+            }
+            maxStartSpeed = startSpeeds[i];
+        }
+        for (int i = 0; i < segments.size(); i++) {
+            PathSegment segment = segments.get(i);
+            double endSpeed = startSpeeds[i + 1];
+            MotionState startState = (i > 0) ? segments.get(i - 1).getEndState() : new MotionState(0, 0, 0, 0);
+            startState = new MotionState(0, 0, startState.vel(), startState.vel());
+            segment.createMotionProfiler(startState, endSpeed);
+        }
+    }
+
+    public boolean hasPassedMarker(String marker) {
+        return mMarkersCrossed.contains(marker);
+    }
+
+    public String toString() {
+        String str = "";
+        for (PathSegment s : segments) {
+            str += s.toString() + "\n";
+        }
+        return str;
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java
new file mode 100644
index 0000000..272f0c5
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java
@@ -0,0 +1,198 @@
+package org.usfirst.frc4388.utility.control;
+
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Twist2d;
+import org.usfirst.frc4388.utility.motion.MotionProfileConstraints;
+import org.usfirst.frc4388.utility.motion.MotionProfileGoal;
+import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
+import org.usfirst.frc4388.utility.motion.MotionState;
+import org.usfirst.frc4388.utility.motion.ProfileFollower;
+
+/**
+ * A PathFollower follows a predefined path using a combination of feedforward and feedback control. It uses an
+ * AdaptivePurePursuitController to choose a reference pose and generate a steering command (curvature), and then a
+ * ProfileFollower to generate a profile (displacement and velocity) command.
+ */
+public class PathFollower {
+    private static final double kReallyBigNumber = 1E6;
+
+    public static class DebugOutput {
+        public double t;
+        public double pose_x;
+        public double pose_y;
+        public double pose_theta;
+        public double linear_displacement;
+        public double linear_velocity;
+        public double profile_displacement;
+        public double profile_velocity;
+        public double velocity_command_dx;
+        public double velocity_command_dy;
+        public double velocity_command_dtheta;
+        public double steering_command_dx;
+        public double steering_command_dy;
+        public double steering_command_dtheta;
+        public double cross_track_error;
+        public double along_track_error;
+        public double lookahead_point_x;
+        public double lookahead_point_y;
+        public double lookahead_point_velocity;
+    }
+
+    public static class Parameters {
+        public final Lookahead lookahead;
+        public final double inertia_gain;
+        public final double profile_kp;
+        public final double profile_ki;
+        public final double profile_kv;
+        public final double profile_kffv;
+        public final double profile_kffa;
+        public final double profile_max_abs_vel;
+        public final double profile_max_abs_acc;
+        public final double goal_pos_tolerance;
+        public final double goal_vel_tolerance;
+        public final double stop_steering_distance;
+
+        public Parameters(Lookahead lookahead, double inertia_gain, double profile_kp, double profile_ki,
+                double profile_kv, double profile_kffv, double profile_kffa, double profile_max_abs_vel,
+                double profile_max_abs_acc, double goal_pos_tolerance, double goal_vel_tolerance,
+                double stop_steering_distance) {
+            this.lookahead = lookahead;
+            this.inertia_gain = inertia_gain;
+            this.profile_kp = profile_kp;
+            this.profile_ki = profile_ki;
+            this.profile_kv = profile_kv;
+            this.profile_kffv = profile_kffv;
+            this.profile_kffa = profile_kffa;
+            this.profile_max_abs_vel = profile_max_abs_vel;
+            this.profile_max_abs_acc = profile_max_abs_acc;
+            this.goal_pos_tolerance = goal_pos_tolerance;
+            this.goal_vel_tolerance = goal_vel_tolerance;
+            this.stop_steering_distance = stop_steering_distance;
+        }
+    }
+
+    AdaptivePurePursuitController mSteeringController;
+    Twist2d mLastSteeringDelta;
+    ProfileFollower mVelocityController;
+    final double mInertiaGain;
+    boolean overrideFinished = false;
+    boolean doneSteering = false;
+    DebugOutput mDebugOutput = new DebugOutput();
+
+    double mMaxProfileVel;
+    double mMaxProfileAcc;
+    final double mGoalPosTolerance;
+    final double mGoalVelTolerance;
+    final double mStopSteeringDistance;
+    double mCrossTrackError = 0.0;
+    double mAlongTrackError = 0.0;
+
+    /**
+     * Create a new PathFollower for a given path.
+     */
+    public PathFollower(Path path, boolean reversed, Parameters parameters) {
+        mSteeringController = new AdaptivePurePursuitController(path, reversed, parameters.lookahead);
+        mLastSteeringDelta = Twist2d.identity();
+        mVelocityController = new ProfileFollower(parameters.profile_kp, parameters.profile_ki, parameters.profile_kv,
+                parameters.profile_kffv, parameters.profile_kffa);
+        mVelocityController.setConstraints(
+                new MotionProfileConstraints(parameters.profile_max_abs_vel, parameters.profile_max_abs_acc));
+        mMaxProfileVel = parameters.profile_max_abs_vel;
+        mMaxProfileAcc = parameters.profile_max_abs_acc;
+        mGoalPosTolerance = parameters.goal_pos_tolerance;
+        mGoalVelTolerance = parameters.goal_vel_tolerance;
+        mInertiaGain = parameters.inertia_gain;
+        mStopSteeringDistance = parameters.stop_steering_distance;
+    }
+
+    /**
+     * Get new velocity commands to follow the path.
+     * 
+     * @param t
+     *            The current timestamp
+     * @param pose
+     *            The current robot pose
+     * @param displacement
+     *            The current robot displacement (total distance driven).
+     * @param velocity
+     *            The current robot velocity.
+     * @return The velocity command to apply
+     */
+    public synchronized Twist2d update(double t, RigidTransform2d pose, double displacement, double velocity) {
+        if (!mSteeringController.isFinished()) {
+            final AdaptivePurePursuitController.Command steering_command = mSteeringController.update(pose);
+            mDebugOutput.lookahead_point_x = steering_command.lookahead_point.x();
+            mDebugOutput.lookahead_point_y = steering_command.lookahead_point.y();
+            mDebugOutput.lookahead_point_velocity = steering_command.end_velocity;
+            mDebugOutput.steering_command_dx = steering_command.delta.dx;
+            mDebugOutput.steering_command_dy = steering_command.delta.dy;
+            mDebugOutput.steering_command_dtheta = steering_command.delta.dtheta;
+            mCrossTrackError = steering_command.cross_track_error;
+            mLastSteeringDelta = steering_command.delta;
+            mVelocityController.setGoalAndConstraints(
+                    new MotionProfileGoal(displacement + steering_command.delta.dx,
+                            Math.abs(steering_command.end_velocity), CompletionBehavior.VIOLATE_MAX_ACCEL,
+                            mGoalPosTolerance, mGoalVelTolerance),
+                    new MotionProfileConstraints(Math.min(mMaxProfileVel, steering_command.max_velocity),
+                            mMaxProfileAcc));
+
+            if (steering_command.remaining_path_length < mStopSteeringDistance) {
+                doneSteering = true;
+            }
+        }
+
+        final double velocity_command = mVelocityController.update(new MotionState(t, displacement, velocity, 0.0), t);
+        mAlongTrackError = mVelocityController.getPosError();
+        final double curvature = mLastSteeringDelta.dtheta / mLastSteeringDelta.dx;
+        double dtheta = mLastSteeringDelta.dtheta;
+        if (!Double.isNaN(curvature) && Math.abs(curvature) < kReallyBigNumber) {
+            // Regenerate angular velocity command from adjusted curvature.
+            final double abs_velocity_setpoint = Math.abs(mVelocityController.getSetpoint().vel());
+            dtheta = mLastSteeringDelta.dx * curvature * (1.0 + mInertiaGain * abs_velocity_setpoint);
+        }
+        final double scale = velocity_command / mLastSteeringDelta.dx;
+        final Twist2d rv = new Twist2d(mLastSteeringDelta.dx * scale, 0.0, dtheta * scale);
+
+        // Fill out debug.
+        mDebugOutput.t = t;
+        mDebugOutput.pose_x = pose.getTranslation().x();
+        mDebugOutput.pose_y = pose.getTranslation().y();
+        mDebugOutput.pose_theta = pose.getRotation().getRadians();
+        mDebugOutput.linear_displacement = displacement;
+        mDebugOutput.linear_velocity = velocity;
+        mDebugOutput.profile_displacement = mVelocityController.getSetpoint().pos();
+        mDebugOutput.profile_velocity = mVelocityController.getSetpoint().vel();
+        mDebugOutput.velocity_command_dx = rv.dx;
+        mDebugOutput.velocity_command_dy = rv.dy;
+        mDebugOutput.velocity_command_dtheta = rv.dtheta;
+        mDebugOutput.cross_track_error = mCrossTrackError;
+        mDebugOutput.along_track_error = mAlongTrackError;
+
+        return rv;
+    }
+
+    public double getCrossTrackError() {
+        return mCrossTrackError;
+    }
+
+    public double getAlongTrackError() {
+        return mAlongTrackError;
+    }
+
+    public DebugOutput getDebug() {
+        return mDebugOutput;
+    }
+
+    public boolean isFinished() {
+        return (mSteeringController.isFinished() && mVelocityController.isFinishedProfile()
+                && mVelocityController.onTarget()) || overrideFinished;
+    }
+
+    public void forceFinish() {
+        overrideFinished = true;
+    }
+
+    public boolean hasPassedMarker(String marker) {
+        return mSteeringController.hasPassedMarker(marker);
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java
new file mode 100644
index 0000000..0645f2e
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java
@@ -0,0 +1,287 @@
+package org.usfirst.frc4388.utility.control;
+
+import java.util.Optional;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+import org.usfirst.frc4388.utility.motion.MotionProfile;
+import org.usfirst.frc4388.utility.motion.MotionProfileConstraints;
+import org.usfirst.frc4388.utility.motion.MotionProfileGenerator;
+import org.usfirst.frc4388.utility.motion.MotionProfileGoal;
+import org.usfirst.frc4388.utility.motion.MotionState;
+
+
+/**
+ * Class representing a segment of the robot's autonomous path.
+ */
+
+public class PathSegment {
+    private Translation2d start;
+    private Translation2d end;
+    private Translation2d center;
+    private Translation2d deltaStart;
+    private Translation2d deltaEnd;
+    private double maxSpeed;
+    private boolean isLine;
+    private MotionProfile speedController;
+    private boolean extrapolateLookahead;
+    private String marker;
+
+    /**
+     * Constructor for a linear segment
+     * 
+     * @param x1
+     *            start x
+     * @param y1
+     *            start y
+     * @param x2
+     *            end x
+     * @param y2
+     *            end y
+     * @param maxSpeed
+     *            maximum speed allowed on the segment
+     */
+    public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState,
+            double endSpeed) {
+        this.start = new Translation2d(x1, y1);
+        this.end = new Translation2d(x2, y2);
+
+        this.deltaStart = new Translation2d(start, end);
+
+        this.maxSpeed = maxSpeed;
+        extrapolateLookahead = false;
+        isLine = true;
+        createMotionProfiler(startState, endSpeed);
+    }
+
+    public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState,
+            double endSpeed, String marker) {
+        this.start = new Translation2d(x1, y1);
+        this.end = new Translation2d(x2, y2);
+
+        this.deltaStart = new Translation2d(start, end);
+
+        this.maxSpeed = maxSpeed;
+        extrapolateLookahead = false;
+        isLine = true;
+        this.marker = marker;
+        createMotionProfiler(startState, endSpeed);
+    }
+
+    /**
+     * Constructor for an arc segment
+     * 
+     * @param x1
+     *            start x
+     * @param y1
+     *            start y
+     * @param x2
+     *            end x
+     * @param y2
+     *            end y
+     * @param cx
+     *            center x
+     * @param cy
+     *            center y
+     * @param maxSpeed
+     *            maximum speed allowed on the segment
+     */
+    public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed,
+            MotionState startState, double endSpeed) {
+        this.start = new Translation2d(x1, y1);
+        this.end = new Translation2d(x2, y2);
+        this.center = new Translation2d(cx, cy);
+
+        this.deltaStart = new Translation2d(center, start);
+        this.deltaEnd = new Translation2d(center, end);
+
+        this.maxSpeed = maxSpeed;
+        extrapolateLookahead = false;
+        isLine = false;
+        createMotionProfiler(startState, endSpeed);
+    }
+
+    public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed,
+            MotionState startState, double endSpeed, String marker) {
+        this.start = new Translation2d(x1, y1);
+        this.end = new Translation2d(x2, y2);
+        this.center = new Translation2d(cx, cy);
+
+        this.deltaStart = new Translation2d(center, start);
+        this.deltaEnd = new Translation2d(center, end);
+
+        this.maxSpeed = maxSpeed;
+        extrapolateLookahead = false;
+        isLine = false;
+        this.marker = marker;
+        createMotionProfiler(startState, endSpeed);
+    }
+
+    /**
+     * @return max speed of the segment
+     */
+    public double getMaxSpeed() {
+        return maxSpeed;
+    }
+
+    public void createMotionProfiler(MotionState start_state, double end_speed) {
+        MotionProfileConstraints motionConstraints = new MotionProfileConstraints(maxSpeed,
+                Constants.kPathFollowingMaxAccel);
+        MotionProfileGoal goal_state = new MotionProfileGoal(getLength(), end_speed);
+        speedController = MotionProfileGenerator.generateProfile(motionConstraints, goal_state, start_state);
+        // System.out.println(speedController);
+    }
+
+    /**
+     * @return starting point of the segment
+     */
+    public Translation2d getStart() {
+        return start;
+    }
+
+    /**
+     * @return end point of the segment
+     */
+    public Translation2d getEnd() {
+        return end;
+    }
+
+    /**
+     * @return the total length of the segment
+     */
+    public double getLength() {
+        if (isLine) {
+            return deltaStart.norm();
+        } else {
+            return deltaStart.norm() * Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
+        }
+    }
+
+    /**
+     * Set whether or not to extrapolate the lookahead point. Should only be true for the last segment in the path
+     * 
+     * @param val
+     */
+    public void extrapolateLookahead(boolean val) {
+        extrapolateLookahead = val;
+    }
+
+    /**
+     * Gets the point on the segment closest to the robot
+     * 
+     * @param position
+     *            the current position of the robot
+     * @return the point on the segment closest to the robot
+     */
+    public Translation2d getClosestPoint(Translation2d position) {
+        if (isLine) {
+            Translation2d delta = new Translation2d(start, end);
+            double u = ((position.x() - start.x()) * delta.x() + (position.y() - start.y()) * delta.y())
+                    / (delta.x() * delta.x() + delta.y() * delta.y());
+            if (u >= 0 && u <= 1)
+                return new Translation2d(start.x() + u * delta.x(), start.y() + u * delta.y());
+            return (u < 0) ? start : end;
+        } else {
+            Translation2d deltaPosition = new Translation2d(center, position);
+            deltaPosition = deltaPosition.scale(deltaStart.norm() / deltaPosition.norm());
+            if (Translation2d.cross(deltaPosition, deltaStart) * Translation2d.cross(deltaPosition, deltaEnd) < 0) {
+                return center.translateBy(deltaPosition);
+            } else {
+                Translation2d startDist = new Translation2d(position, start);
+                Translation2d endDist = new Translation2d(position, end);
+                return (endDist.norm() < startDist.norm()) ? end : start;
+            }
+        }
+    }
+
+    /**
+     * Calculates the point on the segment <code>dist</code> distance from the starting point along the segment.
+     * 
+     * @param dist
+     *            distance from the starting point
+     * @return point on the segment <code>dist</code> distance from the starting point
+     */
+    public Translation2d getPointByDistance(double dist) {
+        double length = getLength();
+        if (!extrapolateLookahead && dist > length) {
+            dist = length;
+        }
+        if (isLine) {
+            return start.translateBy(deltaStart.scale(dist / length));
+        } else {
+            double deltaAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians()
+                    * ((Translation2d.cross(deltaStart, deltaEnd) >= 0) ? 1 : -1);
+            deltaAngle *= dist / length;
+            Translation2d t = deltaStart.rotateBy(Rotation2d.fromRadians(deltaAngle));
+            return center.translateBy(t);
+        }
+    }
+
+    /**
+     * Gets the remaining distance left on the segment from point <code>point</code>
+     * 
+     * @param point
+     *            result of <code>getClosestPoint()</code>
+     * @return distance remaining
+     */
+    public double getRemainingDistance(Translation2d position) {
+        if (isLine) {
+            return new Translation2d(end, position).norm();
+        } else {
+            Translation2d deltaPosition = new Translation2d(center, position);
+            double angle = Translation2d.getAngle(deltaEnd, deltaPosition).getRadians();
+            double totalAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
+            return angle / totalAngle * getLength();
+        }
+    }
+
+    private double getDistanceTravelled(Translation2d robotPosition) {
+        Translation2d pathPosition = getClosestPoint(robotPosition);
+        double remainingDist = getRemainingDistance(pathPosition);
+        return getLength() - remainingDist;
+
+    }
+
+    public double getSpeedByDistance(double dist) {
+        if (dist < speedController.startPos()) {
+            dist = speedController.startPos();
+        } else if (dist > speedController.endPos()) {
+            dist = speedController.endPos();
+        }
+        Optional<MotionState> state = speedController.firstStateByPos(dist);
+        if (state.isPresent()) {
+            return state.get().vel();
+        } else {
+            System.out.println("Velocity does not exist at that position!");
+            return 0.0;
+        }
+    }
+
+    public double getSpeedByClosestPoint(Translation2d robotPosition) {
+        return getSpeedByDistance(getDistanceTravelled(robotPosition));
+    }
+
+    public MotionState getEndState() {
+        return speedController.endState();
+    }
+
+    public MotionState getStartState() {
+        return speedController.startState();
+    }
+
+    public String getMarker() {
+        return marker;
+    }
+
+    public String toString() {
+        if (isLine) {
+            return "(" + "start: " + start + ", end: " + end + ", speed: " + maxSpeed // + ", profile: " +
+                                                                                      // speedController
+                    + ")";
+        } else {
+            return "(" + "start: " + start + ", end: " + end + ", center: " + center + ", speed: " + maxSpeed
+                    + ")"; // + ", profile: " + speedController + ")";
+        }
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java
new file mode 100644
index 0000000..690c1d0
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java
@@ -0,0 +1,133 @@
+package org.usfirst.frc4388.utility.control;
+
+import java.util.Map;
+
+import org.usfirst.frc4388.robot.Robot.OperationMode;
+import org.usfirst.frc4388.utility.math.InterpolatingDouble;
+import org.usfirst.frc4388.utility.math.InterpolatingTreeMap;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Twist2d;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * RobotState keeps track of the poses of various coordinate frames throughout the match. A coordinate frame is simply a
+ * point and direction in space that defines an (x,y) coordinate system. Transforms (or poses) keep track of the spatial
+ * relationship between different frames.
+ *
+ * Robot frames of interest (from parent to child):
+ *
+ * 1. Field frame: origin is where the robot is turned on
+ *
+ * 2. Vehicle frame: origin is the center of the robot wheelbase, facing forwards
+ *
+ * 3. Camera frame: origin is the center of the camera imager relative to the robot base.
+ *
+ * 4. Goal frame: origin is the center of the boiler (note that orientation in this frame is arbitrary). Also note that
+ * there can be multiple goal frames.
+ *
+ * As a kinematic chain with 4 frames, there are 3 transforms of interest:
+ *
+ * 1. Field-to-vehicle: This is tracked over time by integrating encoder and gyro measurements. It will inevitably
+ * drift, but is usually accurate over short time periods.
+ *
+ * 2. Vehicle-to-camera: This is a constant.
+ *
+ * 3. Camera-to-goal: This is a pure translation, and is measured by the vision system.
+ */
+
+public class RobotState {
+    private static RobotState instance_ = new RobotState();
+
+    public static RobotState getInstance() {
+        return instance_;
+    }
+
+    private static final int kObservationBufferSize = 100;
+
+    // FPGATimestamp -> RigidTransform2d or Rotation2d
+    private InterpolatingTreeMap<InterpolatingDouble, RigidTransform2d> field_to_vehicle_;
+    private Twist2d vehicle_velocity_predicted_;
+    private Twist2d vehicle_velocity_measured_;
+    private double distance_driven_;
+
+    private RobotState() {
+        reset(0, new RigidTransform2d());
+    }
+
+    /**
+     * Resets the field to robot transform (robot's position on the field)
+     */
+    public synchronized void reset(double start_time, RigidTransform2d initial_field_to_vehicle) {
+        field_to_vehicle_ = new InterpolatingTreeMap<>(kObservationBufferSize);
+        field_to_vehicle_.put(new InterpolatingDouble(start_time), initial_field_to_vehicle);
+        vehicle_velocity_predicted_ = Twist2d.identity();
+        vehicle_velocity_measured_ = Twist2d.identity();
+        distance_driven_ = 0.0;
+    }
+
+    public synchronized void resetDistanceDriven() {
+        distance_driven_ = 0.0;
+    }
+
+    /**
+     * Returns the robot's position on the field at a certain time. Linearly interpolates between stored robot positions
+     * to fill in the gaps.
+     */
+    public synchronized RigidTransform2d getFieldToVehicle(double timestamp) {
+        return field_to_vehicle_.getInterpolated(new InterpolatingDouble(timestamp));
+    }
+
+    public synchronized Map.Entry<InterpolatingDouble, RigidTransform2d> getLatestFieldToVehicle() {
+        return field_to_vehicle_.lastEntry();
+    }
+
+    public synchronized RigidTransform2d getPredictedFieldToVehicle(double lookahead_time) {
+        return getLatestFieldToVehicle().getValue()
+                .transformBy(RigidTransform2d.exp(vehicle_velocity_predicted_.scaled(lookahead_time)));
+    }
+
+    public synchronized void addFieldToVehicleObservation(double timestamp, RigidTransform2d observation) {
+        field_to_vehicle_.put(new InterpolatingDouble(timestamp), observation);
+    }
+
+    public synchronized void addObservations(double timestamp, Twist2d measured_velocity,
+            Twist2d predicted_velocity) {
+        addFieldToVehicleObservation(timestamp,
+                Kinematics.integrateForwardKinematics(getLatestFieldToVehicle().getValue(), measured_velocity));
+        vehicle_velocity_measured_ = measured_velocity;
+        vehicle_velocity_predicted_ = predicted_velocity;
+    }
+
+    public synchronized Twist2d generateOdometryFromSensors(double left_encoder_delta_distance,
+            double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
+        final RigidTransform2d last_measurement = getLatestFieldToVehicle().getValue();
+        final Twist2d delta = Kinematics.forwardKinematics(last_measurement.getRotation(),
+                left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle);
+        distance_driven_ += delta.dx;
+        return delta;
+    }
+
+    public synchronized double getDistanceDriven() {
+        return distance_driven_;
+    }
+
+    public synchronized Twist2d getPredictedVelocity() {
+        return vehicle_velocity_predicted_;
+    }
+
+    public synchronized Twist2d getMeasuredVelocity() {
+        return vehicle_velocity_measured_;
+    }
+
+    public void updateStatus(OperationMode operationMode) {
+    	if (operationMode == OperationMode.TEST) {
+	        RigidTransform2d odometry = getLatestFieldToVehicle().getValue();
+	        SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x());
+	        SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y());
+	        SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees());
+	        SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx);
+    	}
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java
new file mode 100644
index 0000000..bbf77e3
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java
@@ -0,0 +1,333 @@
+package org.usfirst.frc4388.utility.control;
+
+//import edu.wpi.first.wpilibj.util.BoundaryException;
+
+/**
+ * This class implements a PID Control Loop.
+ * 
+ * Does all computation synchronously (i.e. the calculate() function must be called by the user from his own thread)
+ */
+public class SynchronousPIDF {
+    private double m_P; // factor for "proportional" control
+    private double m_I; // factor for "integral" control
+    private double m_D; // factor for "derivative" control
+    private double m_F; // factor for feed forward gain
+    private double m_maximumOutput = 1.0; // |maximum output|
+    private double m_minimumOutput = -1.0; // |minimum output|
+    private double m_maximumInput = 0.0; // maximum input - limit setpoint to
+                                         // this
+    private double m_minimumInput = 0.0; // minimum input - limit setpoint to
+                                         // this
+    private boolean m_continuous = false; // do the endpoints wrap around? eg.
+                                          // Absolute encoder
+    private double m_prevError = 0.0; // the prior sensor input (used to compute
+                                      // velocity)
+    private double m_totalError = 0.0; // the sum of the errors for use in the
+                                       // integral calc
+    private double m_setpoint = 0.0;
+    private double m_error = 0.0;
+    private double m_result = 0.0;
+    private double m_last_input = Double.NaN;
+    private double m_deadband = 0.0; // If the absolute error is less than
+                                     // deadband
+                                     // then treat error for the proportional
+                                     // term as 0
+
+    public SynchronousPIDF() {
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D
+     *
+     * @param Kp
+     *            the proportional coefficient
+     * @param Ki
+     *            the integral coefficient
+     * @param Kd
+     *            the derivative coefficient
+     */
+    public SynchronousPIDF(double Kp, double Ki, double Kd) {
+        m_P = Kp;
+        m_I = Ki;
+        m_D = Kd;
+        m_F = 0;
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D
+     *
+     * @param Kp
+     *            the proportional coefficient
+     * @param Ki
+     *            the integral coefficient
+     * @param Kd
+     *            the derivative coefficient
+     * @param Kf
+     *            the feed forward gain coefficient
+     */
+    public SynchronousPIDF(double Kp, double Ki, double Kd, double Kf) {
+        m_P = Kp;
+        m_I = Ki;
+        m_D = Kd;
+        m_F = Kf;
+    }
+
+    /**
+     * Read the input, calculate the output accordingly, and write to the output. This should be called at a constant
+     * rate by the user (ex. in a timed thread)
+     *
+     * @param input
+     *            the input
+     * @param dt
+     *            time passed since previous call to calculate
+     */
+    public double calculate(double input, double dt) {
+        if (dt < 1E-6)
+            dt = 1E-6;
+        m_last_input = input;
+        m_error = m_setpoint - input;
+        if (m_continuous) {
+            if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
+                if (m_error > 0) {
+                    m_error = m_error - m_maximumInput + m_minimumInput;
+                } else {
+                    m_error = m_error + m_maximumInput - m_minimumInput;
+                }
+            }
+        }
+
+        if ((m_error * m_P < m_maximumOutput) && (m_error * m_P > m_minimumOutput)) {
+            m_totalError += m_error * dt;
+        } else {
+            m_totalError = 0;
+        }
+
+        // Don't blow away m_error so as to not break derivative
+        double proportionalError = Math.abs(m_error) < m_deadband ? 0 : m_error;
+
+        m_result = (m_P * proportionalError + m_I * m_totalError + m_D * (m_error - m_prevError) / dt
+                + m_F * m_setpoint);
+        m_prevError = m_error;
+
+        if (m_result > m_maximumOutput) {
+            m_result = m_maximumOutput;
+        } else if (m_result < m_minimumOutput) {
+            m_result = m_minimumOutput;
+        }
+        return m_result;
+    }
+
+    /**
+     * Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients.
+     *
+     * @param p
+     *            Proportional coefficient
+     * @param i
+     *            Integral coefficient
+     * @param d
+     *            Differential coefficient
+     */
+    public void setPID(double p, double i, double d) {
+        m_P = p;
+        m_I = i;
+        m_D = d;
+    }
+
+    /**
+     * Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients.
+     *
+     * @param p
+     *            Proportional coefficient
+     * @param i
+     *            Integral coefficient
+     * @param d
+     *            Differential coefficient
+     * @param f
+     *            Feed forward coefficient
+     */
+    public void setPID(double p, double i, double d, double f) {
+        m_P = p;
+        m_I = i;
+        m_D = d;
+        m_F = f;
+    }
+
+    /**
+     * Get the Proportional coefficient
+     *
+     * @return proportional coefficient
+     */
+    public double getP() {
+        return m_P;
+    }
+
+    /**
+     * Get the Integral coefficient
+     *
+     * @return integral coefficient
+     */
+    public double getI() {
+        return m_I;
+    }
+
+    /**
+     * Get the Differential coefficient
+     *
+     * @return differential coefficient
+     */
+    public double getD() {
+        return m_D;
+    }
+
+    /**
+     * Get the Feed forward coefficient
+     *
+     * @return feed forward coefficient
+     */
+    public double getF() {
+        return m_F;
+    }
+
+    /**
+     * Return the current PID result This is always centered on zero and constrained the the max and min outs
+     *
+     * @return the latest calculated output
+     */
+    public double get() {
+        return m_result;
+    }
+
+    /**
+     * Set the PID controller to consider the input to be continuous, Rather then using the max and min in as
+     * constraints, it considers them to be the same point and automatically calculates the shortest route to the
+     * setpoint.
+     *
+     * @param continuous
+     *            Set to true turns on continuous, false turns off continuous
+     */
+    public void setContinuous(boolean continuous) {
+        m_continuous = continuous;
+    }
+
+    public void setDeadband(double deadband) {
+        m_deadband = deadband;
+    }
+
+    /**
+     * Set the PID controller to consider the input to be continuous, Rather then using the max and min in as
+     * constraints, it considers them to be the same point and automatically calculates the shortest route to the
+     * setpoint.
+     */
+    public void setContinuous() {
+        this.setContinuous(true);
+    }
+
+    /**
+     * Sets the maximum and minimum values expected from the input.
+     *
+     * @param minimumInput
+     *            the minimum value expected from the input
+     * @param maximumInput
+     *            the maximum value expected from the output
+     */
+    public void setInputRange(double minimumInput, double maximumInput) {
+        if (minimumInput > maximumInput) {
+            //throw new BoundaryException("Lower bound is greater than upper bound");
+        }
+        m_minimumInput = minimumInput;
+        m_maximumInput = maximumInput;
+        setSetpoint(m_setpoint);
+    }
+
+    /**
+     * Sets the minimum and maximum values to write.
+     *
+     * @param minimumOutput
+     *            the minimum value to write to the output
+     * @param maximumOutput
+     *            the maximum value to write to the output
+     */
+    public void setOutputRange(double minimumOutput, double maximumOutput) {
+        if (minimumOutput > maximumOutput) {
+            //throw new BoundaryException("Lower bound is greater than upper bound");
+        }
+        m_minimumOutput = minimumOutput;
+        m_maximumOutput = maximumOutput;
+    }
+
+    /**
+     * Set the setpoint for the PID controller
+     *
+     * @param setpoint
+     *            the desired setpoint
+     */
+    public void setSetpoint(double setpoint) {
+        if (m_maximumInput > m_minimumInput) {
+            if (setpoint > m_maximumInput) {
+                m_setpoint = m_maximumInput;
+            } else if (setpoint < m_minimumInput) {
+                m_setpoint = m_minimumInput;
+            } else {
+                m_setpoint = setpoint;
+            }
+        } else {
+            m_setpoint = setpoint;
+        }
+    }
+
+    /**
+     * Returns the current setpoint of the PID controller
+     *
+     * @return the current setpoint
+     */
+    public double getSetpoint() {
+        return m_setpoint;
+    }
+
+    /**
+     * Returns the current difference of the input from the setpoint
+     *
+     * @return the current error
+     */
+    public double getError() {
+        return m_error;
+    }
+
+    /**
+     * Return true if the error is within the tolerance
+     *
+     * @return true if the error is less than the tolerance
+     */
+    public boolean onTarget(double tolerance) {
+        return m_last_input != Double.NaN && Math.abs(m_last_input - m_setpoint) < tolerance;
+    }
+
+    /**
+     * Reset all internal terms.
+     */
+    public void reset() {
+        m_last_input = Double.NaN;
+        m_prevError = 0;
+        m_totalError = 0;
+        m_result = 0;
+        m_setpoint = 0;
+    }
+
+    public void resetIntegrator() {
+        m_totalError = 0;
+    }
+
+    public String getState() {
+        String lState = "";
+
+        lState += "Kp: " + m_P + "\n";
+        lState += "Ki: " + m_I + "\n";
+        lState += "Kd: " + m_D + "\n";
+
+        return lState;
+    }
+
+    public String getType() {
+        return "PIDController";
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java
similarity index 54%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java
index 33e0b54..359e181 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java
@@ -1,9 +1,8 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
 
 /**
- * Interpolable is an interface used by an Interpolating Tree as the Value type.
- * Given two end points and an interpolation parameter on [0, 1], it calculates
- * a new Interpolable representing the interpolated value.
+ * Interpolable is an interface used by an Interpolating Tree as the Value type. Given two end points and an
+ * interpolation parameter on [0, 1], it calculates a new Interpolable representing the interpolated value.
  * 
  * @param <T>
  *            The Type of Interpolable
@@ -11,17 +10,15 @@ package org.usfirst.frc4388.utility;
  */
 public interface Interpolable<T> {
     /**
-     * Interpolates between this value and an other value according to a given
-     * parameter. If x is 0, the method should return this value. If x is 1, the
-     * method should return the other value. If 0 < x < 1, the return value
-     * should be interpolated proportionally between the two.
+     * Interpolates between this value and an other value according to a given parameter. If x is 0, the method should
+     * return this value. If x is 1, the method should return the other value. If 0 < x < 1, the return value should be
+     * interpolated proportionally between the two.
      *
      * @param other
      *            The value of the upper bound
      * @param x
      *            The requested value. Should be between 0 and 1.
-     * @return Interpolable<T> The estimated average between the surrounding
-     *         data
+     * @return Interpolable<T> The estimated average between the surrounding data
      */
     public T interpolate(T other, double x);
 }
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java
new file mode 100644
index 0000000..28cc4c2
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.utility.math;
+
+/**
+ * A Double that can be interpolated using the InterpolatingTreeMap.
+ * 
+ * @see InterpolatingTreeMap
+ */
+public class InterpolatingDouble implements Interpolable<InterpolatingDouble>, InverseInterpolable<InterpolatingDouble>,
+        Comparable<InterpolatingDouble> {
+    public Double value = 0.0;
+
+    public InterpolatingDouble(Double val) {
+        value = val;
+    }
+
+    @Override
+    public InterpolatingDouble interpolate(InterpolatingDouble other, double x) {
+        Double dydx = other.value - value;
+        Double searchY = dydx * x + value;
+        return new InterpolatingDouble(searchY);
+    }
+
+    @Override
+    public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) {
+        double upper_to_lower = upper.value - value;
+        if (upper_to_lower <= 0) {
+            return 0;
+        }
+        double query_to_lower = query.value - value;
+        if (query_to_lower <= 0) {
+            return 0;
+        }
+        return query_to_lower / upper_to_lower;
+    }
+
+    @Override
+    public int compareTo(InterpolatingDouble other) {
+        if (other.value < value) {
+            return 1;
+        } else if (other.value > value) {
+            return -1;
+        } else {
+            return 0;
+        }
+    }
+
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java
new file mode 100644
index 0000000..1303252
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc4388.utility.math;
+
+/**
+ * A Long that can be interpolated using the InterpolatingTreeMap.
+ * 
+ * @see InterpolatingTreeMap
+ */
+public class InterpolatingLong implements Interpolable<InterpolatingLong>, InverseInterpolable<InterpolatingLong>,
+        Comparable<InterpolatingLong> {
+    public Long value = 0L;
+
+    public InterpolatingLong(Long val) {
+        value = val;
+    }
+
+    @Override
+    public InterpolatingLong interpolate(InterpolatingLong other, double x) {
+        Long dydx = other.value - value;
+        Double searchY = dydx * x + value;
+        return new InterpolatingLong(searchY.longValue());
+    }
+
+    @Override
+    public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) {
+        long upper_to_lower = upper.value - value;
+        if (upper_to_lower <= 0) {
+            return 0;
+        }
+        long query_to_lower = query.value - value;
+        if (query_to_lower <= 0) {
+            return 0;
+        }
+        return query_to_lower / (double) upper_to_lower;
+    }
+
+    @Override
+    public int compareTo(InterpolatingLong other) {
+        if (other.value < value) {
+            return 1;
+        } else if (other.value > value) {
+            return -1;
+        } else {
+            return 0;
+        }
+    }
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java
similarity index 91%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java
index 7c6de07..260a445 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java
@@ -1,12 +1,11 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
 
 import java.util.Map;
 import java.util.TreeMap;
 
 /**
- * Interpolating Tree Maps are used to get values at points that are not defined
- * by making a guess from points that are defined. This uses linear
- * interpolation.
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess from points that are
+ * defined. This uses linear interpolation.
  * 
  * @param <K>
  *            The type of the key (must implement InverseInterpolable)
@@ -58,8 +57,7 @@ public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<
      *
      * @param key
      *            Lookup for a value (does not have to exist)
-     * @return V or null; V if it is Interpolable or exists, null if it is at a
-     *         bound and cannot average
+     * @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot average
      */
     public V getInterpolated(K key) {
         V gotval = get(key);
@@ -69,8 +67,7 @@ public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<
             K bottomBound = floorKey(key);
 
             /**
-             * If attempting interpolation at ends of tree, return the nearest
-             * data point
+             * If attempting interpolation at ends of tree, return the nearest data point
              */
             if (topBound == null && bottomBound == null) {
                 return null;
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java
similarity index 54%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java
index dc9f6a4..99341ab 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java
@@ -1,9 +1,8 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
 
 /**
- * InverseInterpolable is an interface used by an Interpolating Tree as the Key
- * type. Given two endpoint keys and a third query key, an InverseInterpolable
- * object can calculate the interpolation parameter of the query key on the
+ * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a
+ * third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the
  * interval [0, 1].
  * 
  * @param <T>
@@ -12,14 +11,13 @@ package org.usfirst.frc4388.utility;
  */
 public interface InverseInterpolable<T> {
     /**
-     * Given this point (lower), a query point (query), and an upper point
-     * (upper), estimate how far (on [0, 1]) between 'lower' and 'upper' the
-     * query point lies.
+     * Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between
+     * 'lower' and 'upper' the query point lies.
      * 
      * @param upper
      * @param query
-     * @return The interpolation parameter on [0, 1] representing how far
-     *         between this point and the upper point the query point lies.
+     * @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the
+     *         query point lies.
      */
     public double inverseInterpolate(T upper, T query);
 }
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java
similarity index 50%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java
index c48b552..b8c6f51 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java
@@ -1,29 +1,23 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
 
 /**
- * Represents a 2d pose (rigid transform) containing translational and
- * rotational elements.
+ * Represents a 2d pose (rigid transform) containing translational and rotational elements.
  * 
  * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
  */
 public class RigidTransform2d implements Interpolable<RigidTransform2d> {
-    private final static double kEps = 1E-9;
+    protected static final double kEpsilon = 1E-9;
 
-    // Movement along an arc at constant curvature and velocity. We can use
-    // ideas from "differential calculus" to create new RigidTransform2d's from
-    // a Delta.
-    public static class Delta {
-        public final double dx;
-        public final double dy;
-        public final double dtheta;
+    protected static final RigidTransform2d kIdentity = new RigidTransform2d();
 
-        public Delta(double dx, double dy, double dtheta) {
-            this.dx = dx;
-            this.dy = dy;
-            this.dtheta = dtheta;
-        }
+    public static final RigidTransform2d identity() {
+        return kIdentity;
     }
 
+    private final static double kEps = 1E-9;
+
     protected Translation2d translation_;
     protected Rotation2d rotation_;
 
@@ -54,7 +48,7 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
      * Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
      * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
      */
-    public static RigidTransform2d fromVelocity(Delta delta) {
+    public static RigidTransform2d exp(Twist2d delta) {
         double sin_theta = Math.sin(delta.dtheta);
         double cos_theta = Math.cos(delta.dtheta);
         double s, c;
@@ -69,6 +63,24 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
                 new Rotation2d(cos_theta, sin_theta, false));
     }
 
+    /**
+     * Logical inverse of the above.
+     */
+    public static Twist2d log(RigidTransform2d transform) {
+        final double dtheta = transform.getRotation().getRadians();
+        final double half_dtheta = 0.5 * dtheta;
+        final double cos_minus_one = transform.getRotation().cos() - 1.0;
+        double halftheta_by_tan_of_halfdtheta;
+        if (Math.abs(cos_minus_one) < kEps) {
+            halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+        } else {
+            halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
+        }
+        final Translation2d translation_part = transform.getTranslation()
+                .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
+        return new Twist2d(translation_part.x(), translation_part.y(), dtheta);
+    }
+
     public Translation2d getTranslation() {
         return translation_;
     }
@@ -86,8 +98,8 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
     }
 
     /**
-     * Transforming this RigidTransform2d means first translating by
-     * other.translation and then rotating by other.rotation
+     * Transforming this RigidTransform2d means first translating by other.translation and then rotating by
+     * other.rotation
      * 
      * @param other
      *            The other transform.
@@ -99,8 +111,7 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
     }
 
     /**
-     * The inverse of this transform "undoes" the effect of translating by this
-     * transform.
+     * The inverse of this transform "undoes" the effect of translating by this transform.
      * 
      * @return The opposite of this transform.
      */
@@ -109,9 +120,49 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
         return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
     }
 
+    public RigidTransform2d normal() {
+        return new RigidTransform2d(translation_, rotation_.normal());
+    }
+
     /**
-     * Do linear interpolation of this transform (there are more accurate ways
-     * using constant curvature, but this is good enough).
+     * Finds the point where the heading of this transform intersects the heading of another. Returns (+INF, +INF) if
+     * parallel.
+     */
+    public Translation2d intersection(RigidTransform2d other) {
+        final Rotation2d other_rotation = other.getRotation();
+        if (rotation_.isParallel(other_rotation)) {
+            // Lines are parallel.
+            return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
+        }
+        if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) {
+            return intersectionInternal(this, other);
+        } else {
+            return intersectionInternal(other, this);
+        }
+    }
+
+    /**
+     * Return true if the heading of this transform is colinear with the heading of another.
+     */
+    public boolean isColinear(RigidTransform2d other) {
+        final Twist2d twist = log(inverse().transformBy(other));
+        return (epsilonEquals(twist.dy, 0.0, kEpsilon) && epsilonEquals(twist.dtheta, 0.0, kEpsilon));
+    }
+
+    private static Translation2d intersectionInternal(RigidTransform2d a, RigidTransform2d b) {
+        final Rotation2d a_r = a.getRotation();
+        final Rotation2d b_r = b.getRotation();
+        final Translation2d a_t = a.getTranslation();
+        final Translation2d b_t = b.getTranslation();
+
+        final double tan_b = b_r.tan();
+        final double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y())
+                / (a_r.sin() - a_r.cos() * tan_b);
+        return a_t.translateBy(a_r.toTranslation().scale(t));
+    }
+
+    /**
+     * Do twist interpolation of this transform assuming constant curvature.
      */
     @Override
     public RigidTransform2d interpolate(RigidTransform2d other, double x) {
@@ -120,8 +171,8 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
         } else if (x >= 1) {
             return new RigidTransform2d(other);
         }
-        return new RigidTransform2d(translation_.interpolate(other.translation_, x),
-                rotation_.interpolate(other.rotation_, x));
+        final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other));
+        return transformBy(RigidTransform2d.exp(twist.scaled(x)));
     }
 
     @Override
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java
similarity index 73%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java
index 4da86a3..e57f297 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java
@@ -1,14 +1,21 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
 
 import java.text.DecimalFormat;
 
 /**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
- * (cosine and sine).
+ * A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
  * 
  * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
  */
 public class Rotation2d implements Interpolable<Rotation2d> {
+    protected static final Rotation2d kIdentity = new Rotation2d();
+
+    public static final Rotation2d identity() {
+        return kIdentity;
+    }
+
     protected static final double kEpsilon = 1E-9;
 
     protected double cos_angle_;
@@ -31,6 +38,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
         sin_angle_ = other.sin_angle_;
     }
 
+    public Rotation2d(Translation2d direction, boolean normalize) {
+        this(direction.x(), direction.y(), normalize);
+    }
+
     public static Rotation2d fromRadians(double angle_radians) {
         return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false);
     }
@@ -40,9 +51,8 @@ public class Rotation2d implements Interpolable<Rotation2d> {
     }
 
     /**
-     * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this
-     * object we might accumulate rounding errors. Normalizing forces us to
-     * re-scale the sin and cos to reset rounding errors.
+     * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might accumulate rounding errors.
+     * Normalizing forces us to re-scale the sin and cos to reset rounding errors.
      */
     public void normalize() {
         double magnitude = Math.hypot(cos_angle_, sin_angle_);
@@ -64,7 +74,7 @@ public class Rotation2d implements Interpolable<Rotation2d> {
     }
 
     public double tan() {
-        if (cos_angle_ < kEpsilon) {
+        if (Math.abs(cos_angle_) < kEpsilon) {
             if (sin_angle_ >= 0.0) {
                 return Double.POSITIVE_INFINITY;
             } else {
@@ -83,12 +93,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
     }
 
     /**
-     * We can rotate this Rotation2d by adding together the effects of it and
-     * another rotation.
+     * We can rotate this Rotation2d by adding together the effects of it and another rotation.
      * 
      * @param other
-     *            The other rotation. See:
-     *            https://en.wikipedia.org/wiki/Rotation_matrix
+     *            The other rotation. See: https://en.wikipedia.org/wiki/Rotation_matrix
      * @return This rotation rotated by other.
      */
     public Rotation2d rotateBy(Rotation2d other) {
@@ -96,6 +104,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
                 cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true);
     }
 
+    public Rotation2d normal() {
+        return new Rotation2d(-sin_angle_, cos_angle_, false);
+    }
+
     /**
      * The inverse of a Rotation2d "undoes" the effect of this rotation.
      * 
@@ -105,6 +117,14 @@ public class Rotation2d implements Interpolable<Rotation2d> {
         return new Rotation2d(cos_angle_, -sin_angle_, false);
     }
 
+    public boolean isParallel(Rotation2d other) {
+        return epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0, kEpsilon);
+    }
+
+    public Translation2d toTranslation() {
+        return new Translation2d(cos_angle_, sin_angle_);
+    }
+
     @Override
     public Rotation2d interpolate(Rotation2d other, double x) {
         if (x <= 0) {
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java
similarity index 63%
rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java
rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java
index 05417a8..e32e62a 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java
@@ -1,12 +1,17 @@
-package org.usfirst.frc4388.utility;
+package org.usfirst.frc4388.utility.math;
 
 import java.text.DecimalFormat;
 
 /**
- * A translation in a 2d coordinate frame. Translations are simply shifts in an
- * (x, y) plane.
+ * A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane.
  */
 public class Translation2d implements Interpolable<Translation2d> {
+    protected static final Translation2d kIdentity = new Translation2d();
+
+    public static final Translation2d identity() {
+        return kIdentity;
+    }
+
     protected double x_;
     protected double y_;
 
@@ -25,20 +30,29 @@ public class Translation2d implements Interpolable<Translation2d> {
         y_ = other.y_;
     }
 
+    public Translation2d(Translation2d start, Translation2d end) {
+        x_ = end.x_ - start.x_;
+        y_ = end.y_ - start.y_;
+    }
+
     /**
      * The "norm" of a transform is the Euclidean distance in x and y.
      * 
-     * @return Sqrt(x^2 + y^2)
+     * @return sqrt(x^2 + y^2)
      */
     public double norm() {
         return Math.hypot(x_, y_);
     }
 
-    public double getX() {
+    public double norm2() {
+        return x_ * x_ + y_ * y_;
+    }
+
+    public double x() {
         return x_;
     }
 
-    public double getY() {
+    public double y() {
         return y_;
     }
 
@@ -62,8 +76,7 @@ public class Translation2d implements Interpolable<Translation2d> {
     }
 
     /**
-     * We can also rotate Translation2d's. See:
-     * https://en.wikipedia.org/wiki/Rotation_matrix
+     * We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
      * 
      * @param rotation
      *            The rotation to apply.
@@ -73,6 +86,10 @@ public class Translation2d implements Interpolable<Translation2d> {
         return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos());
     }
 
+    public Rotation2d direction() {
+        return new Rotation2d(x_, y_, true);
+    }
+
     /**
      * The inverse simply means a Translation2d that "undoes" this object.
      * 
@@ -96,9 +113,29 @@ public class Translation2d implements Interpolable<Translation2d> {
         return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_);
     }
 
+    public Translation2d scale(double s) {
+        return new Translation2d(x_ * s, y_ * s);
+    }
+
     @Override
     public String toString() {
         final DecimalFormat fmt = new DecimalFormat("#0.000");
         return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")";
     }
+
+    public static double dot(Translation2d a, Translation2d b) {
+        return a.x_ * b.x_ + a.y_ * b.y_;
+    }
+
+    public static Rotation2d getAngle(Translation2d a, Translation2d b) {
+        double cos_angle = dot(a, b) / (a.norm() * b.norm());
+        if (Double.isNaN(cos_angle)) {
+            return new Rotation2d();
+        }
+        return Rotation2d.fromRadians(Math.acos(Math.min(1.0, Math.max(cos_angle, -1.0))));
+    }
+
+    public static double cross(Translation2d a, Translation2d b) {
+        return a.x_ * b.y_ - a.y_ * b.x_;
+    }
 }
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java
new file mode 100644
index 0000000..fd4ec14
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc4388.utility.math;
+
+/**
+ * A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create
+ * new RigidTransform2d's from a Twist2d and visa versa.
+ * 
+ * A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc.
+ */
+public class Twist2d {
+    protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0);
+
+    public static final Twist2d identity() {
+        return kIdentity;
+    }
+
+    public final double dx;
+    public final double dy;
+    public final double dtheta; // Radians!
+
+    public Twist2d(double dx, double dy, double dtheta) {
+        this.dx = dx;
+        this.dy = dy;
+        this.dtheta = dtheta;
+    }
+
+    public Twist2d scaled(double scale) {
+        return new Twist2d(dx * scale, dy * scale, dtheta * scale);
+    }
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java
new file mode 100644
index 0000000..176498e
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc4388.utility.motion;
+
+import org.usfirst.frc4388.utility.math.Rotation2d;
+
+/**
+ * Class to deal with angle wrapping for following a heading profile. All states are assumed to be in units of degrees,
+ * and wrap on the interval of [-180, 180].
+ */
+public class HeadingProfileFollower extends ProfileFollower {
+
+    public HeadingProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
+        super(kp, ki, kv, kffv, kffa);
+    }
+
+    @Override
+    public double update(MotionState latest_state, double t) {
+        final Rotation2d goal_rotation_inverse = Rotation2d.fromDegrees(mGoal.pos()).inverse();
+        // Update both the setpoint and latest state to be relative to the new goal.
+        if (mLatestSetpoint != null) {
+            mLatestSetpoint.motion_state = new MotionState(mLatestSetpoint.motion_state.t(),
+                    mGoal.pos() + goal_rotation_inverse
+                            .rotateBy(Rotation2d.fromDegrees(mLatestSetpoint.motion_state.pos())).getDegrees(),
+                    mLatestSetpoint.motion_state.vel(), mLatestSetpoint.motion_state.acc());
+        }
+        final MotionState latest_state_unwrapped = new MotionState(latest_state.t(),
+                mGoal.pos() + goal_rotation_inverse.rotateBy(Rotation2d.fromDegrees(latest_state.pos())).getDegrees(),
+                latest_state.vel(), latest_state.acc());
+        double result = super.update(latest_state_unwrapped, t);
+        // Reset the integrator when we are close to the goal (encourage stiction!).
+        if (Math.abs(latest_state_unwrapped.pos() - mGoal.pos()) < mGoal.pos_tolerance()) {
+            result = 0.0;
+            super.resetIntegral();
+        }
+        return result;
+    }
+
+    /**
+     * Convert a motion state representing an angle to a properly wrapped angle.
+     */
+    public static MotionState canonicalize(MotionState state) {
+        return new MotionState(state.t(), Rotation2d.fromDegrees(state.pos()).getDegrees(), state.vel(), state.acc());
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java
new file mode 100644
index 0000000..a04f94e
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java
@@ -0,0 +1,326 @@
+package org.usfirst.frc4388.utility.motion;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
+import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
+
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Optional;
+
+/**
+ * A motion profile specifies a 1D time-parameterized trajectory. The trajectory is composed of successively coincident
+ * MotionSegments from which the desired state of motion at any given distance or time can be calculated.
+ */
+public class MotionProfile {
+    protected List<MotionSegment> mSegments;
+
+    /**
+     * Create an empty MotionProfile.
+     */
+    public MotionProfile() {
+        mSegments = new ArrayList<>();
+    }
+
+    /**
+     * Create a MotionProfile from an existing list of segments (note that validity is not checked).
+     * 
+     * @param segments
+     *            The new segments of the profile.
+     */
+    public MotionProfile(List<MotionSegment> segments) {
+        mSegments = segments;
+    }
+
+    /**
+     * Checks if the given MotionProfile is valid. This checks that:
+     * 
+     * 1. All segments are valid.
+     * 
+     * 2. Successive segments are C1 continuous in position and C0 continuous in velocity.
+     * 
+     * @return True if the MotionProfile is valid.
+     */
+    public boolean isValid() {
+        MotionSegment prev_segment = null;
+        for (MotionSegment s : mSegments) {
+            if (!s.isValid()) {
+                return false;
+            }
+            if (prev_segment != null && !s.start().coincident(prev_segment.end())) {
+                // Adjacent segments are not continuous.
+                System.err.println("Segments not continuous! End: " + prev_segment.end() + ", Start: " + s.start());
+                return false;
+            }
+            prev_segment = s;
+        }
+        return true;
+    }
+
+    /**
+     * Check if the profile is empty.
+     * 
+     * @return True if there are no segments.
+     */
+    public boolean isEmpty() {
+        return mSegments.isEmpty();
+    }
+
+    /**
+     * Get the interpolated MotionState at any given time.
+     * 
+     * @param t
+     *            The time to query.
+     * @return Empty if the time is outside the time bounds of the profile, or the resulting MotionState otherwise.
+     */
+    public Optional<MotionState> stateByTime(double t) {
+        if (t < startTime() && t + kEpsilon >= startTime()) {
+            return Optional.of(startState());
+        }
+        if (t > endTime() && t - kEpsilon <= endTime()) {
+            return Optional.of(endState());
+        }
+        for (MotionSegment s : mSegments) {
+            if (s.containsTime(t)) {
+                return Optional.of(s.start().extrapolate(t));
+            }
+        }
+        return Optional.empty();
+    }
+
+    /**
+     * Get the interpolated MotionState at any given time, clamping to the endpoints if time is out of bounds.
+     * 
+     * @param t
+     *            The time to query.
+     * @return The MotionState at time t, or closest to it if t is outside the profile.
+     */
+    public MotionState stateByTimeClamped(double t) {
+        if (t < startTime()) {
+            return startState();
+        } else if (t > endTime()) {
+            return endState();
+        }
+        for (MotionSegment s : mSegments) {
+            if (s.containsTime(t)) {
+                return s.start().extrapolate(t);
+            }
+        }
+        // Should never get here.
+        return MotionState.kInvalidState;
+    }
+
+    /**
+     * Get the interpolated MotionState by distance (the "pos()" field of MotionState). Note that since a profile may
+     * reverse, this method only returns the *first* instance of this position.
+     * 
+     * @param pos
+     *            The position to query.
+     * @return Empty if the profile never crosses pos or if the profile is invalid, or the resulting MotionState
+     *         otherwise.
+     */
+    public Optional<MotionState> firstStateByPos(double pos) {
+        for (MotionSegment s : mSegments) {
+            if (s.containsPos(pos)) {
+                if (epsilonEquals(s.end().pos(), pos, kEpsilon)) {
+                    return Optional.of(s.end());
+                }
+                final double t = Math.min(s.start().nextTimeAtPos(pos), s.end().t());
+                if (Double.isNaN(t)) {
+                    System.err.println("Error! We should reach 'pos' but we don't");
+                    return Optional.empty();
+                }
+                return Optional.of(s.start().extrapolate(t));
+            }
+        }
+        // We never reach pos.
+        return Optional.empty();
+    }
+
+    /**
+     * Remove all parts of the profile prior to the query time. This eliminates whole segments and also shortens any
+     * segments containing t.
+     * 
+     * @param t
+     *            The query time.
+     */
+    public void trimBeforeTime(double t) {
+        for (Iterator<MotionSegment> iterator = mSegments.iterator(); iterator.hasNext();) {
+            MotionSegment s = iterator.next();
+            if (s.end().t() <= t) {
+                // Segment is fully before t.
+                iterator.remove();
+                continue;
+            }
+            if (s.start().t() <= t) {
+                // Segment begins before t; let's shorten the segment.
+                s.setStart(s.start().extrapolate(t));
+            }
+            break;
+        }
+    }
+
+    /**
+     * Remove all segments.
+     */
+    public void clear() {
+        mSegments.clear();
+    }
+
+    /**
+     * Remove all segments and initialize to the desired state (actually a segment of length 0 that starts and ends at
+     * initial_state).
+     * 
+     * @param initial_state
+     *            The MotionState to initialize to.
+     */
+    public void reset(MotionState initial_state) {
+        clear();
+        mSegments.add(new MotionSegment(initial_state, initial_state));
+    }
+
+    /**
+     * Remove redundant segments (segments whose start and end states are coincident).
+     */
+    public void consolidate() {
+        for (Iterator<MotionSegment> iterator = mSegments.iterator(); iterator.hasNext() && mSegments.size() > 1;) {
+            MotionSegment s = iterator.next();
+            if (s.start().coincident(s.end())) {
+                iterator.remove();
+            }
+        }
+    }
+
+    /**
+     * Add to the profile by applying an acceleration control for a given time. This is appended to the previous last
+     * state.
+     * 
+     * @param acc
+     *            The acceleration to apply.
+     * @param dt
+     *            The period of time to apply the given acceleration.
+     */
+    public void appendControl(double acc, double dt) {
+        if (isEmpty()) {
+            System.err.println("Error!  Trying to append to empty profile");
+            return;
+        }
+        MotionState last_end_state = mSegments.get(mSegments.size() - 1).end();
+        MotionState new_start_state = new MotionState(last_end_state.t(), last_end_state.pos(), last_end_state.vel(),
+                acc);
+        appendSegment(new MotionSegment(new_start_state, new_start_state.extrapolate(new_start_state.t() + dt)));
+    }
+
+    /**
+     * Add to the profile by inserting a new segment. No validity checking is done.
+     * 
+     * @param segment
+     *            The segment to add.
+     */
+    public void appendSegment(MotionSegment segment) {
+        mSegments.add(segment);
+    }
+
+    /**
+     * Add to the profile by inserting a new profile after the final state. No validity checking is done.
+     * 
+     * @param profile
+     *            The profile to add.
+     */
+    public void appendProfile(MotionProfile profile) {
+        for (MotionSegment s : profile.segments()) {
+            appendSegment(s);
+        }
+    }
+
+    /**
+     * @return The number of segments.
+     */
+    public int size() {
+        return mSegments.size();
+    }
+
+    /**
+     * @return The list of segments.
+     */
+    public List<MotionSegment> segments() {
+        return mSegments;
+    }
+
+    /**
+     * @return The first state in the profile (or kInvalidState if empty).
+     */
+    public MotionState startState() {
+        if (isEmpty()) {
+            return MotionState.kInvalidState;
+        }
+        return mSegments.get(0).start();
+    }
+
+    /**
+     * @return The time of the first state in the profile (or NaN if empty).
+     */
+    public double startTime() {
+        return startState().t();
+    }
+
+    /**
+     * @return The pos of the first state in the profile (or NaN if empty).
+     */
+    public double startPos() {
+        return startState().pos();
+    }
+
+    /**
+     * @return The last state in the profile (or kInvalidState if empty).
+     */
+    public MotionState endState() {
+        if (isEmpty()) {
+            return MotionState.kInvalidState;
+        }
+        return mSegments.get(mSegments.size() - 1).end();
+    }
+
+    /**
+     * @return The time of the last state in the profile (or NaN if empty).
+     */
+    public double endTime() {
+        return endState().t();
+    }
+
+    /**
+     * @return The pos of the last state in the profile (or NaN if empty).
+     */
+    public double endPos() {
+        return endState().pos();
+    }
+
+    /**
+     * @return The duration of the entire profile.
+     */
+    public double duration() {
+        return endTime() - startTime();
+    }
+
+    /**
+     * @return The total distance covered by the profile. Note that distance is the sum of absolute distances of all
+     *         segments, so a reversing profile will count the distance covered in each direction.
+     */
+    public double length() {
+        double length = 0.0;
+        for (MotionSegment s : segments()) {
+            length += Math.abs(s.end().pos() - s.start().pos());
+        }
+        return length;
+    }
+
+    @Override
+    public String toString() {
+        StringBuilder builder = new StringBuilder("Profile:");
+        for (MotionSegment s : segments()) {
+            builder.append("\n\t");
+            builder.append(s);
+        }
+        return builder.toString();
+    }
+}
\ No newline at end of file
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java
new file mode 100644
index 0000000..4ff1e61
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.utility.motion;
+
+/**
+ * Constraints for constructing a MotionProfile.
+ */
+public class MotionProfileConstraints {
+    protected double max_abs_vel = Double.POSITIVE_INFINITY;
+    protected double max_abs_acc = Double.POSITIVE_INFINITY;
+
+    public MotionProfileConstraints(double max_vel, double max_acc) {
+        this.max_abs_vel = Math.abs(max_vel);
+        this.max_abs_acc = Math.abs(max_acc);
+    }
+
+    /**
+     * @return The (positive) maximum allowed velocity.
+     */
+    public double max_abs_vel() {
+        return max_abs_vel;
+    }
+
+    /**
+     * @return The (positive) maximum allowed acceleration.
+     */
+    public double max_abs_acc() {
+        return max_abs_acc;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (!(obj instanceof MotionProfileConstraints)) {
+            return false;
+        }
+        final MotionProfileConstraints other = (MotionProfileConstraints) obj;
+        return (other.max_abs_acc() == max_abs_acc()) && (other.max_abs_vel() == max_abs_vel());
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java
new file mode 100644
index 0000000..ff28e22
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java
@@ -0,0 +1,133 @@
+package org.usfirst.frc4388.utility.motion;
+
+import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
+
+/**
+ * A MotionProfileGenerator generates minimum-time MotionProfiles to travel from a given MotionState to a given
+ * MotionProfileGoal while obeying a set of MotionProfileConstraints.
+ */
+public class MotionProfileGenerator {
+    // Static class.
+    private MotionProfileGenerator() {
+    }
+
+    protected static MotionProfile generateFlippedProfile(MotionProfileConstraints constraints,
+            MotionProfileGoal goal_state, MotionState prev_state) {
+        MotionProfile profile = generateProfile(constraints, goal_state.flipped(), prev_state.flipped());
+        for (MotionSegment s : profile.segments()) {
+            s.setStart(s.start().flipped());
+            s.setEnd(s.end().flipped());
+        }
+        return profile;
+    }
+
+    /**
+     * Generate a motion profile.
+     * 
+     * @param constraints
+     *            The constraints to use.
+     * @param goal_state
+     *            The goal to use.
+     * @param prev_state
+     *            The initial state to use.
+     * @return A motion profile from prev_state to goal_state that satisfies constraints.
+     */
+    public synchronized static MotionProfile generateProfile(MotionProfileConstraints constraints,
+            MotionProfileGoal goal_state,
+            MotionState prev_state) {
+        double delta_pos = goal_state.pos() - prev_state.pos();
+        if (delta_pos < 0.0 || (delta_pos == 0.0 && prev_state.vel() < 0.0)) {
+            // For simplicity, we always assume the goal requires positive movement. If negative, we flip to solve, then
+            // flip the solution.
+            return generateFlippedProfile(constraints, goal_state, prev_state);
+        }
+        // Invariant from this point on: delta_pos >= 0.0
+        // Clamp the start state to be valid.
+        MotionState start_state = new MotionState(prev_state.t(), prev_state.pos(),
+                Math.signum(prev_state.vel()) * Math.min(Math.abs(prev_state.vel()), constraints.max_abs_vel()),
+                Math.signum(prev_state.acc()) * Math.min(Math.abs(prev_state.acc()), constraints.max_abs_acc()));
+        MotionProfile profile = new MotionProfile();
+        profile.reset(start_state);
+        // If our velocity is headed away from the goal, the first thing we need to do is to stop.
+        if (start_state.vel() < 0.0 && delta_pos > 0.0) {
+            final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc());
+            profile.appendControl(constraints.max_abs_acc(), stopping_time);
+            start_state = profile.endState();
+            delta_pos = goal_state.pos() - start_state.pos();
+        }
+        // Invariant from this point on: start_state.vel() >= 0.0
+        final double min_abs_vel_at_goal_sqr = start_state.vel2() - 2.0 * constraints.max_abs_acc() * delta_pos;
+        final double min_abs_vel_at_goal = Math.sqrt(Math.abs(min_abs_vel_at_goal_sqr));
+        final double max_abs_vel_at_goal = Math.sqrt(start_state.vel2() + 2.0 * constraints.max_abs_acc() * delta_pos);
+        double goal_vel = goal_state.max_abs_vel();
+        double max_acc = constraints.max_abs_acc();
+        if (min_abs_vel_at_goal_sqr > 0.0
+                && min_abs_vel_at_goal > (goal_state.max_abs_vel() + goal_state.vel_tolerance())) {
+            // Overshoot is unavoidable with the current constraints. Look at completion_behavior to see what we should
+            // do.
+            if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ABS_VEL) {
+                // Adjust the goal velocity.
+                goal_vel = min_abs_vel_at_goal;
+            } else if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ACCEL) {
+                if (Math.abs(delta_pos) < goal_state.pos_tolerance()) {
+                    // Special case: We are at the goal but moving too fast. This requires 'infinite' acceleration,
+                    // which will result in NaNs below, so we can return the profile immediately.
+                    profile.appendSegment(new MotionSegment(
+                            new MotionState(profile.endTime(), profile.endPos(), profile.endState().vel(),
+                                    Double.NEGATIVE_INFINITY),
+                            new MotionState(profile.endTime(), profile.endPos(), goal_vel, Double.NEGATIVE_INFINITY)));
+                    profile.consolidate();
+                    return profile;
+                }
+                // Adjust the max acceleration.
+                max_acc = Math.abs(goal_vel * goal_vel - start_state.vel2()) / (2.0 * delta_pos);
+            } else {
+                // We are going to overshoot the goal, so the first thing we need to do is come to a stop.
+                final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc());
+                profile.appendControl(-constraints.max_abs_acc(), stopping_time);
+                // Now we need to travel backwards, so generate a flipped profile.
+                profile.appendProfile(generateFlippedProfile(constraints, goal_state, profile.endState()));
+                profile.consolidate();
+                return profile;
+            }
+        }
+        goal_vel = Math.min(goal_vel, max_abs_vel_at_goal);
+        // Invariant from this point forward: We can achieve goal_vel at goal_state.pos exactly using no more than +/-
+        // max_acc.
+
+        // What is the maximum velocity we can reach (Vmax)? This is the intersection of two curves: one accelerating
+        // towards the goal from profile.finalState(), the other coming from the goal at max vel (in reverse). If Vmax
+        // is greater than constraints.max_abs_vel, we will clamp and cruise.
+        // Solve the following three equations to find Vmax (by substitution):
+        // Vmax^2 = Vstart^2 + 2*a*d_accel
+        // Vgoal^2 = Vmax^2 - 2*a*d_decel
+        // delta_pos = d_accel + d_decel
+        final double v_max = Math.min(constraints.max_abs_vel(),
+                Math.sqrt((start_state.vel2() + goal_vel * goal_vel) / 2.0 + delta_pos * max_acc));
+
+        // Accelerate to v_max
+        if (v_max > start_state.vel()) {
+            final double accel_time = (v_max - start_state.vel()) / max_acc;
+            profile.appendControl(max_acc, accel_time);
+            start_state = profile.endState();
+        }
+        // Figure out how much distance will be covered during deceleration.
+        final double distance_decel = Math.max(0.0,
+                (start_state.vel2() - goal_vel * goal_vel) / (2.0 * constraints.max_abs_acc()));
+        final double distance_cruise = Math.max(0.0, goal_state.pos() - start_state.pos() - distance_decel);
+        // Cruise at constant velocity.
+        if (distance_cruise > 0.0) {
+            final double cruise_time = distance_cruise / start_state.vel();
+            profile.appendControl(0.0, cruise_time);
+            start_state = profile.endState();
+        }
+        // Decelerate to goal velocity.
+        if (distance_decel > 0.0) {
+            final double decel_time = (start_state.vel() - goal_vel) / max_acc;
+            profile.appendControl(-max_acc, decel_time);
+        }
+
+        profile.consolidate();
+        return profile;
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java
new file mode 100644
index 0000000..1afaefe
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java
@@ -0,0 +1,143 @@
+package org.usfirst.frc4388.utility.motion;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
+
+/**
+ * A MotionProfileGoal defines a desired position and maximum velocity (at this position), along with the behavior that
+ * should be used to determine if we are at the goal and what to do if it is infeasible to reach the goal within the
+ * desired velocity bounds.
+ *
+ */
+public class MotionProfileGoal {
+    /**
+     * A goal consists of a desired position and specified maximum velocity magnitude. But what should we do if we would
+     * reach the goal at a velocity greater than the maximum? This enum allows a user to specify a preference on
+     * behavior in this case.
+     * 
+     * Example use-cases of each:
+     * 
+     * OVERSHOOT - Generally used with a goal max_abs_vel of 0.0 to stop at the desired pos without violating any
+     * constraints.
+     * 
+     * VIOLATE_MAX_ACCEL - If we absolutely do not want to pass the goal and are unwilling to violate the max_abs_vel
+     * (for example, there is an obstacle in front of us - slam the brakes harder than we'd like in order to avoid
+     * hitting it).
+     * 
+     * VIOLATE_MAX_ABS_VEL - If the max velocity is just a general guideline and not a hard performance limit, it's
+     * better to slightly exceed it to avoid skidding wheels.
+     */
+    public static enum CompletionBehavior {
+        // Overshoot the goal if necessary (at a velocity greater than max_abs_vel) and come back.
+        // Only valid if the goal velocity is 0.0 (otherwise VIOLATE_MAX_ACCEL will be used).
+        OVERSHOOT,
+        // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the max accel
+        // constraint.
+        VIOLATE_MAX_ACCEL,
+        // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the goal velocity.
+        VIOLATE_MAX_ABS_VEL
+    }
+
+    protected double pos;
+    protected double max_abs_vel;
+    protected CompletionBehavior completion_behavior = CompletionBehavior.OVERSHOOT;
+    protected double pos_tolerance = 1E-3;
+    protected double vel_tolerance = 1E-2;
+
+    public MotionProfileGoal() {
+    }
+
+    public MotionProfileGoal(double pos) {
+        this.pos = pos;
+        this.max_abs_vel = 0.0;
+        sanityCheck();
+    }
+
+    public MotionProfileGoal(double pos, double max_abs_vel) {
+        this.pos = pos;
+        this.max_abs_vel = max_abs_vel;
+        sanityCheck();
+    }
+
+    public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior) {
+        this.pos = pos;
+        this.max_abs_vel = max_abs_vel;
+        this.completion_behavior = completion_behavior;
+        sanityCheck();
+    }
+
+    public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior,
+            double pos_tolerance, double vel_tolerance) {
+        this.pos = pos;
+        this.max_abs_vel = max_abs_vel;
+        this.completion_behavior = completion_behavior;
+        this.pos_tolerance = pos_tolerance;
+        this.vel_tolerance = vel_tolerance;
+        sanityCheck();
+    }
+
+    public MotionProfileGoal(MotionProfileGoal other) {
+        this(other.pos, other.max_abs_vel, other.completion_behavior, other.pos_tolerance, other.vel_tolerance);
+    }
+
+    /**
+     * @return A flipped MotionProfileGoal (where the position is negated, but all other attributes remain the same).
+     */
+    public MotionProfileGoal flipped() {
+        return new MotionProfileGoal(-pos, max_abs_vel, completion_behavior, pos_tolerance, vel_tolerance);
+    }
+
+    public double pos() {
+        return pos;
+    }
+
+    public double max_abs_vel() {
+        return max_abs_vel;
+    }
+
+    public double pos_tolerance() {
+        return pos_tolerance;
+    }
+
+    public double vel_tolerance() {
+        return vel_tolerance;
+    }
+
+    public CompletionBehavior completion_behavior() {
+        return completion_behavior;
+    }
+
+    public boolean atGoalState(MotionState state) {
+        return atGoalPos(state.pos()) && (Math.abs(state.vel()) < (max_abs_vel + vel_tolerance)
+                || completion_behavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL);
+    }
+
+    public boolean atGoalPos(double pos) {
+        return epsilonEquals(pos, this.pos, pos_tolerance);
+    }
+
+    /**
+     * This method makes sure that the completion behavior is compatible with the max goal velocity.
+     */
+    protected void sanityCheck() {
+        if (max_abs_vel > vel_tolerance && completion_behavior == CompletionBehavior.OVERSHOOT) {
+            completion_behavior = CompletionBehavior.VIOLATE_MAX_ACCEL;
+        }
+    }
+
+    @Override
+    public String toString() {
+        return "pos: " + pos + " (+/- " + pos_tolerance + "), max_abs_vel: " + max_abs_vel + " (+/- " + vel_tolerance
+                + "), completion behavior: " + completion_behavior.name();
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (!(obj instanceof MotionProfileGoal)) {
+            return false;
+        }
+        final MotionProfileGoal other = (MotionProfileGoal) obj;
+        return (other.completion_behavior() == completion_behavior()) && (other.pos() == pos())
+                && (other.max_abs_vel() == max_abs_vel()) && (other.pos_tolerance() == pos_tolerance())
+                && (other.vel_tolerance() == vel_tolerance());
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java
new file mode 100644
index 0000000..5c18542
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java
@@ -0,0 +1,80 @@
+package org.usfirst.frc4388.utility.motion;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
+import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
+
+/**
+ * A MotionSegment is a movement from a start MotionState to an end MotionState with a constant acceleration.
+ */
+public class MotionSegment {
+    protected MotionState mStart;
+    protected MotionState mEnd;
+
+    public MotionSegment(MotionState start, MotionState end) {
+        mStart = start;
+        mEnd = end;
+    }
+
+    /**
+     * Verifies that:
+     * 
+     * 1. All segments have a constant acceleration.
+     * 
+     * 2. All segments have monotonic position (sign of velocity doesn't change).
+     * 
+     * 3. The time, position, velocity, and acceleration of the profile are consistent.
+     */
+    public boolean isValid() {
+        if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) {
+            // Acceleration is not constant within the segment.
+            System.err.println(
+                    "Segment acceleration not constant! Start acc: " + start().acc() + ", End acc: " + end().acc());
+            return false;
+        }
+        if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 && !epsilonEquals(start().vel(), 0.0, kEpsilon)
+                && !epsilonEquals(end().vel(), 0.0, kEpsilon)) {
+            // Velocity direction reverses within the segment.
+            System.err.println("Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel());
+            return false;
+        }
+        if (!start().extrapolate(end().t()).equals(end())) {
+            // A single segment is not consistent.
+            if (start().t() == end().t() && Double.isInfinite(start().acc())) {
+                // One allowed exception: If acc is infinite and dt is zero.
+                return true;
+            }
+            System.err.println("Segment not consistent! Start: " + start() + ", End: " + end());
+            return false;
+        }
+        return true;
+    }
+
+    public boolean containsTime(double t) {
+        return t >= start().t() && t <= end().t();
+    }
+
+    public boolean containsPos(double pos) {
+        return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos();
+    }
+
+    public MotionState start() {
+        return mStart;
+    }
+
+    public void setStart(MotionState start) {
+        mStart = start;
+    }
+
+    public MotionState end() {
+        return mEnd;
+    }
+
+    public void setEnd(MotionState end) {
+        mEnd = end;
+    }
+
+    @Override
+    public String toString() {
+        return "Start: " + start() + ", End: " + end();
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java
new file mode 100644
index 0000000..ec4d95f
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java
@@ -0,0 +1,166 @@
+package org.usfirst.frc4388.utility.motion;
+
+import static org.usfirst.frc4388.utility.Util.epsilonEquals;
+import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
+
+/**
+ * A MotionState is a completely specified state of 1D motion through time.
+ */
+public class MotionState {
+    protected final double t;
+    protected final double pos;
+    protected final double vel;
+    protected final double acc;
+
+    public static MotionState kInvalidState = new MotionState(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
+
+    public MotionState(double t, double pos, double vel, double acc) {
+        this.t = t;
+        this.pos = pos;
+        this.vel = vel;
+        this.acc = acc;
+    }
+
+    public MotionState(MotionState state) {
+        this(state.t, state.pos, state.vel, state.acc);
+    }
+
+    public double t() {
+        return t;
+    }
+
+    public double pos() {
+        return pos;
+    }
+
+    public double vel() {
+        return vel;
+    }
+
+    public double vel2() {
+        return vel * vel;
+    }
+
+    public double acc() {
+        return acc;
+    }
+
+    /**
+     * Extrapolates this MotionState to the specified time by applying this MotionState's acceleration.
+     * 
+     * @param t
+     *            The time of the new MotionState.
+     * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state.
+     */
+    public MotionState extrapolate(double t) {
+        return extrapolate(t, acc);
+    }
+
+    /**
+     * 
+     * Extrapolates this MotionState to the specified time by applying a given acceleration to the (t, pos, vel) portion
+     * of this MotionState.
+     * 
+     * @param t
+     *            The time of the new MotionState.
+     * @param acc
+     *            The acceleration to apply.
+     * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state (with the
+     *         specified accel).
+     */
+    public MotionState extrapolate(double t, double acc) {
+        final double dt = t - this.t;
+        return new MotionState(t, pos + vel * dt + .5 * acc * dt * dt, vel + acc * dt, acc);
+    }
+
+    /**
+     * Find the next time (first time > MotionState.t()) that this MotionState will be at pos. This is an inverse of the
+     * extrapolate() method.
+     * 
+     * @param pos
+     *            The position to query.
+     * @return The time when we are next at pos() if we are extrapolating with a positive dt. NaN if we never reach pos.
+     */
+    public double nextTimeAtPos(double pos) {
+        if (epsilonEquals(pos, this.pos, kEpsilon)) {
+            // Already at pos.
+            return t;
+        }
+        if (epsilonEquals(acc, 0.0, kEpsilon)) {
+            // Zero acceleration case.
+            final double delta_pos = pos - this.pos;
+            if (!epsilonEquals(vel, 0.0, kEpsilon) && Math.signum(delta_pos) == Math.signum(vel)) {
+                // Constant velocity heading towards pos.
+                return delta_pos / vel + t;
+            }
+            return Double.NaN;
+        }
+
+        // Solve the quadratic formula.
+        // ax^2 + bx + c == 0
+        // x = dt
+        // a = .5 * acc
+        // b = vel
+        // c = this.pos - pos
+        final double disc = vel * vel - 2.0 * acc * (this.pos - pos);
+        if (disc < 0.0) {
+            // Extrapolating this MotionState never reaches the desired pos.
+            return Double.NaN;
+        }
+        final double sqrt_disc = Math.sqrt(disc);
+        final double max_dt = (-vel + sqrt_disc) / acc;
+        final double min_dt = (-vel - sqrt_disc) / acc;
+        if (min_dt >= 0.0 && (max_dt < 0.0 || min_dt < max_dt)) {
+            return t + min_dt;
+        }
+        if (max_dt >= 0.0) {
+            return t + max_dt;
+        }
+        // We only reach the desired pos in the past.
+        return Double.NaN;
+    }
+
+    @Override
+    public String toString() {
+        return "(t=" + t + ", pos=" + pos + ", vel=" + vel + ", acc=" + acc + ")";
+    }
+
+    /**
+     * Checks if two MotionStates are epsilon-equals (all fields are equal within a nominal tolerance).
+     */
+    @Override
+    public boolean equals(Object other) {
+        return (other instanceof MotionState) && equals((MotionState) other, kEpsilon);
+    }
+
+    /**
+     * Checks if two MotionStates are epsilon-equals (all fields are equal within a specified tolerance).
+     */
+    public boolean equals(MotionState other, double epsilon) {
+        return coincident(other, epsilon) && epsilonEquals(acc, other.acc, epsilon);
+    }
+
+    /**
+     * Checks if two MotionStates are coincident (t, pos, and vel are equal within a nominal tolerance, but acceleration
+     * may be different).
+     */
+    public boolean coincident(MotionState other) {
+        return coincident(other, kEpsilon);
+    }
+
+    /**
+     * Checks if two MotionStates are coincident (t, pos, and vel are equal within a specified tolerance, but
+     * acceleration may be different).
+     */
+    public boolean coincident(MotionState other, double epsilon) {
+        return epsilonEquals(t, other.t, epsilon) && epsilonEquals(pos, other.pos, epsilon)
+                && epsilonEquals(vel, other.vel, epsilon);
+    }
+
+    /**
+     * Returns a MotionState that is the mirror image of this one. Pos, vel, and acc are all negated, but time is not.
+     */
+    public MotionState flipped() {
+        return new MotionState(t, -pos, -vel, -acc);
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java
new file mode 100644
index 0000000..dc912c8
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java
@@ -0,0 +1,8 @@
+package org.usfirst.frc4388.utility.motion;
+
+public class MotionUtil {
+    /**
+     * A constant for consistent floating-point equality checking within this library.
+     */
+    public static double kEpsilon = 1e-6;
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java
new file mode 100644
index 0000000..a7390b5
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java
@@ -0,0 +1,203 @@
+package org.usfirst.frc4388.utility.motion;
+
+import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
+
+/**
+ * A controller for tracking a profile generated to attain a MotionProfileGoal. The controller uses feedforward on
+ * acceleration, velocity, and position; proportional feedback on velocity and position; and integral feedback on
+ * position.
+ */
+public class ProfileFollower {
+    protected double mKp;
+    protected double mKi;
+    protected double mKv;
+    protected double mKffv;
+    protected double mKffa;
+
+    protected double mMinOutput = Double.NEGATIVE_INFINITY;
+    protected double mMaxOutput = Double.POSITIVE_INFINITY;
+    protected MotionState mLatestActualState;
+    protected MotionState mInitialState;
+    protected double mLatestPosError;
+    protected double mLatestVelError;
+    protected double mTotalError;
+
+    protected MotionProfileGoal mGoal = null;
+    protected MotionProfileConstraints mConstraints = null;
+    protected SetpointGenerator mSetpointGenerator = new SetpointGenerator();
+    protected SetpointGenerator.Setpoint mLatestSetpoint = null;
+
+    /**
+     * Create a new ProfileFollower.
+     * 
+     * @param kp
+     *            The proportional gain on position error.
+     * @param ki
+     *            The integral gain on position error.
+     * @param kv
+     *            The proportional gain on velocity error (or derivative gain on position error).
+     * @param kffv
+     *            The feedforward gain on velocity. Should be 1.0 if the units of the profile match the units of the
+     *            output.
+     * @param kffa
+     *            The feedforward gain on acceleration.
+     */
+    public ProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
+        resetProfile();
+        setGains(kp, ki, kv, kffv, kffa);
+    }
+
+    public void setGains(double kp, double ki, double kv, double kffv, double kffa) {
+        mKp = kp;
+        mKi = ki;
+        mKv = kv;
+        mKffv = kffv;
+        mKffa = kffa;
+    }
+
+    /**
+     * Completely clear all state related to the current profile (min and max outputs are maintained).
+     */
+    public void resetProfile() {
+        mTotalError = 0.0;
+        mInitialState = MotionState.kInvalidState;
+        mLatestActualState = MotionState.kInvalidState;
+        mLatestPosError = Double.NaN;
+        mLatestVelError = Double.NaN;
+        mSetpointGenerator.reset();
+        mGoal = null;
+        mConstraints = null;
+        resetSetpoint();
+    }
+
+    /**
+     * Specify a goal and constraints for achieving the goal.
+     */
+    public void setGoalAndConstraints(MotionProfileGoal goal, MotionProfileConstraints constraints) {
+        if (mGoal != null && !mGoal.equals(goal) && mLatestSetpoint != null) {
+            // Clear the final state bit since the goal has changed.
+            mLatestSetpoint.final_setpoint = false;
+        }
+        mGoal = goal;
+        mConstraints = constraints;
+    }
+
+    public void setGoal(MotionProfileGoal goal) {
+        setGoalAndConstraints(goal, mConstraints);
+    }
+
+    /**
+     * @return The current goal (null if no goal has been set since the latest call to reset()).
+     */
+    public MotionProfileGoal getGoal() {
+        return mGoal;
+    }
+
+    public void setConstraints(MotionProfileConstraints constraints) {
+        setGoalAndConstraints(mGoal, constraints);
+    }
+
+    public MotionState getSetpoint() {
+        return (mLatestSetpoint == null ? MotionState.kInvalidState : mLatestSetpoint.motion_state);
+    }
+
+    /**
+     * Reset just the setpoint. This means that the latest_state provided to update() will be used rather than feeding
+     * forward the previous setpoint the next time update() is called. This almost always forces a MotionProfile update,
+     * and may be warranted if tracking error gets very large.
+     */
+    public void resetSetpoint() {
+        mLatestSetpoint = null;
+    }
+
+    public void resetIntegral() {
+        mTotalError = 0.0;
+    }
+
+    /**
+     * Update the setpoint and apply the control gains to generate a control output.
+     * 
+     * @param latest_state
+     *            The latest *actual* state, used only for feedback purposes (unless this is the first iteration or
+     *            reset()/resetSetpoint() was just called, in which case this is the new start state for the profile).
+     * @param t
+     *            The timestamp for which the setpoint is desired.
+     * @return An output that reflects the control output to apply to achieve the new setpoint.
+     */
+    public synchronized double update(MotionState latest_state, double t) {
+        mLatestActualState = latest_state;
+        MotionState prev_state = latest_state;
+        if (mLatestSetpoint != null) {
+            prev_state = mLatestSetpoint.motion_state;
+        } else {
+            mInitialState = prev_state;
+        }
+        final double dt = Math.max(0.0, t - prev_state.t());
+        mLatestSetpoint = mSetpointGenerator.getSetpoint(mConstraints, mGoal, prev_state, t);
+
+        // Update error.
+        mLatestPosError = mLatestSetpoint.motion_state.pos() - latest_state.pos();
+        mLatestVelError = mLatestSetpoint.motion_state.vel() - latest_state.vel();
+
+        // Calculate the feedforward and proportional terms.
+        double output = mKp * mLatestPosError + mKv * mLatestVelError + mKffv * mLatestSetpoint.motion_state.vel()
+                + (Double.isNaN(mLatestSetpoint.motion_state.acc()) ? 0.0 : mKffa * mLatestSetpoint.motion_state.acc());
+        if (output >= mMinOutput && output <= mMaxOutput) {
+            // Update integral.
+            mTotalError += mLatestPosError * dt;
+            output += mKi * mTotalError;
+        } else {
+            // Reset integral windup.
+            mTotalError = 0.0;
+        }
+        // Clamp to limits.
+        output = Math.max(mMinOutput, Math.min(mMaxOutput, output));
+
+        return output;
+    }
+
+    public void setMinOutput(double min_output) {
+        mMinOutput = min_output;
+    }
+
+    public void setMaxOutput(double max_output) {
+        mMaxOutput = max_output;
+    }
+
+    public double getPosError() {
+        return mLatestPosError;
+    }
+
+    public double getVelError() {
+        return mLatestVelError;
+    }
+
+    /**
+     * We are finished the profile when the final setpoint has been generated. Note that this does not check whether we
+     * are anywhere close to the final setpoint, however.
+     * 
+     * @return True if the final setpoint has been generated for the current goal.
+     */
+    public boolean isFinishedProfile() {
+        return mGoal != null && mLatestSetpoint != null && mLatestSetpoint.final_setpoint;
+    }
+
+    /**
+     * We are on target if our actual state achieves the goal (where the definition of achievement depends on the goal's
+     * completion behavior).
+     * 
+     * @return True if we have actually achieved the current goal.
+     */
+    public boolean onTarget() {
+        if (mGoal == null || mLatestSetpoint == null) {
+            return false;
+        }
+        // For the options that don't achieve the goal velocity exactly, also count any instance where we have passed
+        // the finish line.
+        final double goal_to_start = mGoal.pos() - mInitialState.pos();
+        final double goal_to_actual = mGoal.pos() - mLatestActualState.pos();
+        final boolean passed_goal_state = Math.signum(goal_to_start) * Math.signum(goal_to_actual) < 0.0;
+        return mGoal.atGoalState(mLatestActualState)
+                || (mGoal.completion_behavior() != CompletionBehavior.OVERSHOOT && passed_goal_state);
+    }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java
new file mode 100644
index 0000000..f3ccb0c
--- /dev/null
+++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java
@@ -0,0 +1,114 @@
+package org.usfirst.frc4388.utility.motion;
+
+import java.util.Optional;
+
+/**
+ * A SetpointGenerate does just-in-time motion profile generation to supply a stream of setpoints that obey the given
+ * constraints to a controller. The profile is regenerated when any of the inputs change, but is cached (and trimmed as
+ * we go) if the only update is to the current state.
+ * 
+ * Note that typically for smooth control, a user will feed the last iteration's setpoint as the argument to
+ * getSetpoint(), and should only use a measured state directly on the first iteration or if a large disturbance is
+ * detected.
+ */
+public class SetpointGenerator {
+    /**
+     * A Setpoint is just a MotionState and an additional flag indicating whether this is setpoint achieves the goal
+     * (useful for higher-level logic to know that it is now time to do something else).
+     */
+    public static class Setpoint {
+        public MotionState motion_state;
+        public boolean final_setpoint;
+
+        public Setpoint(MotionState motion_state, boolean final_setpoint) {
+            this.motion_state = motion_state;
+            this.final_setpoint = final_setpoint;
+        }
+    }
+
+    protected MotionProfile mProfile = null;
+    protected MotionProfileGoal mGoal = null;
+    protected MotionProfileConstraints mConstraints = null;
+
+    public SetpointGenerator() {
+    }
+
+    /**
+     * Force a reset of the profile.
+     */
+    public void reset() {
+        mProfile = null;
+        mGoal = null;
+        mConstraints = null;
+    }
+
+    /**
+     * Get a new Setpoint (and generate a new MotionProfile if necessary).
+     * 
+     * @param constraints
+     *            The constraints to use.
+     * @param goal
+     *            The goal to use.
+     * @param prev_state
+     *            The previous setpoint (or measured state of the system to do a reset).
+     * @param t
+     *            The time to generate a setpoint for.
+     * @return The new Setpoint at time t.
+     */
+    public synchronized Setpoint getSetpoint(MotionProfileConstraints constraints, MotionProfileGoal goal,
+            MotionState prev_state,
+            double t) {
+        boolean regenerate = mConstraints == null || !mConstraints.equals(constraints) || mGoal == null
+                || !mGoal.equals(goal) || mProfile == null;
+        if (!regenerate && !mProfile.isEmpty()) {
+            Optional<MotionState> expected_state = mProfile.stateByTime(prev_state.t());
+            regenerate = !expected_state.isPresent() || !expected_state.get().equals(prev_state);
+        }
+        if (regenerate) {
+            // Regenerate the profile, as our current profile does not satisfy the inputs.
+            mConstraints = constraints;
+            mGoal = goal;
+            mProfile = MotionProfileGenerator.generateProfile(constraints, goal, prev_state);
+            // System.out.println("Regenerating profile: " + mProfile);
+        }
+
+        // Sample the profile at time t.
+        Setpoint rv = null;
+        if (!mProfile.isEmpty() && mProfile.isValid()) {
+            MotionState setpoint;
+            if (t > mProfile.endTime()) {
+                setpoint = mProfile.endState();
+            } else if (t < mProfile.startTime()) {
+                setpoint = mProfile.startState();
+            } else {
+                setpoint = mProfile.stateByTime(t).get();
+            }
+            // Shorten the profile and return the new setpoint.
+            mProfile.trimBeforeTime(t);
+            rv = new Setpoint(setpoint, mProfile.isEmpty() || mGoal.atGoalState(setpoint));
+        }
+
+        // Invalid or empty profile - just output the same state again.
+        if (rv == null) {
+            rv = new Setpoint(prev_state, true);
+        }
+
+        if (rv.final_setpoint) {
+            // Ensure the final setpoint matches the goal exactly.
+            rv.motion_state = new MotionState(rv.motion_state.t(), mGoal.pos(),
+                    Math.signum(rv.motion_state.vel()) * Math.max(mGoal.max_abs_vel(), Math.abs(rv.motion_state.vel())),
+                    0.0);
+        }
+
+        return rv;
+    }
+
+    /**
+     * Get the full profile from the latest call to getSetpoint(). Useful to check estimated time or distance to goal.
+     * 
+     * @return The profile from the latest call to getSetpoint(), or null if there is not yet a profile.
+     */
+    public MotionProfile getProfile() {
+        return mProfile;
+    }
+}