Merge branch 'develop'

.gradle/buildOutputCleanup/cache.properties

@@ -0,0 +1,2 @@
+#Sat Jan 19 11:45:29 MST 2019
+gradle.version=5.0

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "currentLanguage": "java",
+    "enableCppIntellisense": false,
+    "projectYear": "none",
+    "teamNumber": 4388
+}

2019robot/build.gradle

@@ -0,0 +1,62 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+}
+
+def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project EmbeddedTools.
+deploy {
+    targets {
+        roboRIO("roborio") {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = frc.getTeamNumber()
+        }
+    }
+    artifacts {
+        frcJavaArtifact('frcJava') {
+            targets << "roborio"
+            // Debug can be overridden by command line, for use with VSCode
+            debug = frc.getDebugOrDefault(false)
+        }
+        // Built in artifact to deploy arbitrary files to the roboRIO.
+        fileTreeArtifact('frcStaticFileDeploy') {
+            // The directory below is the local directory to deploy
+            files = fileTree(dir: 'src/main/deploy')
+            // Deploy to RoboRIO target, into /home/lvuser/deploy
+            targets << "roborio"
+            directory = '/home/lvuser/deploy'
+        }
+    }
+}
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Maven central needed for JUnit
+repositories {
+    mavenCentral()
+}
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+    compile wpi.deps.wpilib()
+    compile wpi.deps.vendor.java()
+    nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
+    nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
+    testCompile 'junit:junit:4.12'
+    //compile pathfinder()
+}
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } }
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+}

2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4388.controller;
+
+public interface IHandController {
+
+	public double getLeftXAxis();
+
+	public double getLeftYAxis();
+
+	public double getRightXAxis();
+	
+	public double getRightYAxis();
+
+	public double getLeftTriggerAxis();
+
+	public double getRightTriggerAxis();
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java

@@ -0,0 +1,72 @@
+
+package org.usfirst.frc4388.robot;
+
+
+/**
+ * A list of constants used by the rest of the robot code. This include physics
+ * constants as well as constants determined through calibrations.
+ */
+
+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;
+    public static double kTrackEffectiveDiameter = (kTrackWidthInches * kTrackWidthInches + kTrackLengthInches * kTrackLengthInches) / kTrackWidthInches;
+    public static double kTrackScrubFactor = 0.75;
+
+    // Drive constants
+    public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0;
+    public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0;
+    public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0;
+    public static double kDriveStraightBasicYawErrorDivisor = 20.0;		// steer parameter will be -yawError divided by this
+    public static double kDriveStraightBasicMaxSteerMagnitude = 0.8;	// keep absolute value of steer parameter below this
+    public static double kDriveTurnBasicTankMotorOutput = 0.4;
+    public static double kDriveTurnBasicSingleMotorOutput = 0.15;
+    public static double kElevatorWheelDiameterInches = 1;
+	// Encoders
+	public static int kDriveEncoderTicksPerRev = 4096;
+	public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI);
+	
+	// Elevator
+	public static int kElevatorEncoderTickesPerRev = 256;
+    public static double kElevatorInchesOfTravelPerRev = 3.75;
+	public static double kElevatorEncoderTicksPerInch = 126.36;
+	public static double kElevatorBasicPercentOutputUp = -0.85;
+	public static double kElevatorBasicPercentOutputDown =.7;
+	
+    // CONTROL LOOP GAINS
+
+    // PID gains for drive velocity loop (LOW GEAR)
+    // Units: error is 4096 counts/rev. Max output is +/- 1023 units.
+    public static double kDriveVelocityKp = 1.0;
+    public static double kDriveVelocityKi = 0.0;
+    public static double kDriveVelocityKd = 6.0;
+    public static double kDriveVelocityKf = 0.5;
+    public static int kDriveVelocityIZone = 0;
+    public static double kDriveVelocityRampRate = 0.0;
+    public static int kDriveVelocityAllowableError = 0;
+
+    // PID gains for drive base lock loop
+    // Units: error is 4096 counts/rev. Max output is +/- 1023 units.
+    public static double kDriveBaseLockKp = 0.5;
+    public static double kDriveBaseLockKi = 0;
+    public static double kDriveBaseLockKd = 0;
+    public static double kDriveBaseLockKf = 0;
+    public static int kDriveBaseLockIZone = 0;
+    public static double kDriveBaseLockRampRate = 0;
+    public static int kDriveBaseLockAllowableError = 10;
+
+    // PID gains for constant heading velocity control
+    // Units: Error is degrees. Output is inches/second difference to
+    // left/right.
+    public static double kDriveHeadingVelocityKp = 4.0; // 6.0;
+    public static double kDriveHeadingVelocityKi = 0.0;
+    public static double kDriveHeadingVelocityKd = 50.0;
+
+    // Path following constants
+    public static double kPathFollowingLookahead = 24.0; // inches
+    public static double kPathFollowingMaxVel = 120.0; // inches/sec
+    public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2
+
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -0,0 +1,123 @@
+
+
+
+package org.usfirst.frc4388.robot;
+
+import buttons.XBoxTriggerButton;
+import org.usfirst.frc4388.controller.IHandController;
+import org.usfirst.frc4388.controller.XboxController;
+import org.usfirst.frc4388.robot.commands.*;
+
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.*;
+import org.usfirst.frc4388.robot.subsystems.*;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.InternalButton;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import jaci.pathfinder.Pathfinder;
+
+
+
+public class OI
+{
+		private static OI instance;
+
+		private XboxController m_driverXbox;
+		private XboxController m_operatorXbox;
+
+		private OI()
+		{
+		  try
+		  {
+			// Driver controller
+			m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID);
+			m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
+
+	        XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
+	        CarriageIntake.whenPressed(new SetIntakeSpeed(BallIntake.BALL_INTAKE_SPEED));
+	        CarriageIntake.whenReleased(new SetIntakeSpeed(0.0));
+
+	        XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
+	        CarriageEject.whenPressed(new SetIntakeSpeed(BallIntake.BALL_EXTAKE_SPEED));
+			CarriageEject.whenReleased(new SetIntakeSpeed(0.0));
+
+			JoystickButton Expand = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
+			Expand.whenPressed(new WristPlacement(true));
+
+			JoystickButton Contract = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
+	        Contract.whenPressed(new WristPlacement(false));
+
+
+			JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
+			liftBothIntake.whenPressed(new HatchAndBallUp());
+
+			JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
+			liftHatchIntake.whenPressed(new LiftHatchDropBall());
+
+
+			JoystickButton liftBallIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+			//liftBallIntake.whenPressed(new HatchFlip(false));
+			liftBallIntake.whenPressed(new LiftBallDropHatch());
+
+
+			JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
+			JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS);
+
+			JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
+			ratchetFlip.whenPressed(new ratchetFlip(0.5));
+			ratchetFlip.whenReleased(new ratchetFlip(0));
+
+	        JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
+	        shiftUp.whenPressed(new DriveSpeedShift(true));
+	        //shiftUp.whenPressed(new LEDIndicators(true));
+
+	        JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+	        shiftDown.whenPressed(new DriveSpeedShift(false));
+	       // shiftDown.whenPressed(new LEDIndicators(false));
+	        //Operator Xbox
+			/*
+	        JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+	        openIntake.whenPressed(new IntakePosition(true));
+
+	        JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
+			CloseIntake.whenPressed(new IntakePosition(false));
+			*/
+			JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
+			safteySwitch.whenPressed(new setClimberSafety(true));
+			safteySwitch.whenReleased(new setClimberSafety(false));
+
+	        //SmartDashboard.putData("PRE GAME!!!!", new PreGame());
+		  } catch (Exception e) {
+			  System.err.println("An error occurred in the OI constructor");
+		  }
+		}
+
+		public static OI getInstance() {
+			if(instance == null) {
+				instance = new OI();
+			}
+			return instance;
+		}
+
+		public IHandController getDriverController() {
+			return m_driverXbox;
+		}
+
+		public IHandController getOperatorController()
+		{
+			return m_operatorXbox;
+		}
+
+		public Joystick getOperatorJoystick()
+		{
+			return m_operatorXbox.getJoyStick();
+		}
+	}

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -0,0 +1,153 @@
+
+package org.usfirst.frc4388.robot;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.DriverStation;
+//import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+//import org.usfirst.frc4388.controller.Robot.OperationMode;
+import org.usfirst.frc4388.robot.commands.*;
+
+import org.usfirst.frc4388.robot.OI;
+import org.usfirst.frc4388.robot.subsystems.*;
+import org.usfirst.frc4388.utility.ControlLooper;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;;
+
+public class Robot extends IterativeRobot 
+{
+
+	public static OI oi;
+	
+	public static final Drive drive = new Drive();
+	public static final Arm arm = new Arm();
+	public static final Wrist wrist = new Wrist();
+
+
+ 
+	public static final BallIntake ballIntake = new BallIntake();
+    
+
+
+    public static final Climber climber = new Climber();
+    public static final Pnumatics pnumatics = new Pnumatics();
+	public static final long periodMS = 10;
+	public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS);
+
+	public static enum OperationMode { TEST, COMPETITION };
+	public static OperationMode operationMode = OperationMode.TEST;
+
+	private SendableChooser<OperationMode> operationModeChooser;
+    public void robotInit() 
+    {
+    	//Printing game data to riolog
+    	String gameData = DriverStation.getInstance().getGameSpecificMessage();
+    	System.out.println(gameData);
+    	CameraServer.getInstance().startAutomaticCapture();
+    	CameraServer.getInstance().putVideo("res", 300, 220);
+    	
+      try {
+		oi = OI.getInstance();
+		
+		controlLoop.addLoopable(drive);
+		controlLoop.addLoopable(arm);
+		controlLoop.addLoopable(wrist);
+			
+
+        operationModeChooser = new SendableChooser<OperationMode>();
+	    operationModeChooser.addDefault("Test", OperationMode.TEST);
+	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
+		SmartDashboard.putData("Operation Mode", operationModeChooser);
+		
+		
+		
+		
+		
+		
+
+		
+		//ledLights.setAllLightsOn(false);
+      } catch (Exception e) {
+    		System.err.println("An error occurred in robotInit()");
+      }
+    }
+	
+    public void disabledInit(){
+
+    }
+	
+	public void disabledPeriodic() {
+		Scheduler.getInstance().run();
+		updateStatus();
+	}
+
+    public void autonomousInit() {    	
+    	updateChoosers();
+    	
+    	controlLoop.start();
+    	drive.endGyroCalibration();
+    	drive.resetEncoders();
+    	drive.resetGyro();
+    	drive.setIsRed(getAlliance().equals(Alliance.Red));
+        
+	}
+ 
+
+    
+    public void autonomousPeriodic() {
+        Scheduler.getInstance().run();
+		updateStatus();
+    }
+
+    public void teleopInit() {
+		System.err.println("Beginning of teleopInit.");
+
+        drive.setToBrakeOnNeutral(false);	
+		updateChoosers();
+		System.err.println("TeleopInit after UpdateChoosers");
+		controlLoop.start();
+		System.err.println("TeleopInit after controlLoop");
+		//drive.resetEncoders();
+		//System.err.println("TeleopInit after resetEncoders");
+		drive.endGyroCalibration();
+		System.err.println("TeleopInit after endGyroCalibrations");
+
+        updateStatus();
+    }
+
+
+    public void teleopPeriodic() 
+    {
+        Scheduler.getInstance().run();
+		updateStatus();
+    }
+    
+    public void testPeriodic() {
+        LiveWindow.run();
+		updateStatus();
+   }
+    
+    private void updateChoosers() {
+		operationMode = (OperationMode)operationModeChooser.getSelected();
+    }
+    
+    public Alliance getAlliance() {
+    	return m_ds.getAlliance();
+    }
+    
+    public void updateStatus() {
+    	drive.updateStatus(operationMode);
+
+   }
+
+}
+

2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java

@@ -0,0 +1,41 @@
+
+package org.usfirst.frc4388.robot;
+
+
+public class RobotMap {
+	// USB Port IDs
+	public static final int DRIVER_JOYSTICK_1_USB_ID = 0;
+	public static final int OPERATOR_JOYSTICK_1_USB_ID = 1;
+
+
+	// MOTORS 
+	
+	public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2;
+	public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3;
+
+
+	public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
+	public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
+	
+	//Intake motors
+	public static final int INTAKE_BELT_DRIVE_CAN_ID = 8;
+	  
+	//Elevator Motors
+	public static final int ARM_MOTOR1_ID= 6;
+	public static final int ARM_MOTOR2_ID = 7;
+	public static final int WRIST_MOTOR_ID = 9;
+
+	
+	//Climber Motors
+	public static final int CLIMBER_CAN_ID = 11;
+	public static final int CLIMBER_WHEEL1_ID = 12;
+	public static final int CLIMBER_WHEEL2_ID = 13;
+	
+	
+	// Pneumatics
+	public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0;
+	public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1;
+	public static final int OPEN_INTAKE_PCM_ID = 4;
+	public static final int CLOSE_INTAKE_PCM_ID = 5;
+
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java

@@ -0,0 +1,63 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ArmAutoZero extends Command
+{
+	private double MIN_ELEVATOR_POSITION_CHANGE = 0.05;
+	private double lastArmPosition;
+	private int encoderCount;
+	
+	public ArmAutoZero(boolean interrutible) {
+			requires(Robot.arm);
+		setInterruptible(interrutible);
+	}
+
+	@Override
+	protected void initialize() {
+		lastArmPosition = Arm.MAX_POSITION_INCHES;
+		Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
+		encoderCount = 0;
+//		System.out.println("Auto zero initialize");
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
+		double currentArmPosition = Robot.arm.getPositionInches();
+		double armPositionChange = lastArmPosition - currentArmPosition;
+		lastArmPosition = currentArmPosition;
+		boolean test = encoderCount > 2 && Math.abs(armPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.arm.getAverageMotorCurrent() > Arm.AUTO_ZERO_MOTOR_CURRENT;
+		System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", arm change = " + armPositionChange + ", current = " + Robot.arm.getAverageMotorCurrent());
+		
+		if (Math.abs(armPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) {
+			encoderCount++;
+		}
+		else {
+			encoderCount = 0;
+		}
+		
+		return test;
+	}
+
+	@Override
+	protected void end() {
+		Robot.arm.setSpeed(0);
+		Robot.arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
+		Robot.arm.setPositionPID(Arm.MIN_POSITION_INCHES);
+//		System.out.println("Arm Zeroed");
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java

@@ -0,0 +1,50 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ArmSetMode extends Command {
+
+	private ArmControlMode controlMode;
+	
+    public ArmSetMode(ArmControlMode controlMode) {
+    	this.controlMode = controlMode;
+        requires(Robot.arm);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	if (controlMode == ArmControlMode.JOYSTICK_PID) {
+    		Robot.arm.setPositionPID(Robot.arm.getPositionInches());
+    	}
+    	else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
+    		Robot.arm.setSpeedJoystick(0);
+    	}
+    	else {
+    		Robot.arm.setSpeed(0.0);
+    	}
+    }
+
+    // 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 true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java

@@ -0,0 +1,55 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ArmSetPositionMP extends Command {
+	
+	private double targetPositionInches;
+	private boolean isAtTarget;
+	private static final double MIN_DELTA_TARGET = 0.3;
+
+    public ArmSetPositionMP(double targetPositionInches) {
+    	this.targetPositionInches = targetPositionInches;
+        requires(Robot.arm);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	if (Math.abs(targetPositionInches - Robot.arm.getPositionInches()) < MIN_DELTA_TARGET) {
+    		isAtTarget = true;
+    	}
+    	else {
+        	isAtTarget = false;
+        	Robot.arm.setPositionMP(targetPositionInches);
+    	}
+//    	System.out.println("Arm set MP initialized, target = " + targetPositionInches);
+    }
+
+    // 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 isAtTarget || Robot.arm.isFinished();
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+		Robot.arm.setPositionPID(Robot.arm.getPositionInches());
+//    	System.out.println("Arm set MP end");
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+//    	System.out.println("ArmSetPositionMP interrupted");
+    	Robot.arm.setFinished(true);
+		Robot.arm.setPositionPID(Robot.arm.getPositionInches());
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ArmSetPositionPID extends Command {
+	
+	private double targetPositionInches;
+
+    public ArmSetPositionPID(double targetPositionInches) {
+    	this.targetPositionInches = targetPositionInches;
+        requires(Robot.arm);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+		Robot.arm.setPositionPID(targetPositionInches);
+    }
+
+    // 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 Math.abs(Robot.arm.getPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java

@@ -0,0 +1,44 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import org.usfirst.frc4388.robot.subsystems.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ArmSetSpeed extends Command {
+
+	private double RaiseSpeed;
+	
+	// Constructor with speed
+    public ArmSetSpeed(double RaiseSpeed) {
+    	this.RaiseSpeed = RaiseSpeed;
+       // requires(Robot.elevatorAuton);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	//Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
+    }
+
+    // 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 true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetZero.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ArmSetZero extends Command
+{
+	private double position;
+	
+	public ArmSetZero(double position) {
+		this.position = position;
+		requires(Robot.arm);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.arm.resetZeroPosition(position);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DeployBallIntake.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DeployBallIntake extends Command
+{
+	public boolean IsUp;
+	
+	public DeployBallIntake(boolean IsUp) {
+		this.IsUp=IsUp;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setBallIntake(IsUp);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java

@@ -0,0 +1,151 @@
+package org.usfirst.frc4388.robot.commands;
+
+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.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DriveTurnBasic extends Command
+{
+	private boolean m_useAbsolute;			// if true, we don't reset the gyro before starting the turn
+	private double m_turnAngleDeg;			// either absolute or relative to current heading, depending on useAbsolute
+	private double m_maxTurnRateDegPerSec;
+	private MPSoftwareTurnType m_turnType;
+	private boolean m_turningLeft;
+	private double m_targetGyroAngleDeg;	// what we want the gyro to read when we're done
+
+	public DriveTurnBasic(boolean useAbsolute, double turnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) {
+		requires(Robot.drive);
+		m_useAbsolute = useAbsolute;
+		m_turnAngleDeg = turnAngleDeg;
+		m_maxTurnRateDegPerSec = maxTurnRateDegPerSec;
+		m_turnType = turnType;
+	}
+
+	protected void initialize() {
+		double currentAngleDeg = Robot.drive.getGyroAngleDeg();
+		if (m_useAbsolute) {
+			m_targetGyroAngleDeg = m_turnAngleDeg;
+		} else {
+			m_targetGyroAngleDeg = currentAngleDeg + m_turnAngleDeg;
+		}
+		if ((m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.RIGHT_ARC)) {
+			m_turningLeft = true;	// gyro angle will be DEcreasing, so make sure we aim for an angle in that direction
+			while (m_targetGyroAngleDeg >= currentAngleDeg) {
+				m_targetGyroAngleDeg = m_targetGyroAngleDeg - 360.0;
+			}
+		} else if ((m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.LEFT_ARC)) {
+			m_turningLeft = false;	// gyro angle will be INcreasing, so make sure we aim for an angle in that direction
+			while (m_targetGyroAngleDeg <= currentAngleDeg) {
+				m_targetGyroAngleDeg = m_targetGyroAngleDeg + 360.0;
+			}
+		} else {	// MPSoftwareTurnType.TANK -- need to determine based on values passed
+			if (m_useAbsolute) {	// this is the only case where we have to DECIDE which direction to turn
+				m_turnAngleDeg = BHRMathUtils.normalizeAngle0To360(m_turnAngleDeg);
+				m_targetGyroAngleDeg = BHRMathUtils.adjustAccumAngleToDesired(currentAngleDeg, m_turnAngleDeg);
+				m_turningLeft = (m_targetGyroAngleDeg < currentAngleDeg);
+			} else {
+				m_turningLeft = (m_turnAngleDeg < 0);
+			}
+		}
+		System.out.println("Turning " + (m_turningLeft?"left":"right") + " from " + currentAngleDeg + " to " + m_targetGyroAngleDeg + " degrees");
+    	Robot.drive.setControlMode(DriveControlMode.RAW);
+    	Robot.drive.resetEncoders();
+	}
+
+	protected void execute() {
+		double output = Constants.kDriveTurnBasicSingleMotorOutput;
+		
+		if (m_turnType == MPSoftwareTurnType.TANK) {
+			output = Constants.kDriveTurnBasicTankMotorOutput;
+			if (m_turningLeft) {
+				Robot.drive.rawDriveLeftRight(-output, -output);	// left backward, right forward
+			} else {
+				Robot.drive.rawDriveLeftRight(output, output);		// left forward, right backward
+			}
+//			for (CANTalonEncoder motorController : motorControllers) {
+//				//motorController.set(output);
+//				motorController.set(ControlMode.PercentOutput, output);
+//			}
+		}
+		else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
+			Robot.drive.rawDriveLeftRight(2.0 * output, 0.0);		// left forward double speed
+//			for (CANTalonEncoder 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);
+//				}
+//			}
+		}
+		else if (m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
+			Robot.drive.rawDriveLeftRight(0.0, -2.0 * output);		// right forward double speed
+//			for (CANTalonEncoder 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);
+//				}
+//			}
+		}
+		else if (m_turnType == MPSoftwareTurnType.LEFT_ARC) {
+			Robot.drive.rawDriveLeftRight(2.0 * output, -output);	// left fwd 2x, right fwd 1x
+//			for (CANTalonEncoder 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 (m_turnType == MPSoftwareTurnType.RIGHT_ARC) {
+			Robot.drive.rawDriveLeftRight(output, -2.0 * output);	// left fwd 1x, right fwd 2x
+//			for (CANTalonEncoder 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);					
+//				}
+//			}
+		}
+	}
+
+	protected boolean isFinished() {
+		boolean finished;
+		double currentAngleDeg = Robot.drive.getGyroAngleDeg();
+		
+		if (m_turningLeft) {
+			finished = currentAngleDeg <= m_targetGyroAngleDeg;
+		} else {
+			finished = currentAngleDeg >= m_targetGyroAngleDeg;
+		}
+		return finished;
+	}
+
+	protected void end() {
+		Robot.drive.rawDriveLeftRight(0.0, 0.0);
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+	}
+
+	protected void interrupted() {
+		end();
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchAndBallUp.java

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class HatchAndBallUp extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public HatchAndBallUp() {
+    addSequential(new HatchFlip(true));
+    addParallel(new DeployBallIntake(false));
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class HatchFlip extends Command
+{
+	public boolean PickingUp;
+	
+	public HatchFlip(boolean PickingUP) {
+		this.PickingUp=PickingUP;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setHatchIntakeState(PickingUp);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LiftBallDropHatch.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class LiftBallDropHatch extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public LiftBallDropHatch() {
+    addSequential(new HatchFlip(false));
+    addParallel(new DeployBallIntake(false));
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LiftHatchDropBall.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class LiftHatchDropBall extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public LiftHatchDropBall() {
+    addSequential(new HatchFlip(true));
+    addParallel(new DeployBallIntake(true));
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetIntakeSpeed.java

@@ -0,0 +1,44 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import org.usfirst.frc4388.robot.subsystems.BallIntake;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SetIntakeSpeed extends Command {
+
+	private double WheelSpeed;
+	
+	// Constructor with speed
+    public SetIntakeSpeed(double WheelSpeed) {
+    	this.WheelSpeed = WheelSpeed;
+         requires(Robot.ballIntake);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.ballIntake.setWheelSpeed(WheelSpeed);
+    }
+
+    // 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 true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristPlacement.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class WristPlacement extends Command
+{
+	public boolean Forward;
+	
+	public WristPlacement(boolean Forward) {
+		this.Forward=Forward;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setWrist(Forward);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ratchetFlip.java

@@ -0,0 +1,46 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Climber;
+import org.usfirst.frc4388.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ratchetFlip extends Command {
+  private double speed;
+  public ratchetFlip(double speed) {
+    requires(Robot.climber);
+    this.speed = speed;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    if (Climber.SPEED == 0){
+      Robot.climber.flipRatchet(speed);
+    }
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+      
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class setClimberSafety extends Command {
+  boolean safety;
+  public setClimberSafety(boolean safety) {
+    requires(Robot.climber);
+		this.safety = safety;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.climber.safetySwitch(safety);
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java

@@ -0,0 +1,361 @@
+package org.usfirst.frc4388.robot.subsystems;
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.Loop;
+import org.usfirst.frc4388.utility.MPTalonPIDController;
+import org.usfirst.frc4388.utility.PIDParams;
+import org.usfirst.frc4388.utility.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.TalonSRXFactory;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.SensorCollection;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Arm extends Subsystem implements ControlLoopable
+{
+	private static Arm instance;
+
+	public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
+
+	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks 
+	public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60;  
+	
+
+	private double periodMs;
+	private double lastControlLoopUpdatePeriod = 0.0;
+	private double lastControlLoopUpdateTimestamp = 0.0;
+	// Defined speeds
+	public static final double CLIMB_SPEED = -1.0;
+	public static final double TEST_SPEED_UP = 0.3;
+	public static final double TEST_SPEED_DOWN = -0.3;
+	public static final double AUTO_ZERO_SPEED = -0.3;
+	public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
+	public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
+	
+	// Defined positions
+	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
+	public static final double ZERO_POSITION_INCHES = -0.25;
+	public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
+	public static final double MIN_POSITION_INCHES = 0.0;
+	public static final double MAX_POSITION_INCHES = 83.4;
+	public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
+
+	public static final double SWITCH_POSITION_INCHES = 24.0;
+	public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
+	public static final double SCALE_LOW_POSITION_INCHES = 56.0;
+	public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
+	public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
+	public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
+	public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
+	public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
+	public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
+
+	// Motion profile max velocities and accel times
+	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60; 
+	public static final double MP_T1 = 400;  // Fast = 300
+	public static final double MP_T2 = 150;  // Fast = 150
+	
+	// Motor controllers
+	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
+
+	private TalonSRXEncoder motor1;
+	private TalonSRX motor2;
+	
+	// PID controller and params
+	private MPTalonPIDController mpController;
+
+	public static int PID_SLOT = 0;
+	public static int MP_SLOT = 1;
+
+	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
+	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);  
+	public static final double KF_UP = 0.005;
+	public static final double KF_DOWN = 0.0;
+	public static final double PID_ERROR_INCHES = 1.0;
+	LimitSwitchSource limitSwitchSource;
+	// Pneumatics
+	private Solenoid speedShift;
+
+	private ArmControlMode controlMode = ArmControlMode.JOYSTICK_MANUAL;
+	// Misc
+	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;	
+	private boolean isFinished;
+	private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
+	private double targetPositionInchesPID = 0;
+	private boolean firstMpPoint;
+	private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
+	
+	public Arm() {
+		try {
+			motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
+			motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID);
+			
+			
+			motor1.setInverted(true);
+			motor2.setInverted(true);
+										
+//	        if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
+//	            Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
+//	        }
+			motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			motor1.setNeutralMode(NeutralMode.Brake);
+    		motor2.setNeutralMode(NeutralMode.Brake);
+			
+			motorControllers.add(motor1);
+			
+			
+		}
+		catch (Exception e) {
+			System.err.println("An error occurred in the Arm constructor");
+		}
+	}
+
+	@Override
+	public void initDefaultCommand() {
+	}
+		
+	public void resetZeroPosition(double position) {
+		mpController.resetZeroPosition(position);
+	}	
+	
+	private synchronized void setArmControlMode(ArmControlMode controlMode) {
+		this.armControlMode = controlMode;
+	}
+	
+	private synchronized ArmControlMode getArmControlMode() {
+		return this.armControlMode;
+	}
+
+	public void setSpeed(double speed) {
+		motor1.set(ControlMode.PercentOutput, speed);
+		setArmControlMode(ArmControlMode.MANUAL);
+	}
+		
+	public void setSpeedJoystick(double speed) {
+		motor1.set(ControlMode.PercentOutput, speed);
+		setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
+	}
+		
+	public void setPositionPID(double targetPositionInches) {
+		mpController.setPIDSlot(PID_SLOT);
+		updatePositionPID(targetPositionInches);
+		setArmControlMode(ArmControlMode.JOYSTICK_PID);	
+		setFinished(false);
+	}
+	
+	public void updatePositionPID(double targetPositionInches) {
+ 		targetPositionInchesPID = limitPosition(targetPositionInches);
+		double startPositionInches = motor1.getPositionWorld();
+		mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
+	}
+	
+	public void setPositionMP(double targetPositionInches) {
+		double startPositionInches = motor1.getPositionWorld();
+		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
+		setFinished(false);
+		firstMpPoint = true;
+		setArmControlMode(ArmControlMode.MOTION_PROFILE);
+ 	}
+	
+	private double limitPosition(double targetPosition) {
+		if (targetPosition < MIN_POSITION_INCHES) {
+			return MIN_POSITION_INCHES;
+		}
+		else if (targetPosition > MAX_POSITION_INCHES) {
+			return MAX_POSITION_INCHES;
+		}
+		
+		return targetPosition;
+	}
+	@Override
+	public void setPeriodMs(long periodMs) {
+		mpController = new MPTalonPIDController(periodMs, motorControllers);
+		mpController.setPID(mpPIDParams, MP_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		this.periodMs = periodMs;
+	}
+	/*@Override
+	public void onStart(double timestamp) {
+		mpController = new MPTalonPIDController(periodMs, motorControllers);
+		mpController.setPID(mpPIDParams, MP_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+	}
+
+	@Override
+	public void onStop(double timestamp) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void onLoop(double timestamp) {
+		synchronized (Arm.this) {
+			switch( getElevatorControlMode() ) {
+				case JOYSTICK_PID: 
+					controlPidWithJoystick();
+					break;
+				case JOYSTICK_MANUAL:
+					controlManualWithJoystick();
+					break;
+				case MOTION_PROFILE: 
+					if (!isFinished()) {
+						if (firstMpPoint) {
+							mpController.setPIDSlot(MP_SLOT);
+							firstMpPoint = false;
+						}
+						setFinished(mpController.controlLoopUpdate()); 
+						if (isFinished()) {
+							mpController.setPIDSlot(PID_SLOT);
+						}
+					}
+					break;
+				default:
+					break;
+			}
+		}
+	}
+
+*/
+
+
+
+
+
+	public synchronized void controlLoopUpdate() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (lastControlLoopUpdateTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
+		}
+		lastControlLoopUpdateTimestamp = currentTimestamp;
+		
+		// Do the update
+		if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
+			controlManualWithJoystick();
+		}
+		else if (!isFinished) {
+			if (controlMode == ArmControlMode.MOTION_PROFILE) {
+				isFinished = mpController.controlLoopUpdate(getPositionInches()); 
+			}
+			
+			/*else if (controlMode == ArmControlMode.MP_PATH_VELOCITY) {
+				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == ArmControlMode.ADAPTIVE_PURSUIT) {
+				updatePose();
+				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
+			}*/
+		}
+	}
+
+
+
+
+
+
+	
+	private void controlPidWithJoystick() {
+		double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
+		double deltaPosition = joystickPosition * joystickInchesPerMs;
+		targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
+		updatePositionPID(targetPositionInchesPID);
+	}
+
+	private void ControlWithJoystickhold(){
+		double holdPosition = motor1.getPositionWorld();
+		updatePositionPID(holdPosition);
+
+	}
+	
+	private void controlManualWithJoystick() {
+		double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
+		setSpeedJoystick(joyStickSpeed*.60);
+	}
+	/*
+	public void setShiftState(ElevatorSpeedShiftState state) {
+
+			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI;
+			speedShift.set(true);
+			mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		}
+		else if(state == ElevatorSpeedShiftState.LO) {
+			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
+			speedShift.set(false);
+			mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
+		}
+	}
+	
+	public ElevatorSpeedShiftState getShiftState() {
+		return shiftState;
+	}
+*/
+	public double getPositionInches() {
+		return motor1.getPositionWorld();
+	}
+	
+//	public double getAverageMotorCurrent() {
+//		return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
+//	}
+		
+	public double getAverageMotorCurrent() {
+		return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2;
+	}
+		
+	public synchronized boolean isFinished() {
+		return isFinished;
+	}
+	
+	public synchronized void setFinished(boolean isFinished) {
+		this.isFinished = isFinished;
+	}
+		
+	public double getPeriodMs() {
+		return periodMs;
+	}
+	
+	public void updateStatus(Robot.OperationMode operationMode) {
+		if (operationMode == Robot.OperationMode.TEST) {
+			try {
+				SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld());
+				SmartDashboard.putNumber("Elevator Motor 1 Amps", motor1.getOutputCurrent());
+				SmartDashboard.putNumber("Elevator Motor 2 Amps", motor2.getOutputCurrent());
+				SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
+//				SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
+//				SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
+//				SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
+				SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
+			}
+			catch (Exception e) {
+			}
+		}
+	}	
+	
+	public static Arm getInstance() {
+		if(instance == null) {
+			instance = new Arm();
+		}
+		return instance;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java

@@ -0,0 +1,60 @@
+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.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.Looper;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import org.usfirst.frc4388.robot.OI;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+
+
+
+/**
+ *
+ */
+public class BallIntake extends Subsystem {
+
+	private WPI_TalonSRX BallIntakeMain;
+	public static enum BallIntakeControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL};
+	public static final double BALL_INTAKE_SPEED = -1.0;
+	public static final double BALL_EXTAKE_SPEED = 1.0;
+	public static final double CUBE_STOP_SPEED = 0;
+	/////^^^^^^^^^ replace this line with the modes we need
+	
+	
+	private boolean isFinished;
+	//private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK;
+	
+	
+	
+	
+	public BallIntake() {
+		try {
+			BallIntakeMain = new WPI_TalonSRX(RobotMap.INTAKE_BELT_DRIVE_CAN_ID);
+			//\][carriageLeft.set(CurrentLimit, value);
+			
+    }
+		catch (Exception e) {
+			System.err.println("An error occurred in the Ball Intake constructor");
+			
+			
+		}
+    }
+	
+		public void setWheelSpeed(double speed) {
+			BallIntakeMain.set(-speed);
+    }
+
+
+    @Override
+    public void periodic() {
+    	
+    }
+    	public void initDefaultCommand() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java

@@ -0,0 +1,129 @@
+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.OI;
+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.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.Looper;
+import org.usfirst.frc4388.utility.PIDParams;
+import org.usfirst.frc4388.utility.SoftwarePIDController;
+
+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 Climber extends Subsystem{
+	
+	//Motors
+	private WPI_TalonSRX climberBack;
+	private WPI_TalonSRX climberFront;
+	private WPI_TalonSRX climberFront2;
+	private WPI_TalonSRX flipOutMotor;
+
+	//Frequency Control
+	static float BACK_FREQ = 1;
+	static float FRONT_FREQ = 1;
+	static float FREQ_RATIO = 0.2443744576F;
+
+	//Limit and Saftey vars
+	LimitSwitchSource limitSwitchSource;
+	SensorCollection isPressed;
+	boolean safetySwitch;
+
+	//Climb Trigger
+	double rightTriggerInput;
+	double leftTriggerInput;
+
+	public static double SPEED;
+	
+	public Climber(){
+		
+		try{
+			climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID);
+			climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID);
+			climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID);
+			climberFront2.set(ControlMode.Follower, climberFront.getDeviceID());
+
+			climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			climberBack.setNeutralMode(NeutralMode.Brake);
+			climberBack.setInverted(true);
+			climberFront.setNeutralMode(NeutralMode.Brake);
+			FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed
+		} 
+		catch (Exception e) {
+			System.err.println("The code broke before the guard did. An error occurred in the climbing constructor");
+		}
+	}
+
+    @Override
+    public void initDefaultCommand() {
+
+    }
+
+
+    @Override
+    public void periodic() {
+
+		rightTriggerInput = OI.getInstance().getDriverController().getRightTriggerAxis();
+		leftTriggerInput = OI.getInstance().getDriverController().getLeftTriggerAxis();
+		//SPEED = rightTriggerInput - leftTriggerInput;
+
+		// Put code here to be run every loop
+		if (safetySwitch) {
+			/*
+			if (isPressed.isRevLimitSwitchClosed() && speed < 0){ //If leg at min height, and the input would retract the leg
+				climberBack.set(0);
+				climberFront.set(0);
+			}
+			else if(isPressed.isFwdLimitSwitchClosed() && speed > 0){ //If leg at max height, and the input would extend the leg
+				climberBack.set(0);
+				climberFront.set(FRONT_FREQ * speed);
+			} *//*
+			if (SPEED < 0){
+				climberBack.set(0.3 * SPEED);
+			} 
+			else { //If leg not at max height
+				climberBack.set(0.5 * SPEED);
+				climberFront.set(SPEED);
+			}*/
+			climberBack.set(-rightTriggerInput * 0.3);
+			climberFront.set(leftTriggerInput);		
+		}
+		else {
+			climberBack.set(0);
+		}
+	}
+
+	public void safetySwitch(boolean safetySwitch){
+		this.safetySwitch = safetySwitch;
+	}
+
+	public void flipRatchet(double speed){
+		climberFront.set(speed);
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java

@@ -0,0 +1,902 @@
+
+package org.usfirst.frc4388.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.Set;
+
+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.control.AdaptivePurePursuitController;
+import org.usfirst.frc4388.utility.BHRMathUtils;
+//import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+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;
+import org.usfirst.frc4388.utility.MPTalonPIDPathController;
+import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController;
+//import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
+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.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.SoftwarePIDController;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+//import com.ctre.PigeonImu;
+//import com.ctre.PigeonImu.CalibrationMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+
+import com.kauailabs.navx.frc.AHRS;
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+//import edu.wpi.first.wpilibj.DigitalInput;
+//import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+//import edu.wpi.first.wpilibj.Solenoid;
+//import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+/**
+ *
+ */
+public class Drive extends Subsystem implements ControlLoopable
+{
+	public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW};
+	public static enum SpeedShiftState { HI, LO };
+	public static enum ClimberState { DEPLOYED, RETRACTED };
+
+	public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches;
+	public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
+	
+	public static final double CLIMB_SPEED = 0.45;
+	
+	public static final double VOLTAGE_RAMP_RATE = 150;  // Volts per second
+	public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE;	// Seconds from neutral to full
+	public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE;	// Seconds from neutral to full
+
+	// Motion profile max velocities and accel times
+	public static final double MAX_TURN_RATE_DEG_PER_SEC = 320;
+	public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC =  120;  //72;
+	public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC =  200;  
+	public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC =  320;  
+	public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC =  400;  
+	public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC =  270;
+	public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC =  400;
+	public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25;
+	public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80;
+	
+	
+	
+	
+	public static final double MP_STRAIGHT_T1 = 600;
+	public static final double MP_STRAIGHT_T2 = 300;
+	public static final double MP_TURN_T1 = 600;
+	public static final double MP_TURN_T2 = 300;
+	public static final double MP_MAX_TURN_T1 = 400;
+	public static final double MP_MAX_TURN_T2 = 200;
+	
+	// Motor controllers
+	private ArrayList<CANPIDController> motorControllers = new ArrayList<CANPIDController>();	
+
+	private CANSparkMax leftDrive1;
+	private CANSparkMax leftDrive2;	
+	private CANSparkMax rightDrive1;
+	private CANSparkMax rightDrive2;
+	private CANEncoder encoderLeft;
+	private CANEncoder encoderRight;
+
+	private DifferentialDrive m_drive;
+	
+	//PID encoder and motor
+
+	//private DigitalInput hopperSensorRed;
+	//private DigitalInput hopperSensorBlue;
+
+	
+	
+	private double climbSpeed;
+	
+	private boolean isRed = true;
+	
+	private double periodMs;
+	private double lastControlLoopUpdatePeriod = 0.0;
+	private double lastControlLoopUpdateTimestamp = 0.0;
+
+	// Pneumatics
+	//private Solenoid speedShift;
+
+	// Input devices
+	public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0;
+	public static final int DRIVER_INPUT_JOYSTICK_TANK = 1;
+	public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2;
+	public static final int DRIVER_INPUT_XBOX_CHEESY = 3;
+	public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4;
+	public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5;
+	public static final int DRIVER_INPUT_WHEEL = 6;
+
+	public static final double STEER_NON_LINEARITY = 0.5;
+	public static final double MOVE_NON_LINEARITY = 1.0;
+	
+	public static final double STICK_DEADBAND = 0.02;
+
+	private int m_moveNonLinear = 0;
+	private int m_steerNonLinear = -3;
+
+	private double m_moveScale = 1.0;
+	private double m_steerScale = 1.0;
+
+	private double m_moveInput = 0.0;
+	private double m_steerInput = 0.0;
+
+	private double m_moveOutput = 0.0;
+	private double m_steerOutput = 0.0;
+
+	private double m_moveTrim = 0.0;
+	private double m_steerTrim = 0.0;
+
+	private boolean isFinished;
+	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
+	
+	private MPTalonPIDController mpStraightController;
+	private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7);  // 4 colsons
+//	private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03);  // 4 omni
+	private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); 
+	
+	private MPSoftwarePIDController mpTurnController; // p    i   d     a      v      g    izone
+	private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100);  // 4 colson wheels
+//	private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100);  // 4 omni
+	
+	private SoftwarePIDController pidTurnController;
+	//private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008
+	private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008
+	private double targetPIDAngle;
+
+	private MPTalonPIDPathController mpPathController;
+	private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100);  // 4 omni   g=.3
+
+	//PID target
+	private double targetPIDPosition;
+
+	private MPTalonPIDPathVelocityController mpPathVelocityController;
+	private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); 
+
+	private AdaptivePurePursuitController adaptivePursuitController;
+	private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); 
+	
+	private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0));
+	private RigidTransform2d currentPose = zeroPose;
+	private RigidTransform2d lastPose = zeroPose;
+    private double left_encoder_prev_distance_ = 0;
+    private double right_encoder_prev_distance_ = 0;
+
+	//private PigeonImu gyroPigeon;
+	//private double[] yprPigeon = new double[3];
+    private AHRS gyroNavX;
+	private boolean useGyroLock;
+	private double gyroLockAngleDeg;
+	private double kPGyro = 0.07;
+	//private double kPGyro = 0.0625;
+	
+	private boolean isCalibrating = false;
+	private double gyroOffsetDeg = 0;
+
+	public Drive() {
+		try {
+			//System.err.println("Beginning of Drive.");
+
+			leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless);
+			//leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
+			leftDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless);
+			encoderLeft = new CANEncoder(leftDrive1);
+			leftDrive2.follow(leftDrive1);
+
+			rightDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless);
+			rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
+			encoderRight = new CANEncoder(rightDrive1);
+			rightDrive2.follow(rightDrive1);
+			
+			//System.err.println("After Constructors.");
+
+			//gyroPigeon = new PigeonImu(leftDrive2);
+			gyroNavX = new AHRS(SPI.Port.kMXP);
+			
+			//hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID);
+			//hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID);
+	
+			//leftDrive1.clearStickyFaults();
+			//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
+			//leftDrive1.setNominalClosedLoopVoltage(12.0);
+			leftDrive1.clearFaults();
+			leftDrive1.setInverted(false);//false on comp 2108, false on microbot
+			//leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark
+			//leftDrive1.setSafetyEnabled(false); //not needed for spark
+			//leftDrive1.setCurrentLimit(15);
+			//leftDrive1.enableCurrentLimit(true);
+			leftDrive1.setIdleMode(IdleMode.kBrake);
+			//leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark?
+			//leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark?
+			//leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark?
+			//leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark?
+
+			
+//	        if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
+//	            Driver.reportError("Could not detect left drive encoder encoder!", false);
+//	        }
+			
+
+			leftDrive2.setInverted(false);//false on comp 2108, false on microbot
+			//leftDrive2.setSafetyEnabled(false);
+			leftDrive2.setIdleMode(IdleMode.kBrake);
+			//leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); // set above
+	
+
+			
+			//rightDrive1.clearStickyFaults();
+			//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
+			//rightDrive1.setNominalClosedLoopVoltage(12.0);
+			rightDrive1.clearFaults();
+			rightDrive1.setInverted(false);//true on comp 2108, false on microbot
+			//rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
+			//rightDrive1.setSafetyEnabled(false);
+			rightDrive1.setIdleMode(IdleMode.kBrake);
+			//rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
+			//rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
+			//rightDrive1.configNominalOutputForward(+1.0f, 0);
+			//rightDrive1.configNominalOutputReverse(-1.0f, 0);
+//	        if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
+//	            DriverStation.reportError("Could not detect right drive encoder encoder!", false);
+//	        }
+			
+
+			rightDrive2.setInverted(false);//True on comp 2108, false on microbot
+			//rightDrive2.setSafetyEnabled(false);
+			rightDrive2.setIdleMode(IdleMode.kBrake);
+			//rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
+
+			//System.err.println("After motor settings.");
+			
+			CANPIDController leftDrive1_Controller = new CANPIDController(leftDrive1);
+			CANPIDController rightDrive1_Controller = new CANPIDController(rightDrive1);
+							
+			motorControllers.add(leftDrive1_Controller);
+			motorControllers.add(rightDrive1_Controller);
+
+			//System.err.println("After motorControllers.");
+
+			//m_drive = new RobotDrive(leftDrive1, rightDrive1);
+			//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
+			//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
+			//m_drive.setSafetyEnabled(false);
+			m_drive = new DifferentialDrive(leftDrive1, rightDrive1);
+			//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true);	//TODO URGENT: verify
+			//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true);	//TODO URGENT: verify
+			m_drive.setSafetyEnabled(false);
+			
+		
+			//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
+			//System.err.println("End of Drive.");
+		}
+
+		catch (Exception e) {
+			System.err.println("An error occurred in the DriveTrain constructor");
+		}
+	}
+
+	public void setToBrakeOnNeutral(boolean brakeVsCoast) {
+		if (brakeVsCoast) {
+			leftDrive1.setIdleMode(IdleMode.kBrake);
+			leftDrive2.setIdleMode(IdleMode.kBrake);
+			rightDrive1.setIdleMode(IdleMode.kBrake);
+			rightDrive2.setIdleMode(IdleMode.kBrake);
+		} else {
+			leftDrive1.setIdleMode(IdleMode.kCoast);
+			leftDrive2.setIdleMode(IdleMode.kCoast);
+			rightDrive1.setIdleMode(IdleMode.kCoast);
+			rightDrive2.setIdleMode(IdleMode.kCoast);
+		}
+	}
+	
+	@Override
+	public void initDefaultCommand() {
+	}
+	
+	public double getGyroAngleDeg() {
+		//return getGyroPigeonAngleDeg();
+		return getGyroNavXAngleDeg();
+	}
+	
+	//public double getGyroPigeonAngleDeg() {
+	//	gyroPigeon.GetYawPitchRoll(yprPigeon);
+	//	return -yprPigeon[0] + gyroOffsetDeg;
+	//}
+	
+	public double getGyroNavXAngleDeg() {
+		return gyroNavX.getAngle() + gyroOffsetDeg;
+	}
+
+	public void resetGyro() {
+		//gyroPigeon.SetYaw(0);
+		gyroNavX.zeroYaw();
+	}
+	
+	public void resetEncoders() {
+		mpStraightController.resetZeroPosition();
+	}
+	
+	public void calibrateGyro() {
+		//gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature);
+	}
+	
+	public void endGyroCalibration() {
+		if (isCalibrating == true) {
+			isCalibrating = false;
+		}
+	}
+	
+	public void setGyroOffset(double offsetDeg) {
+		gyroOffsetDeg = offsetDeg;
+	}
+	
+	//public boolean isHopperSensorRedOn() {
+	//	return hopperSensorRed.get();
+	//}
+
+	//public boolean isHopperSensorBlueOn() {
+	//	return hopperSensorBlue.get();
+	//}
+	
+	//public boolean isHopperSensorOn() {
+	//	if (isRed() == true) {
+	//		return isHopperSensorRedOn();
+	//	}		
+	//	else {
+	//		return isHopperSensorBlueOn();
+	//	}
+	//}
+	
+
+
+
+
+
+
+
+
+
+
+	/*
+	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);
+	//	mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); 
+	//	setControlMode(DriveControlMode.MP_STRAIGHT);
+	//}
+	
+	public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
+		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
+		setControlMode(DriveControlMode.MP_TURN);
+	}
+	
+	//public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) {
+	//	mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
+	//	setControlMode(DriveControlMode.MP_TURN);
+	//}
+	
+	public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
+		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES);
+		setControlMode(DriveControlMode.MP_TURN);
+	}
+	
+	public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
+		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
+		setControlMode(DriveControlMode.MP_TURN);
+	}
+	
+	//public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) {
+	//	mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
+	//	setControlMode(DriveControlMode.MP_TURN);
+	//}
+	
+	public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
+		this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg();
+		pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
+		setControlMode(DriveControlMode.PID_TURN);
+	}
+/*
+	public void setPathMP(PathGenerator path) {
+		mpPathController.setPID(mpPathPIDParams);
+		mpPathController.setMPPathTarget(path); 
+		setControlMode(DriveControlMode.MP_PATH);
+	}
+	
+	public void setPathVelocityMP(PathGenerator path) {
+		mpPathVelocityController.setPID(mpPathPIDParams);
+		mpPathVelocityController.setMPPathTarget(path); 
+		setControlMode(DriveControlMode.MP_PATH_VELOCITY);
+	}
+	
+    public void setPathAdaptivePursuit(Path path, boolean reversed) {
+    	currentPose = zeroPose;
+    	lastPose = zeroPose;
+        left_encoder_prev_distance_ = 0;
+        right_encoder_prev_distance_ = 0;
+        adaptivePursuitController.setPID(adaptivePursuitPIDParams);
+    	adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); 
+		setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
+    }
+*/
+    public void setDriveHold(boolean status) {
+		if (status) {
+			setControlMode(DriveControlMode.HOLD);
+		}
+		else {
+			setControlMode(DriveControlMode.JOYSTICK);
+		}
+	}
+    
+    public void updatePose() {
+        double left_distance = encoderLeft.getPosition();
+        double right_distance = encoderRight.getPosition();
+        Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
+        lastPose = currentPose;
+        currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
+        left_encoder_prev_distance_ = left_distance;
+        right_encoder_prev_distance_ = right_distance;
+    }
+    
+    public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
+        return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle);
+    }
+	
+    /**
+     * Path Markers are an optional functionality that name the various
+     * Waypoints in a Path with a String. This can make defining set locations
+     * much easier.
+     * 
+     * @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;
+		if (controlMode == DriveControlMode.JOYSTICK) {
+			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			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.MANUAL) {
+			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			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.CLIMB) {
+			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
+			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);
+			//leftDrive1.setPosition(0);
+			//leftDrive1.set(0);
+			//rightDrive1.changeControlMode(TalonControlMode.Position);
+			//rightDrive1.setPosition(0);
+			//rightDrive1.set(0);
+			//leftDrive1.setSelectedSensorPosition(0, 0, 0);	//not needed for spark?  TODO: verify want 0="Primary closed-loop", with no timeout
+			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;
+	}
+	
+	public synchronized void controlLoopUpdate() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (lastControlLoopUpdateTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
+		}
+		lastControlLoopUpdateTimestamp = currentTimestamp;
+		
+		// Do the update
+		if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) {
+			driveWithJoystick();
+		}
+		else if (!isFinished) {
+			if (controlMode == DriveControlMode.MP_STRAIGHT) {
+				isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == DriveControlMode.MP_TURN) {
+				isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == DriveControlMode.PID_TURN) {
+				isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == DriveControlMode.MP_PATH) {
+				isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			/*else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
+				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
+				updatePose();
+				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
+			}*/
+		}
+	}
+	
+	public void setSpeed(double speed) {
+		if (speed == 0) {
+			setControlMode(DriveControlMode.JOYSTICK);
+		}
+		else {
+			setControlMode(DriveControlMode.MANUAL);
+			rightDrive1.set(-speed);
+			leftDrive1.set(speed);
+		}
+	}
+	
+	public void setClimbSpeed(double climbSpeed) {
+		this.climbSpeed = climbSpeed;
+		if (climbSpeed == 0) {
+			setControlMode(DriveControlMode.JOYSTICK);
+		}
+		else {
+			setControlMode(DriveControlMode.CLIMB);
+		}
+	}
+	
+	public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) {
+		if (snapToAbsolute0or180) {
+			gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg());
+		}
+		else {
+			gyroLockAngleDeg = getGyroAngleDeg();
+		}
+		this.useGyroLock = useGyroLock;
+	}
+
+	public void driveWithJoystick() {
+		if(m_drive == null) return;
+		// switch(m_controllerMode) {
+		// case CONTROLLER_JOYSTICK_ARCADE:
+		// m_moveInput = OI.getInstance().getJoystick1().getY();
+		// m_steerInput = OI.getInstance().getJoystick1().getX();
+		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
+		// break;
+		// case CONTROLLER_JOYSTICK_TANK:
+		// m_moveInput = OI.getInstance().getJoystick1().getY();
+		// m_steerInput = OI.getInstance().getJoystick2().getY();
+		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_drive.tankDrive(m_moveOutput, m_steerOutput);
+		// break;
+		// case CONTROLLER_JOYSTICK_CHEESY:
+		// m_moveInput = OI.getInstance().getJoystick1().getY();
+		// m_steerInput = OI.getInstance().getJoystick2().getX();
+		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
+		// break;
+		// case CONTROLLER_XBOX_CHEESY:
+		// boolean turbo = OI.getInstance().getDriveTrainController()
+		// .getLeftJoystickButton();
+		// boolean slow = OI.getInstance().getDriveTrainController()
+		// .getRightJoystickButton();
+		// double speedToUseMove, speedToUseSteer;
+		// if (turbo && !slow) {
+		// speedToUseMove = m_moveScaleTurbo;
+		// speedToUseSteer = m_steerScaleTurbo;
+		// } else if (!turbo && slow) {
+		// speedToUseMove = m_moveScaleSlow;
+		// speedToUseSteer = m_steerScaleSlow;
+		// } else {
+		// speedToUseMove = m_moveScale;
+		// speedToUseSteer = m_steerScale;
+		// }
+
+		// m_moveInput =
+		// OI.getInstance().getDriveTrainController().getLeftYAxis();
+		// m_steerInput =
+		// OI.getInstance().getDriveTrainController().getRightXAxis();
+		m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis();
+		m_steerInput = OI.getInstance().getDriverController().getRightXAxis();
+
+		if (controlMode == DriveControlMode.JOYSTICK) {
+			m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+					m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		}
+		else if (controlMode == DriveControlMode.CLIMB) {
+			m_moveOutput = climbSpeed;
+		}
+		
+		if (useGyroLock) {											// If currently in gyro lock mode,
+			if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) {	// but stopped driving or started turning,
+				setGyroLock(false, false);							// turn off gyro lock mode
+			}
+		} else {													// If not yet in gyro lock mode,
+			if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) {	// but just started driving without turning,
+				setGyroLock(true, false);							// gyro lock to current heading
+			}
+		}
+		
+		if (useGyroLock) {
+			double yawError = gyroLockAngleDeg - getGyroAngleDeg();
+			m_steerOutput = kPGyro * yawError;
+		} else {
+			m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+					m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		}
+
+		m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
+		// break;
+		// case CONTROLLER_XBOX_ARCADE_RIGHT:
+		// m_moveInput =
+		// OI.getInstance().getDrivetrainController().getRightYAxis();
+		// m_steerInput =
+		// OI.getInstance().getDrivetrainController().getRightXAxis();
+		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
+		// break;
+		// case CONTROLLER_XBOX_ARCADE_LEFT:
+		// m_moveInput =
+		// OI.getInstance().getDrivetrainController().getLeftYAxis();
+		// m_steerInput =
+		// OI.getInstance().getDrivetrainController().getLeftXAxis();
+		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
+		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
+		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
+		// break;
+		// }
+	}
+
+	public void rawMoveSteer(double move, double steer) {
+		m_drive.arcadeDrive(move, steer, false);
+	}
+
+	public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
+		
+			
+				leftDrive1.set(leftPercentOutput);
+				rightDrive1.set(rightPercentOutput);
+			
+	}
+
+	private boolean inDeadZone(double input) {
+		boolean inDeadZone;
+		if (Math.abs(input) < STICK_DEADBAND) {
+			inDeadZone = true;
+		} else {
+			inDeadZone = false;
+		}
+		return inDeadZone;
+	}
+
+	public double adjustForSensitivity(double scale, double trim,
+			double steer, int nonLinearFactor, double wheelNonLinearity) {
+		if (inDeadZone(steer))
+			return 0;
+
+		steer += trim;
+		steer *= scale;
+		steer = limitValue(steer);
+
+		int iterations = Math.abs(nonLinearFactor);
+		for (int i = 0; i < iterations; i++) {
+			if (nonLinearFactor > 0) {
+				steer = nonlinearStickCalcPositive(steer, wheelNonLinearity);
+			} else {
+				steer = nonlinearStickCalcNegative(steer, wheelNonLinearity);
+			}
+		}
+		return steer;
+	}
+
+	private double limitValue(double value) {
+		if (value > 1.0) {
+			value = 1.0;
+		} else if (value < -1.0) {
+			value = -1.0;
+		}
+		return value;
+	}
+
+	private double nonlinearStickCalcPositive(double steer,
+			double steerNonLinearity) {
+		return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer)
+				/ Math.sin(Math.PI / 2.0 * steerNonLinearity);
+	}
+
+	private double nonlinearStickCalcNegative(double steer,
+			double steerNonLinearity) {
+		return Math.asin(steerNonLinearity * steer)
+				/ Math.asin(steerNonLinearity);
+	}
+
+	//public void setShiftState(SpeedShiftState state) {
+	//	if(state == SpeedShiftState.HI) {
+	//		speedShift.set(true);
+	//	}
+	//	else if(state == SpeedShiftState.LO) {
+	//		speedShift.set(false);
+	//	}
+	//}
+
+	public synchronized boolean isFinished() {
+		return isFinished;
+	}
+	
+	public synchronized void setFinished(boolean isFinished) {
+		this.isFinished = isFinished;
+	}
+	
+	@Override
+	public void setPeriodMs(long periodMs) {
+		/*                                       FIX TODAY
+		mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
+		mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
+		pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
+		mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers);
+		mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers);
+		adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers);
+		*/
+		this.periodMs = periodMs;
+	}
+	
+	public double getPeriodMs() {
+		return periodMs;
+	}
+	
+	public boolean isRed() {
+		return isRed;
+	}
+	
+	public void setIsRed(boolean status) {
+		isRed = status;
+	}
+	
+	public static double rotationsToInches(double rotations) {
+        return rotations * (Constants.kDriveWheelDiameterInches * Math.PI);
+    }
+
+    public static double rpmToInchesPerSecond(double rpm) {
+        return rotationsToInches(rpm) / 60;
+    }
+
+    public static double inchesToRotations(double inches) {
+        return inches / (Constants.kDriveWheelDiameterInches * Math.PI);
+    }
+
+    public static double inchesPerSecondToRpm(double inches_per_second) {
+        return inchesToRotations(inches_per_second) * 60;
+    }
+
+    public double getLeftPositionWorld() {
+    	return 0;//leftDrive1.getPositionWorld(); FIX TODAY
+    }
+
+    public double getRightPositionWorld() {
+    	return 0;//rightDrive1.getPositionWorld(); FIX TODAY
+    }
+
+	public void updateStatus(Robot.OperationMode operationMode) {
+		if (operationMode == Robot.OperationMode.TEST) {
+			try {
+				SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg());
+				SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
+				SmartDashboard.putNumber("Right Pos Ticks", 0);//rightDrive1.getSelectedSensorPosition(0));   FIX TODAY
+				SmartDashboard.putNumber("Left Pos Ticks", 0);//leftDrive1.getSelectedSensorPosition(0));
+				SmartDashboard.putNumber("Right Pos Inches", -encoderRight.getPosition());//rightDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12);
+				SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12);
+				//SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
+				//SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
+//				SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));
+				//SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID));
+				//SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID));
+//				SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID));
+				//SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn());
+				//SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn());
+				SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
+				//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
+				SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360));
+				MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); 
+				double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
+				SmartDashboard.putNumber("Gyro Delta", delta);
+				//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
+				SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
+				SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
+				SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg());
+				SmartDashboard.putNumber("Steer Output", m_steerOutput);
+				SmartDashboard.putNumber("Move Output", m_moveOutput);
+				SmartDashboard.putNumber("Steer Input", m_steerInput);
+				SmartDashboard.putNumber("Move Input", m_moveInput);
+				SmartDashboard.putString("MODE", "TEST");
+				if (encoderLeft.getPosition() != 0 && encoderRight.getPosition() != 0){
+					SmartDashboard.putNumber("Distance Inches", (encoderLeft.getPosition()-encoderRight.getPosition())/2);//rightDrive1.getPositionWorld());
+				}
+			}
+			catch (Exception e) {
+				System.err.println("Drivetrain update status error");
+			}
+		}
+		else if (operationMode == Robot.OperationMode.COMPETITION) {
+			SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360));
+			SmartDashboard.putString("MODE", "SICKO");
+			if (encoderLeft.getPosition() != 0 && encoderRight.getPosition() != 0){
+				SmartDashboard.putNumber("Distance Inches", (encoderLeft.getPosition()-encoderRight.getPosition())/2);//rightDrive1.getPositionWorld());
+			}
+			SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
+		}
+	}	
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java

@@ -0,0 +1,66 @@
+package org.usfirst.frc4388.robot.subsystems;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class Pnumatics extends Subsystem {
+    
+		
+	private DoubleSolenoid speedShift;
+	private DoubleSolenoid hatchIntake;
+	private DoubleSolenoid ballIntake;
+	private DoubleSolenoid wrist;
+
+	
+	public Pnumatics() {
+		try {
+			speedShift = new DoubleSolenoid(1,0,1);					
+			hatchIntake = new DoubleSolenoid(1,2,3); 
+			ballIntake = new DoubleSolenoid(1,4,5);
+			wrist = new DoubleSolenoid(1,6,7);
+		} 
+		catch (Exception e) {
+			System.err.println("An error occurred in the Pnumatics constructor");
+		}
+	}
+	
+	public void setShiftState(boolean state) {
+		if (state==true) {
+			speedShift.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			speedShift.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+	public void setHatchIntakeState(boolean state) {
+		if (state==true) {
+			hatchIntake.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			hatchIntake.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+	public void setBallIntake(boolean state) {
+		if (state==true) {
+			ballIntake.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			ballIntake.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+	public void setWrist(boolean state) {
+		if (state==true) {
+			wrist.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			wrist.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+
+	public void initDefaultCommand() {
+    }
+}
+

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java

@@ -0,0 +1,356 @@
+package org.usfirst.frc4388.robot.subsystems;
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.Loop;
+import org.usfirst.frc4388.utility.MPTalonPIDController;
+import org.usfirst.frc4388.utility.PIDParams;
+import org.usfirst.frc4388.utility.TalonSRXEncoder;
+import org.usfirst.frc4388.utility.TalonSRXFactory;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.SensorCollection;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Wrist extends Subsystem implements ControlLoopable
+{
+	private static Wrist instance;
+
+	public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
+
+	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks 
+	public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60;  
+	
+
+	private double periodMs;
+	private double lastControlLoopUpdatePeriod = 0.0;
+	private double lastControlLoopUpdateTimestamp = 0.0;
+	// Defined speeds
+	public static final double CLIMB_SPEED = -1.0;
+	public static final double TEST_SPEED_UP = 0.3;
+	public static final double TEST_SPEED_DOWN = -0.3;
+	public static final double AUTO_ZERO_SPEED = -0.3;
+	public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
+	public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
+	
+	// Defined positions
+	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
+	public static final double ZERO_POSITION_INCHES = -0.25;
+	public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
+	public static final double MIN_POSITION_INCHES = 0.0;
+	public static final double MAX_POSITION_INCHES = 83.4;
+	public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
+
+	public static final double SWITCH_POSITION_INCHES = 24.0;
+	public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
+	public static final double SCALE_LOW_POSITION_INCHES = 56.0;
+	public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
+	public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
+	public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
+	public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
+	public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
+	public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
+
+	// Motion profile max velocities and accel times
+	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60; 
+	public static final double MP_T1 = 400;  // Fast = 300
+	public static final double MP_T2 = 150;  // Fast = 150
+	
+	// Motor controllers
+	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
+
+	private TalonSRXEncoder wristmotor1;
+	
+	// PID controller and params
+	private MPTalonPIDController mpController;
+
+	public static int PID_SLOT = 0;
+	public static int MP_SLOT = 1;
+
+	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
+	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);  
+	public static final double KF_UP = 0.005;
+	public static final double KF_DOWN = 0.0;
+	public static final double PID_ERROR_INCHES = 1.0;
+	LimitSwitchSource limitSwitchSource;
+	// Pneumatics
+	private Solenoid speedShift;
+
+	private WristControlMode controlMode = WristControlMode.JOYSTICK_MANUAL;
+	// Misc
+	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;	
+	private boolean isFinished;
+	private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
+	private double targetPositionInchesPID = 0;
+	private boolean firstMpPoint;
+	private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
+	
+	public Wrist() {
+		try {
+			wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
+			System.err.println("the tallon shold be made in wrist");
+			
+			wristmotor1.setInverted(true);
+										
+//	        if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
+//	            Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
+//	        }
+			wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			wristmotor1.setNeutralMode(NeutralMode.Brake);
+			
+			motorControllers.add(wristmotor1);
+			
+			
+		}
+		catch (Exception e) {
+			System.err.println("An error occurred in the Wrist constructor");
+		}
+	}
+
+	@Override
+	public void initDefaultCommand() {
+	}
+		
+	public void resetZeroPosition(double position) {
+		mpController.resetZeroPosition(position);
+	}	
+	
+	private synchronized void setWristControlMode(WristControlMode controlMode) {
+		this.wristControlMode = controlMode;
+	}
+	
+	private synchronized WristControlMode getWristControlMode() {
+		return this.wristControlMode;
+	}
+
+	public void setSpeed(double speed) {
+		wristmotor1.set(ControlMode.PercentOutput, speed);
+		setWristControlMode(WristControlMode.MANUAL);
+	}
+		
+	public void setSpeedJoystick(double speed) {
+		wristmotor1.set(ControlMode.PercentOutput, speed);
+		setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
+	}
+		
+	public void setPositionPID(double targetPositionInches) {
+		mpController.setPIDSlot(PID_SLOT);
+		updatePositionPID(targetPositionInches);
+		setWristControlMode(WristControlMode.JOYSTICK_PID);	
+		setFinished(false);
+	}
+	
+	public void updatePositionPID(double targetPositionInches) {
+ 		targetPositionInchesPID = limitPosition(targetPositionInches);
+		double startPositionInches = wristmotor1.getPositionWorld();
+		mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
+	}
+	
+	public void setPositionMP(double targetPositionInches) {
+		double startPositionInches = wristmotor1.getPositionWorld();
+		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
+		setFinished(false);
+		firstMpPoint = true;
+		setWristControlMode(WristControlMode.MOTION_PROFILE);
+ 	}
+	
+	private double limitPosition(double targetPosition) {
+		if (targetPosition < MIN_POSITION_INCHES) {
+			return MIN_POSITION_INCHES;
+		}
+		else if (targetPosition > MAX_POSITION_INCHES) {
+			return MAX_POSITION_INCHES;
+		}
+		
+		return targetPosition;
+	}
+	@Override
+	public void setPeriodMs(long periodMs) {
+		mpController = new MPTalonPIDController(periodMs, motorControllers);
+		mpController.setPID(mpPIDParams, MP_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		this.periodMs = periodMs;
+	}
+	/*@Override
+	public void onStart(double timestamp) {
+		mpController = new MPTalonPIDController(periodMs, motorControllers);
+		mpController.setPID(mpPIDParams, MP_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		mpController.setPIDSlot(PID_SLOT);
+	}
+
+	@Override
+	public void onStop(double timestamp) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void onLoop(double timestamp) {
+		synchronized (Wrist.this) {
+			switch( getElevatorControlMode() ) {
+				case JOYSTICK_PID: 
+					controlPidWithJoystick();
+					break;
+				case JOYSTICK_MANUAL:
+					controlManualWithJoystick();
+					break;
+				case MOTION_PROFILE: 
+					if (!isFinished()) {
+						if (firstMpPoint) {
+							mpController.setPIDSlot(MP_SLOT);
+							firstMpPoint = false;
+						}
+						setFinished(mpController.controlLoopUpdate()); 
+						if (isFinished()) {
+							mpController.setPIDSlot(PID_SLOT);
+						}
+					}
+					break;
+				default:
+					break;
+			}
+		}
+	}
+
+*/
+
+
+
+
+
+	public synchronized void controlLoopUpdate() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (lastControlLoopUpdateTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
+		}
+		lastControlLoopUpdateTimestamp = currentTimestamp;
+		
+		// Do the update
+		if (controlMode == WristControlMode.JOYSTICK_MANUAL) {
+			controlManualWithJoystick();
+		}
+		else if (!isFinished) {
+			if (controlMode == WristControlMode.MOTION_PROFILE) {
+				isFinished = mpController.controlLoopUpdate(getPositionInches()); 
+			}
+			
+			/*else if (controlMode == WristControlMode.MP_PATH_VELOCITY) {
+				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
+			}
+			else if (controlMode == WristControlMode.ADAPTIVE_PURSUIT) {
+				updatePose();
+				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
+			}*/
+		}
+	}
+
+
+
+
+
+
+	
+	private void controlPidWithJoystick() {
+		double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
+		double deltaPosition = joystickPosition * joystickInchesPerMs;
+		targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
+		updatePositionPID(targetPositionInchesPID);
+	}
+
+	private void ControlWithJoystickhold(){
+		double holdPosition = wristmotor1.getPositionWorld();
+		updatePositionPID(holdPosition);
+
+	}
+	
+	private void controlManualWithJoystick() {
+		double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
+		setSpeedJoystick(joyStickSpeed*.50);
+	}
+	/*
+	public void setShiftState(ElevatorSpeedShiftState state) {
+
+			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI;
+			speedShift.set(true);
+			mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+		}
+		else if(state == ElevatorSpeedShiftState.LO) {
+			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
+			speedShift.set(false);
+			mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
+		}
+	}
+	
+	public ElevatorSpeedShiftState getShiftState() {
+		return shiftState;
+	}
+*/
+	public double getPositionInches() {
+		return wristmotor1.getPositionWorld();
+	}
+	
+//	public double getAverageMotorCurrent() {
+//		return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
+//	}
+		
+	public double getAverageMotorCurrent() {
+		return (wristmotor1.getOutputCurrent());
+	}
+		
+	public synchronized boolean isFinished() {
+		return isFinished;
+	}
+	
+	public synchronized void setFinished(boolean isFinished) {
+		this.isFinished = isFinished;
+	}
+		
+	public double getPeriodMs() {
+		return periodMs;
+	}
+	
+	public void updateStatus(Robot.OperationMode operationMode) {
+		if (operationMode == Robot.OperationMode.TEST) {
+			try {
+				SmartDashboard.putNumber("Elevator Position Inches", wristmotor1.getPositionWorld());
+				SmartDashboard.putNumber("Elevator Motor 1 Amps", wristmotor1.getOutputCurrent());
+				SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
+//				SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
+//				SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
+//				SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
+				SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
+			}
+			catch (Exception e) {
+			}
+		}
+	}	
+	
+	public static Wrist getInstance() {
+		if(instance == null) {
+			instance = new Wrist();
+		}
+		return instance;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java

@@ -0,0 +1,216 @@
+package org.usfirst.frc4388.utility;
+
+import java.io.File;
+import java.io.FileReader;
+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;
+import java.util.List;
+import java.util.Set;
+
+import org.json.simple.JSONObject;
+import org.json.simple.parser.JSONParser;
+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
+ */
+public abstract class ConstantsBase {
+    HashMap<String, Boolean> modifiedKeys = new HashMap<String, Boolean>();
+
+    public abstract String getFileLocation();
+
+    public static class Constant {
+        public String name;
+        public Class<?> type;
+        public Object value;
+
+        public Constant(String name, Class<?> type, Object value) {
+            this.name = name;
+            this.type = type;
+            this.value = value;
+        }
+
+        @Override
+        public boolean equals(Object o) {
+            String itsName = ((Constant) o).name;
+            Class<?> itsType = ((Constant) o).type;
+            Object itsValue = ((Constant) o).value;
+            return o instanceof Constant && this.name.equals(itsName) && this.type.equals(itsType)
+                    && this.value.equals(itsValue);
+        }
+    }
+
+    public File getFile() {
+        String filePath = getFileLocation();
+        filePath = filePath.replaceFirst("^~", System.getProperty("user.home"));
+        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);
+    }
+
+    public boolean setConstant(String name, Integer value) {
+        return setConstantRaw(name, value);
+    }
+
+    public boolean setConstant(String name, String value) {
+        return setConstantRaw(name, value);
+    }
+
+    private boolean setConstantRaw(String name, Object value) {
+        boolean success = false;
+        Field[] declaredFields = this.getClass().getDeclaredFields();
+        for (Field field : declaredFields) {
+            if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) {
+                try {
+                    Object current = field.get(this);
+                    field.set(this, value);
+                    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);
+                }
+            }
+        }
+        return success;
+    }
+
+    public Object getValueForConstant(String name) throws Exception {
+        Field[] declaredFields = this.getClass().getDeclaredFields();
+        for (Field field : declaredFields) {
+            if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) {
+                try {
+                    return field.get(this);
+                } catch (IllegalArgumentException | IllegalAccessException e) {
+                    throw new Exception("Constant not found");
+                }
+            }
+        }
+        throw new Exception("Constant not found");
+    }
+
+    public Constant getConstant(String name) {
+        Field[] declaredFields = this.getClass().getDeclaredFields();
+        for (Field field : declaredFields) {
+            if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) {
+                try {
+                    return new Constant(field.getName(), field.getType(), field.get(this));
+                } catch (IllegalArgumentException | IllegalAccessException e) {
+                    e.printStackTrace();
+                }
+            }
+        }
+        return new Constant("", Object.class, 0);
+    }
+
+    public Collection<Constant> getConstants() {
+        List<Constant> constants = (List<Constant>) getAllConstants();
+        int stop = constants.size();
+        for (int i = 0; i < constants.size(); ++i) {
+            Constant c = constants.get(i);
+            if ("kEndEditableArea".equals(c.name)) {
+                stop = i;
+            }
+        }
+        return constants.subList(0, stop);
+    }
+
+    private Collection<Constant> getAllConstants() {
+        Field[] declaredFields = this.getClass().getDeclaredFields();
+        List<Constant> constants = new ArrayList<Constant>(declaredFields.length);
+        for (Field field : declaredFields) {
+            if (java.lang.reflect.Modifier.isStatic(field.getModifiers())) {
+                Constant c;
+                try {
+                    c = new Constant(field.getName(), field.getType(), field.get(this));
+                    constants.add(c);
+                } catch (IllegalArgumentException | IllegalAccessException e) {
+                    e.printStackTrace();
+                }
+            }
+        }
+        return constants;
+    }
+
+    public JSONObject getJSONObjectFromFile() throws IOException, ParseException {
+        File file = getFile();
+        if (file == null || !file.exists()) {
+            return new JSONObject();
+        }
+        FileReader reader;
+        reader = new FileReader(file);
+        JSONParser jsonParser = new JSONParser();
+        return (JSONObject) jsonParser.parse(reader);
+    }
+
+    public void loadFromFile() {
+        try {
+            JSONObject jsonObject = getJSONObjectFromFile();
+            Set<?> keys = jsonObject.keySet();
+            for (Object o : keys) {
+                String key = (String) o;
+                Object value = jsonObject.get(o);
+                if (value instanceof Long && getConstant(key).type.equals(int.class)) {
+                    value = new BigDecimal((Long) value).intValueExact();
+                }
+                setConstantRaw(key, value);
+            }
+        } catch (IOException e) {
+            e.printStackTrace();
+        } catch (ParseException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public void saveToFile() {
+        File file = getFile();
+        if (file == null) {
+            return;
+        }
+        try {
+            JSONObject json = getJSONObjectFromFile();
+            FileWriter writer = new FileWriter(file);
+            for (String key : modifiedKeys.keySet()) {
+                try {
+                    Object value = getValueForConstant(key);
+                    json.put(key, value);
+                } catch (Exception e) {
+                    e.printStackTrace();
+                }
+            }
+            writer.write(json.toJSONString());
+            writer.close();
+        } catch (IOException | ParseException e) {
+            e.printStackTrace();
+        }
+    }
+
+}

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();
+        }
+    }
+}

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();
+}

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);
+}

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_);
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java

@@ -0,0 +1,116 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.subsystems.Drive;
+
+//import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+public class MMTalonPIDController
+{	
+	protected static enum MMControlMode { STRAIGHT, TURN };
+	public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
+
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected boolean useGyroLock;
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	protected MMControlMode controlMode;
+	protected MMTalonTurnType turnType;
+	protected double targetValue;
+	
+	public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF);
+		}
+	}
+	
+	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 (TalonSRXEncoder motorController : motorControllers) {
+			if (resetEncoder) {
+				motorController.setPosition(0);
+			}
+			motorController.configMotionCruiseVelocity((int)maxVelocity, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.configMotionAcceleration((int)maxAcceleration, TalonSRXEncoder.TIMEOUT_MS);
+			motorController.set(ControlMode.MotionMagic, targetValue);
+		}
+	}
+			
+	public void setZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.MotionMagic, targetValue);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
+		}
+	}
+
+	private double calcTrackDistance(double deltaAngleDeg, MMTalonTurnType turnType, double trackWidth) {
+		double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth;
+		if (turnType == MMTalonTurnType.TANK) {
+			return trackDistance;
+		}
+		else if (turnType == MMTalonTurnType.LEFT_SIDE_ONLY) {
+			return trackDistance * 2.0;
+		}
+		else if (turnType == MMTalonTurnType.RIGHT_SIDE_ONLY) {
+			return -trackDistance * 2.0;
+		}
+		return 0.0;
+	}
+	
+	public boolean controlLoopUpdate(double currentGyroAngle) {		
+		// Calculate the motion profile feed forward and gyro feedback terms
+		double rightTarget = 0.0;
+		double leftTarget = 0.0;
+
+		// Update the set points 
+		if (controlMode == MMControlMode.STRAIGHT) {
+			double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0;
+			double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES);
+			rightTarget = targetValue + deltaDistance;
+			leftTarget = targetValue - deltaDistance;
+			
+			// Update the controllers with updated set points.
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.MotionMagic, rightTarget);
+				}
+				else {
+					motorController.set(ControlMode.MotionMagic, leftTarget);
+				}
+			}
+		}
+		
+		return false;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java

@@ -0,0 +1,144 @@
+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<TalonSRXEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected MotionProfileBoxCar mp;
+	protected MotionProfilePoint mpPoint;
+	protected boolean useGyroLock;
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected MPSoftwareTurnType turnType;
+	
+	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<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+	}
+	
+	public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPSoftwareTurnType turnType, double trackWidth) {
+		this.turnType = turnType;
+		this.startGyroAngle = startAngleDeg;
+		this.targetGyroAngle = targetAngleDeg;
+		this.useGyroLock = true;
+		
+		// Set up the motion profile 
+		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();
+				
+		prevError = 0.0;
+		totalError = 0.0;
+	}
+	
+	public boolean controlLoopUpdate() {
+		return controlLoopUpdate(0);
+	}
+	
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		mpPoint = mp.getNextPoint(mpPoint);
+		
+		// Check if we are finished
+		if (mpPoint == null) {
+			return true;
+		}
+		
+		// Calculate the motion profile feed forward and error feedback terms
+		double error = mpPoint.position - currentGyroAngle;
+		
+		if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) {
+			totalError += error;
+		}
+		else {
+			totalError = 0;
+		}
+		
+		double output =  pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * (error - prevError) + pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity;
+		prevError = error;
+			
+		// Update the controllers set point.
+		if (turnType == MPSoftwareTurnType.TANK) {
+			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 (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 0);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
+				}
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 0);					
+				}
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.LEFT_ARC) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 1.0 * output);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
+				}
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.RIGHT_ARC) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 1.0 * output);					
+				}
+			}
+		}
+
+		return false;
+	}
+	
+	public MotionProfilePoint getCurrentPoint() {
+		return mpPoint;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java

@@ -0,0 +1,233 @@
+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<TalonSRXEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected MotionProfileBoxCar mp;
+	protected MotionProfilePoint mpPoint;
+	protected boolean useGyroLock;
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	protected MPControlMode controlMode;
+	protected MPTalonTurnType turnType;
+	protected int pidSlot;
+	
+	public MPTalonPIDController(long periodMs, ArrayList<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+	}
+    
+	public void setPID(PIDParams pidParams, int slot) {
+		this.pidParams = pidParams;
+		
+		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);
+		}
+	}
+	
+	public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2) {
+		setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, false);
+	}
+
+	public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean resetEncoder) {
+		setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, resetEncoder);
+	}
+
+	public void setMPStraightTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
+		controlMode = MPControlMode.STRAIGHT;
+		this.startGyroAngle = desiredAngle;
+		this.useGyroLock = useGyroLock;
+		
+		// Set up the motion profile 
+		mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2);
+		for (TalonSRXEncoder motorController : motorControllers) {
+			if (resetEncoder) {
+				motorController.setPosition(0);
+			}
+			motorController.setWorld(ControlMode.Position, mp.getStartDistance());
+		}
+	}
+	
+	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;
+		this.turnType = turnType;
+		this.startGyroAngle = startAngleDeg;
+		this.targetGyroAngle = targetAngleDeg;
+		this.useGyroLock = true;
+		
+		trackDistance = calcTrackDistance(targetAngleDeg - startAngleDeg, turnType, trackWidth);
+
+		// Set up the motion profile 
+		mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2);
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.Position, 0);
+		}
+		
+		if (Math.abs(trackDistance) < 0.0001) {
+			trackDistance = 1;
+		}
+	}
+		
+	private double calcTrackDistance(double deltaAngleDeg, MPTalonTurnType turnType, double trackWidth) {
+		double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth;
+		if (turnType == MPTalonTurnType.TANK) {
+			return trackDistance;
+		}
+		else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) {
+			return trackDistance * 2.0;
+		}
+		else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) {
+			return -trackDistance * 2.0;
+		}
+		return 0.0;
+	}
+	
+	public void setZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+		}
+	}
+
+	public void resetZeroPosition(double position) {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(position);
+		}
+	}
+
+	public boolean controlLoopUpdate() {
+		return controlLoopUpdate(0);
+	}
+	
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		mpPoint = mp.getNextPoint(mpPoint);
+		
+		// Check if we are finished
+		if (mpPoint == 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
+		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;
+			}
+			
+			// Update the controllers Kf and set point.
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+					motorController.setWorld(ControlMode.Position, mpPoint.position);
+				}
+				else {
+					motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+					motorController.setWorld(ControlMode.Position, mpPoint.position);
+				}
+			}
+		}
+		
+		else {
+			double mpAngle = startGyroAngle + ((targetGyroAngle - startGyroAngle) * mpPoint.position / trackDistance);
+			double gyroDelta = mpAngle - currentGyroAngle;
+			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;
+			}
+			
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (turnType == MPTalonTurnType.TANK) {
+					if (motorController.isRight()) {
+						motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, -mpPoint.position);
+					}
+					else {
+						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.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, mpPoint.position);
+					}
+				}
+				else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) {
+					if (motorController.isRight()) {
+						motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+						motorController.setWorld(ControlMode.Position, -mpPoint.position);
+					}
+				}
+			}
+		}
+		
+		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;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java

@@ -0,0 +1,74 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+//import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+
+public class MPTalonPIDPathController
+{	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	
+	public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPID(0, pidParams.kP, pidParams.kI, pidParams.kD);
+		}
+	}
+		
+	public void setZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
+		}
+	}
+
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		
+		// 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
+		
+		// Update the controllers Kf and set point.
+		for (TalonSRXEncoder motorController : motorControllers) {
+			if (motorController.isRight()) {
+				motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
+//				motorController.setWorld(ControlMode.Position, rightPoint.position);
+			}
+			else {
+				motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
+			}
+		}
+		
+		
+		return false;
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java

@@ -0,0 +1,71 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+//import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+
+public class MPTalonPIDPathVelocityController
+{	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+//	protected PathGenerator path;
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	
+	public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		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 setZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPosition(0);
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (TalonSRXEncoder motorController : motorControllers) {
+			motorController.setPositionWorld(angle);
+		}
+//	}
+
+		
+		// Update the controllers Kf and set point.
+		for (TalonSRXEncoder motorController : motorControllers) {
+			if (motorController.isRight()) {
+			}
+			else {
+			}
+		}
+		
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java

@@ -0,0 +1,184 @@
+package org.usfirst.frc4388.utility;
+
+public class MotionProfileBoxCar
+{
+	public static double DEFAULT_T1 = 200;	// millisecond
+	public static double DEFAULT_T2 = 100; // millisecond
+	
+	private double startDistance;  // any distance unit
+	private double targetDistance;  // any distance unit
+	private double maxVelocity;		// velocity unit consistent with targetDistance
+	
+	// Accel profile
+	//
+	//  0 T2   T1
+	//  | |    |
+	//     _____
+	//    /     \
+	//   /       \___
+	//               \      /
+	//                \____/   
+			
+	private double itp;	  			// time between points millisecond
+	private double t1 = DEFAULT_T1; // time when accel starts back to 0.  millisecond  (typically use t1 = 2 * t2)
+	private double t2 = DEFAULT_T2; // time when accel = max accel.  millisecond  
+	
+	private double t4;
+	private int numFilter1Boxes;
+	private int numFilter2Boxes;
+	private int numPoints;
+	
+	private int numITP;
+	private double filter1;
+	private double filter2;
+	private double previousPosition;
+	private double previousVelocity;
+	private double deltaFilter1;
+	private double[] filter2Window;
+	private int windowIndex;
+	private int pointIndex;
+
+	public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp) {
+		this(startDistance, targetDistance, maxVelocity, itp, DEFAULT_T1, DEFAULT_T2);
+	} 
+	
+	public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) {
+		this.startDistance = startDistance;
+		this.targetDistance = targetDistance;
+		this.maxVelocity = maxVelocity;
+		this.itp = itp;
+		this.t1 = t1;
+		this.t2 = t2;
+		
+		initializeProfile();
+	} 
+	
+	private void initializeProfile() {
+		// t4 is the time in ms it takes to get to the end point when at max velocity
+		t4 = Math.abs((targetDistance - startDistance)/maxVelocity) * 1000;
+		
+		// We need to make t4 an even multiple of itp
+		t4 = (int)(itp * Math.ceil(t4/itp));
+		
+		// In the case where t4 is less than the accel times, we need to adjust the
+		// accel times down so the filters works out.  Lots of ways to do this but
+		// to keep things simple we will make t4 slightly larger than the sum of 
+		// the accel times.
+		if (t4 < t1 + t2) {
+			double total = t1 + t2 + t4;
+			double t1t2Ratio = t1/t2;
+			double t2Adjusted = Math.floor(total / 2 / (1 + t1t2Ratio) / itp);
+			if (t2Adjusted % 2 != 0) {
+				t2Adjusted -= 1;
+			}
+			t2 = t2Adjusted * itp;
+			t1 = t2 * t1t2Ratio;
+			t4 = total - t1 - t2;
+		}
+		
+		// Adjust max velocity so that the end point works out to the correct position.
+		maxVelocity = Math.abs((targetDistance - startDistance) / t4) * 1000;
+
+		numFilter1Boxes = (int)Math.ceil(t1/itp);
+		numFilter2Boxes = (int)Math.ceil(t2/itp);
+		numPoints = (int)Math.ceil(t4/itp);
+
+		numITP = numPoints + numFilter1Boxes + numFilter2Boxes;
+		filter1 = 0;
+		filter2 = 0;
+		previousVelocity = 0;
+		previousPosition = startDistance;
+		deltaFilter1 = 1.0/numFilter1Boxes;
+		filter2Window = new double[numFilter2Boxes];
+		windowIndex = 0;
+		pointIndex = 0;
+		if (startDistance > targetDistance && maxVelocity > 0) {
+			maxVelocity = -maxVelocity;
+		}
+	}
+	
+	public MotionProfilePoint getNextPoint(MotionProfilePoint point) {
+		if (point == null) {
+			point = new MotionProfilePoint();
+		}
+		
+		if (pointIndex == 0) {
+			point.initialize(startDistance);
+			pointIndex++;
+			return point;
+		}
+		else if (pointIndex > numITP) {
+			return null;
+		}
+		
+		int input = (pointIndex - 1) < numPoints ? 1 : 0;		
+		if (input > 0) {
+			filter1 = Math.min(1, filter1 + deltaFilter1);
+		}
+		else {
+			filter1 = Math.max(0, filter1 - deltaFilter1);				
+		}
+		
+		double firstFilter1InWindow = filter2Window[windowIndex];
+		if (pointIndex <= numFilter2Boxes) {
+			firstFilter1InWindow = 0;
+		}
+		filter2Window[windowIndex] = filter1;
+		
+		filter2 += (filter1 - firstFilter1InWindow) / numFilter2Boxes;
+		
+		point.time = pointIndex * itp / 1000.0;
+		point.velocity = filter2 * maxVelocity;
+		point.position = previousPosition + (point.velocity + previousVelocity) /  2 * itp / 1000;
+		point.acceleration = (point.velocity - previousVelocity) / itp * 1000;
+					
+		previousVelocity = point.velocity;
+		previousPosition = point.position;
+		windowIndex++;
+		if (windowIndex == numFilter2Boxes) {
+			windowIndex = 0;
+		}	
+		
+		pointIndex++;
+		
+		return point;
+	}
+
+	public double getStartDistance() {
+		return startDistance;
+	}
+
+	public double getTargetDistance() {
+		return targetDistance;
+	}
+
+	public double getMaxVelocity() {
+		return maxVelocity;
+	}
+
+	public double getItp() {
+		return itp;
+	}
+
+	public double getT1() {
+		return t1;
+	}
+
+	public double getT2() {
+		return t2;
+	}
+
+	public static void main(String[] args) {
+		long startTime = System.nanoTime();
+		
+		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) {
+			System.out.println(point.time + ", " + point.position + ", " + point.velocity + ", " + point.acceleration);
+		}
+		
+		long deltaTime = System.nanoTime() - startTime;
+		System.out.println("Time Box Car = " + (double)deltaTime * 1E-6 + " ms");
+	}
+}

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);
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java

@@ -0,0 +1,57 @@
+package org.usfirst.frc4388.utility;
+
+public class PIDParams 
+{
+    public double kP = 0;
+    public double kI = 0;
+    public double kD = 0;
+    public double kF = 0; 
+    public double kA = 0;
+    public double kV = 0;
+    public double kG = 0;
+    public double iZone = 0;
+    public double rampRatePID = 0;
+
+    public PIDParams() {	
+    }
+    
+    public PIDParams(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+    
+    public PIDParams(double kP, double kI, double kD, double kF) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+    }
+    
+    public PIDParams(double kP, double kI, double kD, double kA, double kV) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kA = kA;
+        this.kV = kV;
+    }
+    
+    public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kA = kA;
+        this.kV = kV;
+        this.kG = kG;
+    }
+    
+    public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG, double iZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kA = kA;
+        this.kV = kV;
+        this.kG = kG;
+        this.iZone = iZone;
+    }
+}

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();
+        }
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java

@@ -0,0 +1,101 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+//import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+public class SoftwarePIDController
+{	
+	protected ArrayList<TalonSRXEncoder> motorControllers;	
+	protected PIDParams pidParams;	
+	protected boolean useGyroLock;
+	protected double targetGyroAngle;
+	protected MPSoftwareTurnType turnType;
+	
+	protected double minTurnOutput = 0.002;
+	protected double maxError;
+	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 SoftwarePIDController( PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		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;
+				
+		prevError = 0.0;
+		totalError = 0.0;
+	}
+	
+	public boolean controlLoopUpdate() {
+		return controlLoopUpdate(0);
+	}
+	
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		// Calculate the motion profile feed forward and error feedback terms
+		double error = targetGyroAngle - currentGyroAngle;
+		double deltaLastError = (error - prevError);
+		
+		// Check if we are finished
+		if (Math.abs(error) < maxError && Math.abs(deltaLastError) < maxPrevError) {
+			return true;
+		}
+		
+		if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) {
+			totalError += error;
+		}
+		else {
+			totalError = 0;
+		}
+		
+		double output =  pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * deltaLastError;
+		double turnBoost = output < 0 ? -minTurnOutput : minTurnOutput;
+		output += turnBoost;
+		
+		prevError = error;
+			
+		// Update the controllers set point.
+		if (turnType == MPSoftwareTurnType.TANK) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				motorController.set(ControlMode.PercentOutput, output);
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 0);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);					
+				}
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
+			for (TalonSRXEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					motorController.set(ControlMode.PercentOutput, 2.0 * output);
+				}
+				else {
+					motorController.set(ControlMode.PercentOutput, 0);					
+				}
+			}
+		}
+
+		return false;
+	}	
+}

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);
+    }
+}

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();
+    }
+
+ }

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;
+    }
+}

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;
+    }
+}

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);
+    }
+}

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));
+    }
+}

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;
+    }
+}

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);
+    }
+}

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 + ")";
+        }
+    }
+}

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);
+    	}
+    }
+}

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";
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java

@@ -0,0 +1,24 @@
+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.
+ * 
+ * @param <T>
+ *            The Type of Interpolable
+ * @see InterpolatingTreeMap
+ */
+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.
+     *
+     * @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
+     */
+    public T interpolate(T other, double x);
+}

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;
+        }
+    }
+
+}

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;
+        }
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java

@@ -0,0 +1,88 @@
+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.
+ * 
+ * @param <K>
+ *            The type of the key (must implement InverseInterpolable)
+ * @param <V>
+ *            The type of the value (must implement Interpolable)
+ */
+public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<K>, V extends Interpolable<V>>
+        extends TreeMap<K, V> {
+    private static final long serialVersionUID = 8347275262778054124L;
+
+    int max_;
+
+    public InterpolatingTreeMap(int maximumSize) {
+        max_ = maximumSize;
+    }
+
+    public InterpolatingTreeMap() {
+        this(0);
+    }
+
+    /**
+     * Inserts a key value pair, and trims the tree if a max size is specified
+     * 
+     * @param key
+     *            Key for inserted data
+     * @param value
+     *            Value for inserted data
+     * @return the value
+     */
+    @Override
+    public V put(K key, V value) {
+        if (max_ > 0 && max_ <= size()) {
+            // "Prune" the tree if it is oversize
+            K first = firstKey();
+            remove(first);
+        }
+
+        super.put(key, value);
+
+        return value;
+    }
+
+    @Override
+    public void putAll(Map<? extends K, ? extends V> map) {
+        System.out.println("Unimplemented Method");
+    }
+
+    /**
+     *
+     * @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
+     */
+    public V getInterpolated(K key) {
+        V gotval = get(key);
+        if (gotval == null) {
+            /** Get surrounding keys for interpolation */
+            K topBound = ceilingKey(key);
+            K bottomBound = floorKey(key);
+
+            /**
+             * If attempting interpolation at ends of tree, return the nearest data point
+             */
+            if (topBound == null && bottomBound == null) {
+                return null;
+            } else if (topBound == null) {
+                return get(bottomBound);
+            } else if (bottomBound == null) {
+                return get(topBound);
+            }
+
+            /** Get surrounding values for interpolation */
+            V topElem = get(topBound);
+            V bottomElem = get(bottomBound);
+            return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key));
+        } else {
+            return gotval;
+        }
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java

@@ -0,0 +1,23 @@
+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
+ * interval [0, 1].
+ * 
+ * @param <T>
+ *            The Type of InverseInterpolable
+ * @see InterpolatingTreeMap
+ */
+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.
+     * 
+     * @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.
+     */
+    public double inverseInterpolate(T upper, T query);
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java

@@ -0,0 +1,182 @@
+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.
+ * 
+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
+ */
+public class RigidTransform2d implements Interpolable<RigidTransform2d> {
+    protected static final double kEpsilon = 1E-9;
+
+    protected static final RigidTransform2d kIdentity = new RigidTransform2d();
+
+    public static final RigidTransform2d identity() {
+        return kIdentity;
+    }
+
+    private final static double kEps = 1E-9;
+
+    protected Translation2d translation_;
+    protected Rotation2d rotation_;
+
+    public RigidTransform2d() {
+        translation_ = new Translation2d();
+        rotation_ = new Rotation2d();
+    }
+
+    public RigidTransform2d(Translation2d translation, Rotation2d rotation) {
+        translation_ = translation;
+        rotation_ = rotation;
+    }
+
+    public RigidTransform2d(RigidTransform2d other) {
+        translation_ = new Translation2d(other.translation_);
+        rotation_ = new Rotation2d(other.rotation_);
+    }
+
+    public static RigidTransform2d fromTranslation(Translation2d translation) {
+        return new RigidTransform2d(translation, new Rotation2d());
+    }
+
+    public static RigidTransform2d fromRotation(Rotation2d rotation) {
+        return new RigidTransform2d(new Translation2d(), rotation);
+    }
+
+    /**
+     * Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
+     * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
+     */
+    public static RigidTransform2d exp(Twist2d delta) {
+        double sin_theta = Math.sin(delta.dtheta);
+        double cos_theta = Math.cos(delta.dtheta);
+        double s, c;
+        if (Math.abs(delta.dtheta) < kEps) {
+            s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta;
+            c = .5 * delta.dtheta;
+        } else {
+            s = sin_theta / delta.dtheta;
+            c = (1.0 - cos_theta) / delta.dtheta;
+        }
+        return new RigidTransform2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
+                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_;
+    }
+
+    public void setTranslation(Translation2d translation) {
+        translation_ = translation;
+    }
+
+    public Rotation2d getRotation() {
+        return rotation_;
+    }
+
+    public void setRotation(Rotation2d rotation) {
+        rotation_ = rotation;
+    }
+
+    /**
+     * Transforming this RigidTransform2d means first translating by other.translation and then rotating by
+     * other.rotation
+     * 
+     * @param other
+     *            The other transform.
+     * @return This transform * other
+     */
+    public RigidTransform2d transformBy(RigidTransform2d other) {
+        return new RigidTransform2d(translation_.translateBy(other.translation_.rotateBy(rotation_)),
+                rotation_.rotateBy(other.rotation_));
+    }
+
+    /**
+     * The inverse of this transform "undoes" the effect of translating by this transform.
+     * 
+     * @return The opposite of this transform.
+     */
+    public RigidTransform2d inverse() {
+        Rotation2d rotation_inverted = rotation_.inverse();
+        return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
+    }
+
+    public RigidTransform2d normal() {
+        return new RigidTransform2d(translation_, rotation_.normal());
+    }
+
+    /**
+     * 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) {
+        if (x <= 0) {
+            return new RigidTransform2d(this);
+        } else if (x >= 1) {
+            return new RigidTransform2d(other);
+        }
+        final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other));
+        return transformBy(RigidTransform2d.exp(twist.scaled(x)));
+    }
+
+    @Override
+    public String toString() {
+        return "T:" + translation_.toString() + ", R:" + rotation_.toString();
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java

@@ -0,0 +1,144 @@
+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).
+ * 
+ * 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_;
+    protected double sin_angle_;
+
+    public Rotation2d() {
+        this(1, 0, false);
+    }
+
+    public Rotation2d(double x, double y, boolean normalize) {
+        cos_angle_ = x;
+        sin_angle_ = y;
+        if (normalize) {
+            normalize();
+        }
+    }
+
+    public Rotation2d(Rotation2d other) {
+        cos_angle_ = other.cos_angle_;
+        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);
+    }
+
+    public static Rotation2d fromDegrees(double angle_degrees) {
+        return fromRadians(Math.toRadians(angle_degrees));
+    }
+
+    /**
+     * 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_);
+        if (magnitude > kEpsilon) {
+            sin_angle_ /= magnitude;
+            cos_angle_ /= magnitude;
+        } else {
+            sin_angle_ = 0;
+            cos_angle_ = 1;
+        }
+    }
+
+    public double cos() {
+        return cos_angle_;
+    }
+
+    public double sin() {
+        return sin_angle_;
+    }
+
+    public double tan() {
+        if (Math.abs(cos_angle_) < kEpsilon) {
+            if (sin_angle_ >= 0.0) {
+                return Double.POSITIVE_INFINITY;
+            } else {
+                return Double.NEGATIVE_INFINITY;
+            }
+        }
+        return sin_angle_ / cos_angle_;
+    }
+
+    public double getRadians() {
+        return Math.atan2(sin_angle_, cos_angle_);
+    }
+
+    public double getDegrees() {
+        return Math.toDegrees(getRadians());
+    }
+
+    /**
+     * 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
+     * @return This rotation rotated by other.
+     */
+    public Rotation2d rotateBy(Rotation2d other) {
+        return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_,
+                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.
+     * 
+     * @return The opposite of this rotation.
+     */
+    public Rotation2d inverse() {
+        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) {
+            return new Rotation2d(this);
+        } else if (x >= 1) {
+            return new Rotation2d(other);
+        }
+        double angle_diff = inverse().rotateBy(other).getRadians();
+        return this.rotateBy(Rotation2d.fromRadians(angle_diff * x));
+    }
+
+    @Override
+    public String toString() {
+        final DecimalFormat fmt = new DecimalFormat("#0.000");
+        return "(" + fmt.format(getDegrees()) + " deg)";
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java

@@ -0,0 +1,141 @@
+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.
+ */
+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_;
+
+    public Translation2d() {
+        x_ = 0;
+        y_ = 0;
+    }
+
+    public Translation2d(double x, double y) {
+        x_ = x;
+        y_ = y;
+    }
+
+    public Translation2d(Translation2d other) {
+        x_ = other.x_;
+        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)
+     */
+    public double norm() {
+        return Math.hypot(x_, y_);
+    }
+
+    public double norm2() {
+        return x_ * x_ + y_ * y_;
+    }
+
+    public double x() {
+        return x_;
+    }
+
+    public double y() {
+        return y_;
+    }
+
+    public void setX(double x) {
+        x_ = x;
+    }
+
+    public void setY(double y) {
+        y_ = y;
+    }
+
+    /**
+     * We can compose Translation2d's by adding together the x and y shifts.
+     * 
+     * @param other
+     *            The other translation to add.
+     * @return The combined effect of translating by this object and the other.
+     */
+    public Translation2d translateBy(Translation2d other) {
+        return new Translation2d(x_ + other.x_, y_ + other.y_);
+    }
+
+    /**
+     * We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
+     * 
+     * @param rotation
+     *            The rotation to apply.
+     * @return This translation rotated by rotation.
+     */
+    public Translation2d rotateBy(Rotation2d rotation) {
+        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.
+     * 
+     * @return Translation by -x and -y.
+     */
+    public Translation2d inverse() {
+        return new Translation2d(-x_, -y_);
+    }
+
+    @Override
+    public Translation2d interpolate(Translation2d other, double x) {
+        if (x <= 0) {
+            return new Translation2d(this);
+        } else if (x >= 1) {
+            return new Translation2d(other);
+        }
+        return extrapolate(other, x);
+    }
+
+    public Translation2d extrapolate(Translation2d other, double x) {
+        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_;
+    }
+}

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);
+    }
+}

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());
+    }
+}

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();
+    }
+}

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());
+    }
+}

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;
+    }
+}

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());
+    }
+}

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();
+    }
+}

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);
+    }
+}

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;
+}

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);
+    }
+}

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;
+    }
+}

2019robot/vendordeps/Phoenix.json

@@ -0,0 +1,87 @@
+{
+    "fileName": "Phoenix.json",
+    "name": "CTRE-Phoenix",
+    "version": "5.12.0",
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "http://devsite.ctr-electronics.com/maven/release/"
+    ],
+    "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.12.0"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.12.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.12.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.12.0",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.12.0",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.12.0",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "core",
+            "version": "5.12.0",
+            "libName": "CTRE_PhoenixCore",
+            "headerClassifier": "headers"
+        }
+    ]
+}

2019robot/vendordeps/REVRobotics.json

@@ -0,0 +1,32 @@
+{
+    "fileName": "REVRobotics.json",
+    "name": "REVRobotics",
+    "version": "1.0.27",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "http://www.revrobotics.com/content/sw/max/sdk/maven/"
+    ],
+    "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "SparkMax-java",
+            "version": "1.0.27"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "SparkMax-cpp",
+            "version": "1.0.27",
+            "libName": "libSparkMax",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ]
+}

2019robot/vendordeps/cw.json

@@ -0,0 +1,21 @@
+{
+    "fileName": "cw.json",
+    "name": "cw",
+    "version": "3.1.344",
+    "uuid":"13",
+    "mavenUrls": [
+        "https://mvnrepository.com"
+    ],
+    "jsonUrl": "file:///C:/dev/cw.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.googlecode.json-simple",
+            "artifactId": "json-simple",
+            "version": "1.1.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+       
+    ]
+}