Initial commit

.classpath

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/>
+	<classpathentry kind="var" path="networktables" sourcepath="networktables.sources"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/json-simple-1.1.1.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/Pathfinder-Java.jar"/>
+	<classpathentry kind="var" path="opencv" sourcepath="opencv.sources"/>
+	<classpathentry kind="var" path="cscore" sourcepath="cscore.sources"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
+	<classpathentry kind="var" path="wpiutil" sourcepath="wpiutil.sources"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

.gitignore

@@ -0,0 +1,4 @@
+
+bin/
+build/
+dist/

.project

@@ -0,0 +1,29 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>2018-Robot</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature</nature>
+	</natures>
+	<filteredResources>
+		<filter>
+			<id>1419649143453</id>
+			<name></name>
+			<type>30</type>
+			<matcher>
+				<id>org.eclipse.ui.ide.multiFilter</id>
+				<arguments>1.0-location-matches-false-false-*~</arguments>
+			</matcher>
+		</filter>
+	</filteredResources>
+</projectDescription>

BSD_License_for_WPILib_code.txt

@@ -0,0 +1,24 @@
+* Copyright (c) 2009 FIRST
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*     * Redistributions of source code must retain the above copyright
+*       notice, this list of conditions and the following disclaimer.
+*     * Redistributions in binary form must reproduce the above copyright
+*       notice, this list of conditions and the following disclaimer in the
+*       documentation and/or other materials provided with the distribution.
+*     * Neither the name of the FIRST nor the
+*       names of its contributors may be used to endorse or promote products
+*       derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1 @@
+# 2018-Robot

build.properties

@@ -0,0 +1,7 @@
+# Project specific information
+package=org.usfirst.frc4388.robot
+robot.class=${package}.Robot
+simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
+#Uncomment and point at user libraries to include them in the build. Do not put libraries in the \wpilib\java folder, this folder is completely overwritten on plugin update.
+#userLibs=${user.home}/wpilib/user/lib
+

build.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<project name="FRC Deployment" default="deploy">
+
+  <!--
+  The following properties can be defined to override system level
+  settings. These should not be touched unless you know what you're
+  doing. The primary use is to override the wpilib version when
+  working with older robots that can't compile with the latest
+  libraries.
+  -->
+
+  <!-- By default the system version of WPI is used -->
+  <!-- <property name="version" value=""/> -->
+
+  <!-- By default the system team number is used -->
+  <!-- <property name="team-number" value=""/> -->
+
+  <!-- By default the target is set to 10.TE.AM.2 -->
+  <!-- <property name="target" value=""/> -->
+
+  <!-- Any other property in build.properties can also be overridden. -->
+
+  <property file="${user.home}/wpilib/wpilib.properties"/>
+  <property file="build.properties"/>
+  <property file="${user.home}/wpilib/java/${version}/ant/build.properties"/>
+
+  <import file="${wpilib.ant.dir}/build.xml"/>
+
+</project>

src/buttons/XBoxTriggerButton.java

@@ -0,0 +1,65 @@
+package buttons;
+
+import org.usfirst.frc4388.controller.XboxController;
+
+import edu.wpi.first.wpilibj.buttons.Button;
+
+/**
+ *
+ * @author bselle
+ */
+public class XBoxTriggerButton extends Button 
+{
+	public static final int RIGHT_TRIGGER = 0;
+	public static final int LEFT_TRIGGER = 1;
+	public static final int RIGHT_AXIS_UP_TRIGGER = 2;
+	public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
+	public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
+	public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
+	public static final int LEFT_AXIS_UP_TRIGGER = 6;
+	public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
+	public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
+	public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
+
+	private XboxController m_controller;
+	private int m_trigger;
+
+	public XBoxTriggerButton(XboxController controller, int trigger) {
+		m_controller = controller;
+		m_trigger = trigger;
+	}
+
+	public boolean get() {
+		if (m_trigger == RIGHT_TRIGGER) {
+			return m_controller.getRightTrigger();
+		}
+		else if (m_trigger == LEFT_TRIGGER) {
+			return m_controller.getLeftTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
+			return m_controller.getRightAxisUpTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getRightAxisDownTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getRightAxisRightTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getRightAxisLeftTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
+			return m_controller.getLeftAxisUpTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getLeftAxisDownTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getLeftAxisRightTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getLeftAxisLeftTrigger();
+		}
+		return false;
+	}
+}

src/org/usfirst/frc4388/controller/IHandController.java

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

src/org/usfirst/frc4388/controller/XboxController.java

@@ -0,0 +1,209 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.usfirst.frc4388.controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+//TODO Code support for Start/Back/XBox(?) buttons.
+/**
+ * This is a wrapper for the WPILib Joystick class that represents an XBox 
+ * controller.
+ * @author frc1675
+ */
+public class XboxController implements IHandController
+{
+	public static final int LEFT_X_AXIS = 0;
+	public static final int LEFT_Y_AXIS = 1;
+	public static final int LEFT_TRIGGER_AXIS = 2;
+	public static final int RIGHT_TRIGGER_AXIS = 3;
+	public static final int RIGHT_X_AXIS = 4;
+	public static final int RIGHT_Y_AXIS = 5;
+	public static final int LEFT_RIGHT_DPAD_AXIS = 6;
+	public static final int TOP_BOTTOM_DPAD_AXIS = 6;
+
+	public static final int A_BUTTON = 1;
+	public static final int B_BUTTON = 2;
+	public static final int X_BUTTON = 3;
+	public static final int Y_BUTTON = 4;
+	public static final int LEFT_BUMPER_BUTTON = 5;
+	public static final int RIGHT_BUMPER_BUTTON = 6;
+	public static final int BACK_BUTTON = 7;
+	public static final int START_BUTTON = 8;
+
+	public static final int LEFT_JOYSTICK_BUTTON = 9;
+	public static final int RIGHT_JOYSTICK_BUTTON = 10;
+
+	private static final double LEFT_DPAD_TOLERANCE = -0.9;
+	private static final double RIGHT_DPAD_TOLERANCE = 0.9;
+	private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
+	private static final double TOP_DPAD_TOLERANCE = 0.9;
+
+	private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
+	private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
+
+	private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double DEADZONE = 0.1;
+
+	private Joystick stick;
+
+	public XboxController(int portNumber){
+		stick = new Joystick(portNumber);
+	}
+
+	public Joystick getJoyStick() {
+		return stick;
+	}
+	
+	private boolean inDeadZone(double input){
+		boolean inDeadZone; 
+		if(Math.abs(input) < DEADZONE){
+			inDeadZone = true;
+		}else{
+			inDeadZone = false;
+		}
+		return inDeadZone;
+	}
+
+	private double getAxisWithDeadZoneCheck(double input){
+		if(inDeadZone(input)){
+			input = 0.0;       
+		}
+		return input; 
+	}
+
+	public boolean getAButton(){
+		return stick.getRawButton(A_BUTTON);
+	}
+
+	public boolean getXButton(){
+		return stick.getRawButton(X_BUTTON);    
+	}
+
+	public boolean getBButton(){
+		return stick.getRawButton(B_BUTTON);
+	}
+
+	public boolean getYButton(){
+		return stick.getRawButton(Y_BUTTON);
+	} 
+
+	public boolean getBackButton(){
+		return stick.getRawButton(BACK_BUTTON);
+	}
+
+	public boolean getStartButton(){
+		return stick.getRawButton(START_BUTTON);
+	} 
+
+	public boolean getLeftBumperButton(){
+		return stick.getRawButton(LEFT_BUMPER_BUTTON);
+	}
+
+	public boolean getRightBumperButton(){
+		return stick.getRawButton(RIGHT_BUMPER_BUTTON);
+	}
+
+	public boolean getLeftJoystickButton(){
+		return stick.getRawButton(LEFT_JOYSTICK_BUTTON);
+	}
+
+	public boolean getRightJoystickButton(){
+		return stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
+	}
+
+	public double getLeftXAxis(){         
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_X_AXIS)); 
+	}
+
+	public double getLeftYAxis(){
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_Y_AXIS));
+	}
+
+	public double getRightXAxis(){
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_X_AXIS));
+	}
+
+	public double getRightYAxis(){
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_Y_AXIS)); 
+	}
+
+	public double getLeftTriggerAxis(){
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_TRIGGER_AXIS)); 
+	}
+
+	public double getRightTriggerAxis(){
+		return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_TRIGGER_AXIS)); 
+	}
+
+	/**Returns -1 if nothing is pressed, or the angle of the button pressed 0 = up, 90 = right, etc.*/
+	public int getDpadAngle() {
+		return stick.getPOV();
+	}
+
+	public boolean getDPadLeft(){
+      return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
+  }
+
+  public boolean getDPadRight(){
+      return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
+  }
+
+  public boolean getDPadTop(){
+      return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
+  }
+
+  /*public boolean getDPadBottom(){
+      return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
+  }
+*/
+	public boolean getLeftTrigger(){
+		return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
+	}
+
+	public boolean getRightTrigger(){
+		return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
+	}   
+
+	public boolean getRightAxisUpTrigger(){
+		return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getRightAxisDownTrigger(){
+		return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getRightAxisLeftTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getRightAxisRightTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisUpTrigger(){
+		return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisDownTrigger(){
+		return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisLeftTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisRightTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
+	}
+}

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

@@ -0,0 +1,73 @@
+
+package org.usfirst.frc4388.robot;
+
+//import org.usfirst.frc4388.utility.ConstantsBase;
+
+/**
+ * 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 extends ConstantsBase {
+public class Constants {
+    // Wheels
+    //public static double kDriveWheelDiameterInches = 5.9; 
+    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 kDriveTurnBasicTankMotorOutput = 0.2;
+    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);
+	public static double kElevatorEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev /(kElevatorWheelDiameterInches * Math.PI) ;
+    // 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
+
+//    @Override
+//    public String getFileLocation() {
+//        return "~/constants.txt";
+//    }
+
+//    static {
+//        new Constants().loadFromFile();
+//    }
+}

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

@@ -0,0 +1,479 @@
+
+
+
+package org.usfirst.frc4388.robot;
+
+//
+//import org.usfirst.frc4388.buttons.XBoxDPadTriggerButton;
+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);
+			
+			//ELevator PID buttons
+			
+			//JoystickButton elevatorMaxScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
+			//elevatorMaxScaleHeight.whenPressed(new PositionPIDMaxScaleHeight(50));
+			
+			//JoystickButton elevatorLowScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
+			//elevatorLowScaleHeight.whenPressed(new PositionPIDLowScaleHeight(50));
+			
+			//JoystickButton elevatorSwitchHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
+			//elevatorSwitchHeight.whenPressed(new PositionPIDLowScaleHeight(50));
+			
+			
+			/*
+			JoystickButton elevatorLowestHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
+			elevatorLowestHeight.whenPressed(new PositionPIDLowScaleHeight(50));
+			*/
+		
+			
+			
+	        //JoystickButton shiftDrivetrain = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+	        //shiftDrivetrain.whenPressed(new DriveSpeedShift(Drive.SpeedShiftState.HI));
+	        //shiftDrivetrain.whenReleased(new DriveSpeedShift(Drive.SpeedShiftState.LO));
+	        
+	        //JoystickButton longShot = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
+	        //longShot.whenPressed(new ShootOn(ShotState.FAR, ShooterFeed.SHOOTER_FEED_SHOOT_FAR_SPEED, false));
+	        //longShot.whenReleased(new ShootOff());
+	        
+	        //XBoxTriggerButton shortShot = new XBoxTriggerButton(m_driverXbox, XBoxTriggerButton.RIGHT_TRIGGER);
+	        //shortShot.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, false));
+	        //shortShot.whenReleased(new ShootOff());
+	        
+			
+			
+	        XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
+	        CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+	        CarriageIntake.whenReleased(new IntakeSetSpeed(0.0));
+	        
+	        XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
+	        CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
+	        CarriageEject.whenReleased(new IntakeSetSpeed(0.0));
+	        
+			
+			
+	        //JoystickButton gearIntakeDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON);
+	        //gearIntakeDown.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE));
+	        //gearIntakeDown.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+	       
+	        //JoystickButton gearIntakeRetract = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
+	        //gearIntakeRetract.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT));
+
+	        //JoystickButton gearIntakePresent = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.B_BUTTON);
+	        //gearIntakePresent.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+	        
+	        //JoystickButton gearIntakeDeploy = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.A_BUTTON);
+	        //gearIntakeDeploy.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
+	        
+	        JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
+	        climbUp.whenPressed(new InitiateClimber(true));
+	        climbUp.whenReleased(new InitiateClimber(false));
+	        
+	        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));
+	        
+	        
+	        //XBoxDPadTriggerButton climberReverse = new XBoxDPadTriggerButton(m_driverXbox, XBoxDPadTriggerButton.DOWN);
+	        //climberReverse.whenPressed(new ClimberSetSpeed(0.48, 0.0));
+	        //climberReverse.whenReleased(new ClimberSetSpeed(0.0, 0.0));
+	        
+	        //JoystickButton toggleShooter = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.START_BUTTON);
+	        //toggleShooter.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE)); 
+
+	        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));
+	     	        
+	        //XBoxTriggerButton shortShotOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
+	        //shortShotOperator.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, true));
+	        //shortShotOperator.whenReleased(new ShootOff());
+	                
+	        //XBoxTriggerButton intakeGearOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
+	        //intakeGearOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_LOAD_SPEED));
+	        //intakeGearOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0));
+	                
+	        //JoystickButton gearIntakeEjectOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+	        //gearIntakeEjectOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_EJECT_SPEED));
+	        //gearIntakeEjectOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0));
+	        
+	        //JoystickButton gearIntakeDownOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
+	        //gearIntakeDownOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE));
+	        //gearIntakeDownOperator.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+	        
+	        //JoystickButton gearIntakeRetractOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
+	        //gearIntakeRetractOperator.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT));
+
+	        //JoystickButton gearIntakePresentOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
+	        //gearIntakePresentOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+	                
+	        //JoystickButton gearIntakeDeployOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
+	        //gearIntakeDeployOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
+	        
+	        //XBoxDPadTriggerButton climberForwardOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.UP);
+	        //climberForwardOperator.whenPressed(new ClimberSetSpeed(ShooterFeed.CLIMB_SPEED, 0.0));
+	        //climberForwardOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0));
+	        
+	        //XBoxDPadTriggerButton climberReverseOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.DOWN);
+	        //climberReverseOperator.whenPressed(new ClimberSetSpeed(0.48, 0.0));
+	        //climberReverseOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0));
+	        
+	        //XBoxDPadTriggerButton hopperOpen = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.LEFT);
+	        //hopperOpen.whenPressed(new ShooterSetHopperPosition(HopperState.OPEN));
+	        
+	        //XBoxDPadTriggerButton hopperClose = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.RIGHT);
+	        //hopperClose.whenPressed(new ShooterSetHopperPosition(HopperState.CLOSE));
+	        
+	        //JoystickButton toggleShooterOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
+	        //toggleShooterOperator.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE)); 
+			
+	        // Gear sensor switch 
+//			GearSensorAnalogSwitch gearSwitch = new GearSensorAnalogSwitch();
+//			gearSwitch.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+	        
+	        // SmartDashboard
+//			Button driveMP = new InternalButton();
+//			driveMP.whenPressed(new DriveStraightMP(40, 20, true, false, 0));
+//			SmartDashboard.putData("Drive Straight", driveMP);
+	        
+	        
+			//SmartDashboard.putData("Drive Straight 30in", new DriveStraightMP(30, 20, true, true, 0));
+			//SmartDashboard.putData("Drive Straight 60in", new DriveStraightMP(60, 20, true, true, 0));
+	
+	        //SmartDashboard.putData("Basic Straight 60in", new DriveStraightBasic(60, 30, true, true, 0));
+			//SmartDashboard.putData("Basic Straight -60in", new DriveStraightBasic(-60, 30, true, true, 0));
+			//SmartDashboard.putData("Turn abs 90 TANK", new DriveTurnBasic(true, 90, 90, MPSoftwareTurnType.TANK));
+		//	SmartDashboard.putData("Turn rel -90 RIGHT ONLY", new DriveTurnBasic(false, -90, 90, MPSoftwareTurnType.RIGHT_SIDE_ONLY));
+			//SmartDashboard.putData("RUN INTAKE", new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+		//	SmartDashboard.putData("Stop INTAKE", new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+			//SmartDashboard.putData("move intake", new IntakePosition(true));
+			//SmartDashboard.putData("move intake2", new IntakePosition(false));
+			//SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED));
+			SmartDashboard.putData("move elevator", new ElevatorBasic(20, 15));
+			///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
+			///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));
+			
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); // forward , back to right
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-50, 30, true, true, 0)); // forward, back to the right
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-40, 30, true, true, 0)); //same af -50
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-30, 30, true, true, 0));// same as above shorter
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 20, true, true, 0));// could not break ininita
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(100, 20, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
+			
+			
+			
+			
+			
+			
+			
+			
+			
+	        //List<Waypoint> waypoints = new ArrayList<>();
+	        //waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0));
+	        //waypoints.add(new Waypoint(new Translation2d(-35, 0), 35.0));
+	        //Path.addCircleArc(waypoints, -30.0, 45.0, 10, "hopperSensorOn");
+	        //waypoints.add(new Waypoint(new Translation2d(-85, -30), 35.0));
+
+//	        List<Waypoint> waypoints = new ArrayList<>();
+//	        waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0));
+//	        waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0));
+//	        waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0));
+//	        waypoints.add(new Waypoint(new Translation2d(-61, 0), 50.0));
+//	        waypoints.add(new Waypoint(new Translation2d(-84.93, -10.16), 50.0));
+	        
+//	        waypoints.add(new Waypoint(new Translation2d(-41, 0), 35.0));
+//	        waypoints.add(new Waypoint(new Translation2d(-65, -23), 35.0));
+//	        waypoints.add(new Waypoint(new Translation2d(-70, -39), 35.0));
+	        
+//	        waypoints.add(new Waypoint(new Translation2d(-29, 0), 40.0));
+//	        Path.addCircleArc(waypoints, -30.0, 45.0, 10, null);
+//	        waypoints.add(new Waypoint(new Translation2d(-68.6, -26.5), 40.0));
+			
+			//Button driveAP = new InternalButton();
+			//driveAP.whenPressed(new DrivePathAdaptivePursuit(new Path(waypoints), true));
+			//SmartDashboard.putData("Drive Adaptive Pursuit", driveAP);
+
+			//Button driveMM = new InternalButton();
+			//driveMM.whenPressed(new DriveStraightMM(10, 450, 450, true, false, 0));
+			//SmartDashboard.putData("Drive StraightMM", driveMM);
+			
+			//Button turnRelativePID = new InternalButton();
+			//turnRelativePID.whenPressed(new DriveRelativeTurnPID(-10, MPSoftwareTurnType.RIGHT_SIDE_ONLY));
+			//SmartDashboard.putData("Turn Relative 10 PID", turnRelativePID);
+			
+			//Button turnRelativePID7 = new InternalButton();
+			//turnRelativePID7.whenPressed(new DriveRelativeTurnPID(7, MPSoftwareTurnType.LEFT_SIDE_ONLY));
+			//SmartDashboard.putData("Turn Relative 7 PID", turnRelativePID7);
+			
+			//Button turnRelativeMP = new InternalButton();
+			//turnRelativeMP.whenPressed(new DriveRelativeTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY ));
+			//SmartDashboard.putData("Turn Relative 10", turnRelativeMP);
+			
+			Button turnAbsoluteMP = new InternalButton();
+			turnAbsoluteMP.whenPressed(new DriveAbsoluteTurnMP(45, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY));
+			SmartDashboard.putData("Turn Absolute 45", turnAbsoluteMP);
+
+			Button turnRelativePID = new InternalButton();
+			turnRelativePID.whenPressed(new DriveRelativeTurnPID(45, MPSoftwareTurnType.TANK));
+			SmartDashboard.putData("Turn Relative 45", turnRelativePID);
+
+//			Button gyroReset = new InternalButton();
+//			gyroReset.whenPressed(new DriveGyroReset());
+//			SmartDashboard.putData("Gyro Reset", gyroReset);
+			SmartDashboard.putData("Reset Gyro", new DriveGyroReset());
+			//SmartDashboard.putData("reset Encoders", new drive.resetEncoders());
+
+
+			// Camera
+			//Button cameraUpdateDashboard = new InternalButton();
+			//cameraUpdateDashboard.whenPressed(new CameraUpdateDashboard());
+			//SmartDashboard.putData("Camera Update", cameraUpdateDashboard);
+			
+			//Button cameraSaveImage = new InternalButton();
+			//cameraSaveImage.whenPressed(new CameraSaveImage());
+			//SmartDashboard.putData("Camera Save", cameraSaveImage);
+			
+			//Button cameraTurnToBestTarget = new InternalButton();
+			//cameraTurnToBestTarget.whenPressed(new CameraTurnToBestTarget());
+			//SmartDashboard.putData("Camera Turn To Best", cameraTurnToBestTarget);
+			
+			//Button cameraUpdateBestTarget = new InternalButton();
+			//cameraUpdateBestTarget.whenPressed(new CameraUpdateBestTarget());
+			//SmartDashboard.putData("Camera Update Best", cameraUpdateBestTarget);
+			
+			//Button incrementCameraOffsetPos = new InternalButton();
+			//incrementCameraOffsetPos.whenPressed(new CameraIncrementOffset(0.5));
+			//SmartDashboard.putData("Camera Offset Pos", incrementCameraOffsetPos);
+			
+			//Button incrementCameraOffsetNeg = new InternalButton();
+			//incrementCameraOffsetNeg.whenPressed(new CameraIncrementOffset(-0.5));
+			//SmartDashboard.putData("Camera Offset Neg", incrementCameraOffsetNeg);
+			
+//			Button cameraReadImage = new InternalButton();
+//			cameraReadImage.whenPressed(new CameraReadAndProcessImage());
+//			SmartDashboard.putData("Camera Read", cameraReadImage);
+//			
+//			Button cameraReadImageTurnToBestTarget = new InternalButton();
+//			cameraReadImageTurnToBestTarget.whenPressed(new CameraReadImageTurnToBestTarget());
+//			SmartDashboard.putData("Camera Read Turn", cameraReadImageTurnToBestTarget);
+//			
+//			Button magicCarpetOn05 = new InternalButton();
+//			magicCarpetOn05.whenPressed(new ZarkerFeedSetSpeed(0.5));
+//			SmartDashboard.putData("Magic Carpet 0.5", magicCarpetOn05);
+	//
+//			Button magicCarpetOff = new InternalButton();
+//			magicCarpetOff.whenPressed(new ZarkerFeedSetSpeed(0.0));
+//			SmartDashboard.putData("Magic Carpet Off", magicCarpetOff);
+	//
+//			Button shooterFeedOn10 = new InternalButton();
+//			shooterFeedOn10.whenPressed(new ShooterFeedSetSpeed(1.0));
+//			SmartDashboard.putData("Shooter Feed 1.0", shooterFeedOn10);
+	//
+//			Button shooterFeedOn05 = new InternalButton();
+//			shooterFeedOn05.whenPressed(new ShooterFeedSetSpeed(0.5));
+//			SmartDashboard.putData("Shooter Feed 0.5", shooterFeedOn05);
+	//
+//			Button shooterFeedOff = new InternalButton();
+//			shooterFeedOff.whenPressed(new ShooterFeedSetSpeed(0.0));
+//			SmartDashboard.putData("Shooter Feed Off", shooterFeedOff);
+	//
+//			Button shooterStage2RPMOnDash = new InternalButton();
+//			shooterStage2RPMOnDash.whenPressed(new ShooterStage2SetRpmDashboard());
+//			SmartDashboard.putData("Shooter Stage 2 Dash RPM", shooterStage2RPMOnDash);
+	//
+//			Button shooterStage2On10 = new InternalButton();
+//			shooterStage2On10.whenPressed(new ShooterStage2SetSpeed(1.0));
+//			SmartDashboard.putData("Shooter Stage 2 - 1.0", shooterStage2On10);
+	//
+//			Button shooterStage2On09 = new InternalButton();
+//			shooterStage2On09.whenPressed(new ShooterStage2SetSpeed(0.9));
+//			SmartDashboard.putData("Shooter Stage 2 - 0.9", shooterStage2On09);
+	//
+//			Button shooterStage2On08 = new InternalButton();
+//			shooterStage2On08.whenPressed(new ShooterStage2SetSpeed(0.8));
+//			SmartDashboard.putData("Shooter Stage 2 - 0.8", shooterStage2On08);
+	//
+//			Button shooterStage2On07 = new InternalButton();
+//			shooterStage2On07.whenPressed(new ShooterStage2SetSpeed(0.7));
+//			SmartDashboard.putData("Shooter Stage 2 - 0.7", shooterStage2On07);
+	//
+//			Button shooterStage2On06 = new InternalButton();
+//			shooterStage2On06.whenPressed(new ShooterStage2SetSpeed(0.6));
+//			SmartDashboard.putData("Shooter Stage 2 - 0.6", shooterStage2On06);
+	//
+//			Button shooterStage2On05 = new InternalButton();
+//			shooterStage2On05.whenPressed(new ShooterStage2SetSpeed(0.5));
+//			SmartDashboard.putData("Shooter Stage 2 - 0.5", shooterStage2On05);
+	//
+//			Button shooterStage2Off = new InternalButton();
+//			shooterStage2Off.whenPressed(new ShooterStage2SetSpeed(0.0));
+//			SmartDashboard.putData("Shooter Stage 2 - Off", shooterStage2Off);
+//			
+//			Button shooterStage1RPMOnDash = new InternalButton();
+//			shooterStage1RPMOnDash.whenPressed(new ShooterStage1SetRpmDashboard());
+//			SmartDashboard.putData("Shooter Stage 1 Dash RPM", shooterStage1RPMOnDash);
+	//
+//			Button shooterStage1On05 = new InternalButton();
+//			shooterStage1On05.whenPressed(new ShooterStage1SetSpeed(0.5));
+//			SmartDashboard.putData("Shooter Stage 1 - 0.5", shooterStage1On05);
+//			
+//			Button shooterStage1Off = new InternalButton();
+//			shooterStage1Off.whenPressed(new ShooterStage1SetSpeed(0.0));
+//			SmartDashboard.putData("Shooter Stage 1 - Off", shooterStage1Off);
+//			
+//			Button shooterBothRPMOnDash = new InternalButton();
+//			shooterBothRPMOnDash.whenPressed(new ShooterSetRpmDashboard());
+//			SmartDashboard.putData("Shooter Both Dash RPM", shooterBothRPMOnDash);
+	//
+//			Button shooterBothVBusOnDash = new InternalButton();
+//			shooterBothVBusOnDash.whenPressed(new ShooterSetSpeedDashboard());
+//			SmartDashboard.putData("Shooter Both Dash VBus", shooterBothVBusOnDash);
+	//
+//			Button allButLiftOff = new InternalButton();
+//			allButLiftOff.whenPressed(new ShooterAllOff());
+//			SmartDashboard.putData("Shooter All Off", allButLiftOff);
+//			
+//			Button allButLiftOn = new InternalButton();
+//			allButLiftOn.whenPressed(new ShooterAllOn());
+//			SmartDashboard.putData("Shooter All On", allButLiftOn);
+	//
+//			Button hopperShake = new InternalButton();
+//			hopperShake.whenPressed(new ShooterSetHopperShake(2, 10, 1));
+//			SmartDashboard.putData("Hopper Shake Button", hopperShake);
+	//
+//			Button intakeOn10 = new InternalButton();
+//			intakeOn10.whenPressed(new GearIntakeRollerSetSpeed(1.0));
+//			SmartDashboard.putData("Intake 1.0", intakeOn10);
+	//
+//			Button intakeOn08 = new InternalButton();
+//			intakeOn08.whenPressed(new GearIntakeRollerSetSpeed(0.8));
+//			SmartDashboard.putData("Intake 0.8", intakeOn08);
+	//
+//			Button intakeOn05 = new InternalButton();
+//			intakeOn05.whenPressed(new GearIntakeRollerSetSpeed(0.5));
+//			SmartDashboard.putData("Intake 0.5", intakeOn05);
+	//
+//			Button intakeOn04 = new InternalButton();
+//			intakeOn04.whenPressed(new GearIntakeRollerSetSpeed(0.4));
+//			SmartDashboard.putData("Intake 0.4", intakeOn04);
+	//
+//			Button intakeOff = new InternalButton();
+//			intakeOff.whenPressed(new GearIntakeRollerSetSpeed(0.0));
+//			SmartDashboard.putData("Intake Off", intakeOff);
+	//
+//			Button shooterShotPositionClose = new InternalButton();
+//			shooterShotPositionClose.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.CLOSE));
+//			SmartDashboard.putData("Shooter Close Shot", shooterShotPositionClose);
+//			
+//			Button shooterShotPositionFar = new InternalButton();
+//			shooterShotPositionFar.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.FAR));
+//			SmartDashboard.putData("Shooter Far Shot", shooterShotPositionFar);
+//			
+//			Button climberOn08 = new InternalButton();
+//			climberOn08.whenPressed(new ClimberSetSpeed(0.8, 0.0));
+//			climberOn08.whenReleased(new ClimberSetSpeed(0.0, 0.0));
+//			SmartDashboard.putData("Climber 0.8", climberOn08);
+	//
+//			Button climberSetMaxAmps = new InternalButton();
+//			climberSetMaxAmps.whenPressed(new ClimberSetMaxAmps(0.8, 20));
+//			SmartDashboard.putData("Climber Max Amps", climberSetMaxAmps);
+//			
+	        //jaci.pathfinder.Waypoint[] points = new jaci.pathfinder.Waypoint[] {
+	        //        new jaci.pathfinder.Waypoint(0, 0, 0),
+	        //        new jaci.pathfinder.Waypoint(-90, -16, Pathfinder.d2r(-45))
+	        //};
+
+	        //PathGenerator path = new PathGenerator(points, 0.01, 160, 200.0, 700.0);		
+			
+			//Button pathTest = new InternalButton();
+			//pathTest.whenPressed(new DrivePathMP(path));
+			//SmartDashboard.putData("Path Test", pathTest);
+
+//			Button ledsOn = new InternalButton();
+//			ledsOn.whenPressed(new LEDLightsSet(true));
+//			SmartDashboard.putData("LEDs On", ledsOn);
+//			
+//			Button ledsOff = new InternalButton();
+//			ledsOff.whenPressed(new LEDLightsSet(false));
+//			SmartDashboard.putData("LEDs Off", ledsOff);
+//			
+//	        Button intakeGearRoller = new InternalButton();
+//	        intakeGearRoller.whenPressed(new GearIntakeRollerSetSpeed(0.3));
+//			SmartDashboard.putData("Gear Intake Clean Speed", intakeGearRoller);
+	//
+//	        Button intakeGearStopRoller = new InternalButton();
+//	        intakeGearStopRoller.whenPressed(new GearIntakeRollerSetSpeed(0.0));
+//			SmartDashboard.putData("Gear Intake Clean Off Speed", intakeGearStopRoller);
+		  } 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();
+		}
+	}
+

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

@@ -0,0 +1,227 @@
+
+// RobotBuilder Version: 2.0
+//
+// This file was generated by RobotBuilder. It contains sections of
+// code that are automatically generated and assigned by robotbuilder.
+// These sections will be updated in the future when you export to
+// Java from RobotBuilder. Do not put any code or make any change in
+// the blocks indicating autogenerated code or it will be lost on an
+// update. Deleting the comments indicating the section will prevent
+// it from being updated in the future.
+
+
+package org.usfirst.frc4388.robot;
+
+//import edu.wpi.first.wpilibj.TimedRobot;
+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.commands.auton.*;
+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.Carriage;
+import org.usfirst.frc4388.robot.subsystems.LED;;
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.properties file in 
+ * the project.
+ */
+public class Robot extends IterativeRobot 
+{
+
+	public static OI oi;
+	
+	public static final Drive drive = new Drive();
+
+	//public static final Elevator elevator = new Elevator();
+    public static final Carriage carriage = new Carriage();
+    public static final ElevatorAuton elevatorAuton = new ElevatorAuton();
+    public static final Elevator elevator = new Elevator();
+    
+    
+    public static final LED led = new LED();
+
+    public static final Climber climber = new Climber();
+    public static final Pnumatics pnumatics = new Pnumatics();
+    //public static FlashyBlinky flashyBlinky;
+    //public static Sensors sensors;
+
+	public static final long periodMS = 10;
+	public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS);
+	//public static final PowerDistributionPanel pdp = new PowerDistributionPanel();
+
+	//public static final String ElevatorAuton = null;
+
+
+	public static enum OperationMode { TEST, COMPETITION };
+	public static OperationMode operationMode = OperationMode.TEST;
+
+	private SendableChooser<OperationMode> operationModeChooser;
+	private SendableChooser<Command> autonTaskChooser;
+
+    private Command autonomousCommand;
+    
+    /**
+     * This function is run when the robot is first started up and should be
+     * used for any initialization code.
+     */
+    public void robotInit() 
+    {
+    	//Printing game data to riolog
+    	String gameData = DriverStation.getInstance().getGameSpecificMessage();
+    	System.out.println(gameData);
+    	CameraServer.getInstance().startAutomaticCapture();
+    	CameraServer.getInstance().putVideo("res", 640, 480);
+    	
+      try {
+    	//drive = new Drive();
+    	//controlLoop = new ControlLooper("Main control loop", periodMS);
+		oi = OI.getInstance();
+		//camera.initialize();
+		
+    	controlLoop.addLoopable(drive);
+    	controlLoop.addLoopable(elevator);
+  
+//		Waypoint[] points = new Waypoint[] {
+//                new Waypoint(0, 0, 0),
+//                new Waypoint(-95, -9, Pathfinder.d2r(-27))
+//        };
+//
+//        PathGenerator boilerPath = new PathGenerator(points, 0.01, 120, 200.0, 700.0);			
+
+        operationModeChooser = new SendableChooser<OperationMode>();
+	    operationModeChooser.addDefault("Test", OperationMode.TEST);
+	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
+		SmartDashboard.putData("Operation Mode", operationModeChooser);
+		
+		autonTaskChooser = new SendableChooser<Command>();
+
+		//autonTaskChooser.addDefault("Blue Gear Loading Side Plus Travel Forward", new BlueGearLoadingSideForwardAuton());
+		autonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
+		//autonTaskChooser.addObject("Right Switch Forward", new RightSwitchForward());
+		autonTaskChooser.addDefault("RightStartLeftScore", new RightStartLeftScore());
+		
+		SmartDashboard.putData("Auton Task", autonTaskChooser);
+
+		
+		//ledLights.setAllLightsOn(false);
+      } catch (Exception e) {
+    		System.err.println("An error occurred in robotInit()");
+      }
+    }
+	
+	/**
+     * This function is called once each time the robot enters Disabled mode.
+     * You can use it to reset any subsystem information you want to clear when
+	 * the robot is disabled.
+     */
+    public void disabledInit(){
+
+    }
+	
+	public void disabledPeriodic() {
+		Scheduler.getInstance().run();
+		updateStatus();
+	}
+
+	/**
+	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
+	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
+	 * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
+	 * below the Gyro
+	 *
+	 * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
+	 * or additional comparisons to the switch structure below with additional strings & commands.
+	 */
+    public void autonomousInit() {    	
+    	updateChoosers();
+    	
+        // Schedule the autonomous command (example)
+    	controlLoop.start();
+    	drive.endGyroCalibration();
+    	drive.resetEncoders();
+    	//elevator.resetElevatorEncoder();
+    	drive.resetGyro();
+    	drive.setIsRed(getAlliance().equals(Alliance.Red));
+        if (autonomousCommand != null) autonomousCommand.start();
+    }
+
+    /**
+     * This function is called periodically during autonomous
+     */
+    public void autonomousPeriodic() {
+        Scheduler.getInstance().run();
+		updateStatus();
+    }
+
+    public void teleopInit() {
+		// This makes sure that the autonomous stops running when
+        // teleop starts running. If you want the autonomous to 
+        // continue until interrupted by another command, remove
+        // this line or comment it out.
+        if (autonomousCommand != null) autonomousCommand.cancel();
+        //MotionProfileCache.getInstance().release();
+    	updateChoosers();
+        controlLoop.start();
+    	drive.resetEncoders();
+    	drive.endGyroCalibration();
+    	//shooter.setStage1Speed(0);
+    	//shooter.setStage2Speed(0);
+    	//shooterFeed.setSpeed(0);
+    	//zarkerFeed.setSpeed(0);
+    	//shooter.setHopperPosition(HopperState.CLOSE);
+        updateStatus();
+    }
+
+    /**
+     * This function is called periodically during operator control
+     */
+    public void teleopPeriodic() 
+    {
+        Scheduler.getInstance().run();
+		updateStatus();
+    }
+    
+    /**
+     * This function is called periodically during test mode
+     */
+    public void testPeriodic() {
+        LiveWindow.run();
+		updateStatus();
+   }
+    
+    private void updateChoosers() {
+		operationMode = (OperationMode)operationModeChooser.getSelected();
+		autonomousCommand = (Command)autonTaskChooser.getSelected();
+    }
+    
+    public Alliance getAlliance() {
+    	return m_ds.getAlliance();
+    }
+    
+    public void updateStatus() {
+    	drive.updateStatus(operationMode);
+    	//carriage.updateStatus(operationMode);
+    	//shooter.updateStatus(operationMode);
+    	//shooterFeed.updateStatus(operationMode);
+    	//zarkerFeed.updateStatus(operationMode);
+    	//camera.updateStatus(operationMode);
+   }
+
+}
+

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

@@ -0,0 +1,48 @@
+
+package org.usfirst.frc4388.robot;
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+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;
+	
+	//Controller IDs
+
+	// 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_LEFT_MOTOR3_CAN_ID = 2;
+
+	public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
+	public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
+//	public static final int DRIVETRAIN_RIGHT_MOTOR3_CAN_ID = 13;
+	public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8;
+	public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9;
+	  
+	//Elevator Motors
+	public static final int ELEVATOR_MOTOR1_ID = 6;
+	public static final int ELEVATOR_MOTOR2_ID = 7;
+	public static final int CLIMBER_CAN_ID = 10;
+	
+	
+	// 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;
+	//public static final int HOPPER_POSITION_PCM_ID = 5;		
+	
+	// DIO
+	//public static final int HOPPER_SENSOR_RED_DIO_ID = 4;
+	//public static final int HOPPER_SENSOR_BLUE_DIO_ID = 5;
+
+	// Analog
+	//public static final int GEAR_SENSOR_ANALOG_ID = 0;
+}

src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java

@@ -0,0 +1,59 @@
+// RobotBuilder Version: 2.0
+//
+// This file was generated by RobotBuilder. It contains sections of
+// code that are automatically generated and assigned by robotbuilder.
+// These sections will be updated in the future when you export to
+// Java from RobotBuilder. Do not put any code or make any change in
+// the blocks indicating autogenerated code or it will be lost on an
+// update. Deleting the comments indicating the section will prevent
+// it from being updated in the future.
+
+
+package org.usfirst.frc4388.robot.commands;
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class AutonomousCommand extends Command {
+
+    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
+ 
+    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
+
+    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
+    public AutonomousCommand() {
+
+    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
+        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
+
+        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
+        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
+
+        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    }
+
+    // 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 false;
+    }
+
+    // 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() {
+    }
+}

src/org/usfirst/frc4388/robot/commands/CarriageIntakeSetDeploy.java

@@ -0,0 +1,74 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CarriageIntakeSetDeploy extends Command {
+
+	private IntakePosition position;
+	private double timeout;
+	private double secondTimeout;
+	private boolean isDeploy = false;
+	private boolean secondTimerSet = false;
+	
+	// Constructor with speed
+    public CarriageIntakeSetDeploy(double timeout, double secondTimeout, IntakePosition position) {
+    	this.position = position;
+    	this.timeout = timeout;
+    	this.secondTimeout = secondTimeout;
+   }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	isDeploy = false;
+    	secondTimerSet = false;
+    	if (position == IntakePosition.CUBE_INTAKE) {
+        	this.setTimeout(timeout);
+    		Robot.carriage.setWheelSpeed(Carriage.CUBE_INTAKE_SPEED);
+    		isDeploy = true;
+    	}
+    	else if (position == IntakePosition.CUBE_DEPLOY) {
+        	this.setTimeout(timeout);
+    		Robot.carriage.setWheelSpeed(Carriage.CUBE_EJECT_SPEED);
+    		isDeploy = true;
+    	}
+    	else {
+        	this.setTimeout(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() {
+        if (isDeploy == false) {
+        	return true;
+        }
+        
+        if (isTimedOut()) {
+        	return true;
+        }
+         
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	if (isDeploy == true) {
+    		Robot.carriage.setWheelSpeed(0);
+    	}
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

src/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DriveAbsoluteTurnMP extends Command
+{
+	private double absoluteTurnAngleDeg, maxTurnRateDegPerSec;
+	private MPSoftwareTurnType turnType;
+
+	public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) {
+		requires(Robot.drive);
+		this.absoluteTurnAngleDeg = absoluteTurnAngleDeg;
+		this.maxTurnRateDegPerSec = maxTurnRateDegPerSec;
+		this.turnType = turnType;
+	}
+
+	protected void initialize() {
+//		if (Robot.drive.isRed() == false) {
+//			absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1;
+//		}
+		Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType);
+	}
+
+	protected void execute() {
+	}
+
+	protected boolean isFinished() {
+		return Robot.drive.isFinished(); 
+	}
+
+	protected void end() {
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+	}
+
+	protected void interrupted() {
+		end();
+	}
+}

src/org/usfirst/frc4388/robot/commands/DriveGyroReset.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DriveGyroReset extends Command
+{
+	public DriveGyroReset() {
+		requires(Robot.drive);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.drive.resetGyro();
+		Robot.drive.resetEncoders();
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

src/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DriveRelativeTurnPID extends Command
+{
+	private double relativeTurnAngleDeg;
+	private MPSoftwareTurnType turnType;
+
+	public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) {
+		requires(Robot.drive);
+		this.relativeTurnAngleDeg = relativeTurnAngleDeg;
+		this.turnType = turnType;
+	}
+
+	protected void initialize() {
+		Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType);
+	}
+
+	protected void execute() {
+	}
+
+	protected boolean isFinished() {
+		return Robot.drive.isFinished(); 
+	}
+
+	protected void end() {
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+	}
+
+	protected void interrupted() {
+		end();
+	}
+}

src/org/usfirst/frc4388/robot/commands/DriveSpeedShift.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 DriveSpeedShift extends Command
+{
+	public boolean speed;
+	
+	public DriveSpeedShift(boolean speed) {
+		this.speed=speed;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setShiftState(speed);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

src/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java

@@ -0,0 +1,103 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+public class DriveStraightBasic extends Command {
+    private double m_targetInches;
+    private double m_maxVelocityInchesPerSec;
+    private double m_speed;
+    private boolean m_goingBackwards;
+    private boolean m_useGyroLock;
+    private boolean m_useAbsolute;
+    private double m_desiredAbsoluteAngle;
+    private double m_commandInitTimestamp;
+	private double m_lastCommandExecuteTimestamp = 0.0;
+	private double m_lastCommandExecutePeriod = 0.0;
+ 
+    public DriveStraightBasic(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
+        requires(Robot.drive);
+        m_targetInches = targetInches;
+        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
+        m_useGyroLock = useGyroLock;
+        m_useAbsolute = useAbsolute;
+        m_desiredAbsoluteAngle = desiredAbsoluteAngle;
+        m_goingBackwards = (m_targetInches < 0.0);
+   }
+    
+    protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) {
+    	double sign = (backwards ? -1.0 : 1.0);
+    	// Keep velocity above stiction limit (else bot will freeze and command will not complete)
+    	double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec);
+        // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick)
+    	return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec;
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.drive.resetGyro();
+    	Robot.drive.resetEncoders();
+    	Robot.drive.setControlMode(DriveControlMode.RAW);
+    	// start out at half speed, as crude way to reduce slippage
+        m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards);
+		m_commandInitTimestamp = Timer.getFPGATimestamp();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (m_lastCommandExecuteTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
+		}
+		m_lastCommandExecuteTimestamp = currentTimestamp;
+    	double steer = 0.0;
+    	if (m_useGyroLock) {
+    		steer = - Robot.drive.getGyroAngleDeg() / 16.0;	//TODO: tune
+    	}
+    	Robot.drive.rawMoveSteer(m_speed, steer);
+		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	boolean finished=false;
+    	double velocity = m_maxVelocityInchesPerSec;
+    	double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2;
+    	double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0);
+    	if (remaining < 0.0) {
+    		finished = true;
+    	} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
+    		velocity = m_maxVelocityInchesPerSec / 4.0;		// quarter speed
+    	} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) {		// last 300 ms
+    		velocity = m_maxVelocityInchesPerSec / 2.0;		// half speed
+    	}
+    	if (!finished) {
+    		m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
+    		SmartDashboard.putNumber("DSB Dist", position);
+    	} else {
+    		SmartDashboard.putNumber("DSB finDist", position);
+    	}
+		return finished;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+		double currentTimestamp = Timer.getFPGATimestamp();
+    	SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
+    	Robot.drive.rawMoveSteer(0.0, 0.0);
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

src/org/usfirst/frc4388/robot/commands/DriveStraightMP.java

@@ -0,0 +1,60 @@
+// RobotBuilder Version: 2.0
+//
+// This file was generated by RobotBuilder. It contains sections of
+// code that are automatically generated and assigned by robotbuilder.
+// These sections will be updated in the future when you export to
+// Java from RobotBuilder. Do not put any code or make any change in
+// the blocks indicating autogenerated code or it will be lost on an
+// update. Deleting the comments indicating the section will prevent
+// it from being updated in the future.
+
+
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+
+public class DriveStraightMP extends Command {
+    private double m_distanceInches;
+    private double m_maxVelocityInchesPerSec;
+    private boolean m_useGyroLock;
+    private boolean m_useAbsolute;
+    private double m_desiredAbsoluteAngle;
+ 
+    public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
+        requires(Robot.drive);
+        m_distanceInches = distanceInches;
+        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
+        m_useGyroLock = useGyroLock;
+        m_useAbsolute = useAbsolute;
+        m_desiredAbsoluteAngle = desiredAbsoluteAngle;
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+		Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+		return Robot.drive.isFinished(); 
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

src/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.CANTalonEncoder;
+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();
+	}
+}

src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java

@@ -0,0 +1,95 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+public class ElevatorBasic extends Command {
+    private double m_targetInches;
+    private double m_maxVelocityInchesPerSec;
+    private double m_speed;
+    private boolean m_goingBackwards;
+    private double m_commandInitTimestamp;
+	private double m_lastCommandExecuteTimestamp = 0.0;
+	private double m_lastCommandExecutePeriod = 0.0;
+ 
+    public ElevatorBasic(double targetInches, double maxVelocityInchesPerSec) {
+        requires(Robot.elevator);
+        m_targetInches = targetInches;
+        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
+        m_goingBackwards = (m_targetInches < 0.0);
+   }
+    
+    protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) {
+    	double sign = (backwards ? -1.0 : 1.0);
+    	// Keep velocity above stiction limit (else bot will freeze and command will not complete)
+    	double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec);
+        // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick)
+    	return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec;
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.elevator.resetElevatorEncoder();
+    	Robot.elevator.setControlMode(DriveControlMode.RAW);
+    	// start out at half speed, as crude way to reduce slippage
+        m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards);
+		m_commandInitTimestamp = Timer.getFPGATimestamp();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (m_lastCommandExecuteTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
+		}
+		m_lastCommandExecuteTimestamp = currentTimestamp;
+    	double steer = 0.0;
+
+    	
+    	Robot.elevator.rawMoveSteer(m_speed, steer);
+		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	boolean finished=false;
+    	double velocity = m_maxVelocityInchesPerSec;
+    	double position = (Robot.elevator.getelevatorPositionWorld());
+    	double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0);
+    	if (remaining < 0.0) {
+    		finished = true;
+    	} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
+    		velocity = m_maxVelocityInchesPerSec / 4.0;		// quarter speed
+    	} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) {		// last 300 ms
+    		velocity = m_maxVelocityInchesPerSec / 2.0;		// half speed
+    	}
+    	if (!finished) {
+    		m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
+    		SmartDashboard.putNumber("DSB Dist", position);
+    	} else {
+    		SmartDashboard.putNumber("DSB finDist", position);
+    	}
+		return finished;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+		double currentTimestamp = Timer.getFPGATimestamp();
+    	SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
+    	Robot.elevator.rawMoveSteer(0.0, 0.0);
+		Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java

@@ -0,0 +1,74 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
+import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ElevatorSetDeploy extends Command {
+
+	private IntakePosition position;
+	private double timeout;
+	private double secondTimeout;
+	private boolean isDeploy = false;
+	private boolean secondTimerSet = false;
+	
+	// Constructor with speed
+    public ElevatorSetDeploy(double timeout, double secondTimeout, IntakePosition position) {
+    	this.position = position;
+    	this.timeout = timeout;
+    	this.secondTimeout = secondTimeout;
+   }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	isDeploy = false;
+    	secondTimerSet = false;
+    	if (position == IntakePosition.CUBE_INTAKE) {
+        	this.setTimeout(timeout);
+    		Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED);
+    		isDeploy = true;
+    	}
+    	else if (position == IntakePosition.CUBE_DEPLOY) {
+        	this.setTimeout(timeout);
+    		Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED);
+    		isDeploy = true;
+    	}
+    	else {
+        	this.setTimeout(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() {
+        if (isDeploy == false) {
+        	return true;
+        }
+        
+        if (isTimedOut()) {
+        	return true;
+        }
+         
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	if (isDeploy == true) {
+    		Robot.elevatorAuton.setRaiseSpeed(0);
+    	}
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.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 ElevatorSetSpeed extends Command {
+
+	private double RaiseSpeed;
+	
+	// Constructor with speed
+    public ElevatorSetSpeed(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() {
+    }
+}

src/org/usfirst/frc4388/robot/commands/InitiateClimber.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class InitiateClimber extends Command
+{
+	boolean climb;
+	
+	public InitiateClimber(boolean climb) {
+		this.climb=climb;
+		requires(Robot.climber);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.climber.setClimbSpeed(climb);
+	}
+
+	@Override
+	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() {
+    }
+    
+	@Override
+	protected void interrupted() {
+			
+	}
+}

src/org/usfirst/frc4388/robot/commands/IntakePosition.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 IntakePosition extends Command
+{
+	public boolean position;
+	
+	public IntakePosition(boolean position) {
+		this.position=position;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setIntake(position);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

src/org/usfirst/frc4388/robot/commands/IntakeSetPosition.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class IntakeSetPosition extends CommandGroup {
+
+	public static enum IntakePosition { CUBE_INTAKE, CUBE_DEPLOY };
+
+    public IntakeSetPosition(IntakePosition intakePosition) {
+    	addParallel(new CarriageIntakeSetDeploy(0.5, 0.0, IntakePosition.CUBE_DEPLOY ));
+       	addSequential(new IntakeSetPosition(intakePosition));
+     }
+}

src/org/usfirst/frc4388/robot/commands/IntakeSetSpeed.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 IntakeSetSpeed extends Command {
+
+	private double WheelSpeed;
+	
+	// Constructor with speed
+    public IntakeSetSpeed(double WheelSpeed) {
+    	this.WheelSpeed = WheelSpeed;
+         requires(Robot.carriage);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.carriage.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() {
+    }
+}

src/org/usfirst/frc4388/robot/commands/LEDIndicators.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 LEDIndicators extends Command
+{
+	boolean Light;
+	
+	public LEDIndicators(boolean Light) {
+		this.Light=Light;
+		requires(Robot.led);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.led.setLight(Light);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java

@@ -0,0 +1,49 @@
+
+package org.usfirst.frc4388.robot.commands.auton;
+
+import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+//import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+//import org.usfirst.frc4388.robot.commands.IntakeSetPosition;
+//import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
+//import org.usfirst.frc4388.robot.commands.ShooterSetRpm;
+//import org.usfirst.frc4388.robot.commands.ShooterSetShotPosition;
+//import org.usfirst.frc4388.robot.commands.ShooterSetVoltageRampRate;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+//import org.usfirst.frc4388.robot.subsystems.Shooter;
+//import org.usfirst.frc4388.robot.subsystems.Drive.SpeedShiftState;
+//import org.usfirst.frc4388.robot.subsystems.Shooter.ShotState;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class BlueGearLoadingSideForwardAuton extends CommandGroup {
+    
+    public BlueGearLoadingSideForwardAuton() {
+    	addSequential(new DriveGyroReset());
+    	//addSequential(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+        //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
+        //addSequential(new DriveStraightMP(10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
+        //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
+       // addSequential(new WaitCommand(1.0));
+    	//addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -10));  // 95 for 112" greenville
+        //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
+       /// addSequential(new DriveStraightMP(-10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
+        
+       // addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, 10));
+        //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
+
+       // addSequential(new WaitCommand(0.3));
+       // addSequential(new DriveStraightMP(-20, Drive.MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -60));
+       // addSequential(new DriveAbsoluteTurnMP(-173, Drive.MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
+//        addSequential(new DriveSpeedShift(SpeedShiftState.HI));
+       // addSequential(new DriveStraightMP(-254, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -173));
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class RightStartLeftScore extends CommandGroup {
+    
+    public RightStartLeftScore() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-210, 60, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(150, 60, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(5, 60, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java

@@ -0,0 +1,43 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class RightSwitchAuton extends CommandGroup {
+    
+    public RightSwitchAuton() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-150, 60, true, true, 0));
+    	addSequential(new ElevatorSetSpeed(1.00));
+    	addSequential(new WaitCommand(.5));
+    	addSequential(new ElevatorSetSpeed(0.00));
+    	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(9, 60, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/RightSwitchForward.java

@@ -0,0 +1,44 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class RightSwitchForward extends CommandGroup {
+    
+    public RightSwitchForward() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-25, 25, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 178, 90, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveGyroReset());
+    	addSequential(new WaitCommand(.1));
+    	addSequential(new DriveStraightBasic(125, 30, true, true, 0));
+    	addSequential(new DriveGyroReset());
+    	addSequential(new WaitCommand(.1));
+    	addSequential(new DriveTurnBasic(true, -90, 90, MPSoftwareTurnType.TANK));
+    	//addSequential(new DriveStraightBasic(5, 20, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	addSequential(new WaitCommand(.6));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.9));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}

src/org/usfirst/frc4388/robot/subsystems/Carriage.java

@@ -0,0 +1,95 @@
+
+// RobotBuilder Version: 2.0
+//
+// This file was generated by RobotBuilder. It contains sections of
+// code that are automatically generated and assigned by robotbuilder.
+// These sections will be updated in the future when you export to
+// Java from RobotBuilder. Do not put any code or make any change in
+// the blocks indicating autogenerated code or it will be lost on an
+// update. Deleting the comments indicating the section will prevent
+// it from being updated in the future.
+
+
+package org.usfirst.frc4388.robot.subsystems;
+
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+
+import 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 Carriage extends Subsystem {
+
+	private WPI_TalonSRX carriageLeft;
+	private WPI_TalonSRX carriageRight;
+	public static enum CarriageControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL};
+	public static final double CUBE_INTAKE_SPEED = 0.40;
+	public static final double CUBE_EJECT_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 Carriage() {
+		try {
+			carriageLeft = new WPI_TalonSRX(RobotMap.CARRIAGE_LEFT_MOTOR_CAN_ID);
+			carriageRight = new WPI_TalonSRX(RobotMap.CARRIAGE_RIGHT_MOTOR_CAN_ID);
+			carriageRight.setInverted(true);
+			carriageLeft.setInverted(true);
+			//\][carriageLeft.set(CurrentLimit, value);
+			
+    carriageRight.set(ControlMode.Follower, carriageLeft.getDeviceID());
+		}
+		catch (Exception e) {
+			System.err.println("An error occurred in the Carriage constructor");
+			
+			
+		}
+    }
+	
+		public void setWheelSpeed(double speed) {
+			carriageLeft.set(-speed);
+    }
+
+    	
+    	
+    	
+
+
+      //  public synchronized void setControlMode(CarriageControlMode controlMode) {
+     	//	this.controlMode = controlMode;
+    	//	if (controlMode == CarriageControlMode.JOYSTICK) {
+    			
+    		//	carriageLeft.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+    		//	carriageRight.set(ControlMode.PercentOutput, 0);	
+    	//	}
+ //   }
+
+    @Override
+    public void periodic() {
+    	
+    }
+    	public void initDefaultCommand() {
+    }
+}
+
+
+
+
+

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

@@ -0,0 +1,68 @@
+
+
+
+
+package org.usfirst.frc4388.robot.subsystems;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.*;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+
+/**
+ *
+ */
+public class Climber extends Subsystem{
+	
+	private WPI_TalonSRX Climber;
+
+	public boolean on;
+	
+	public Climber(){
+		
+		try{
+			
+			Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID);
+			
+		} catch (Exception e) {
+			
+			System.err.println("An error occurred in the climbing constructor");
+			
+		}
+	}
+
+    @Override
+    public void initDefaultCommand() {
+
+    }
+
+
+    @Override
+    public void periodic() {
+        // Put code here to be run every loop
+
+    }
+    
+	public void setClimbSpeed(boolean Climb) {
+		if (Climb==true) {
+			Climber.set(0.95);
+		}
+		if (Climb==false) {
+			Climber.set(0);
+		}
+}
+    // Put methods for controlling this subsystem
+    // here. Call these from Commands.
+ {
+			// TODO Auto-generated method stub
+			
+		}
+
+}
+

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

@@ -0,0 +1,827 @@
+
+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.AdaptivePurePursuitController;
+import org.usfirst.frc4388.utility.BHRMathUtils;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.Kinematics;
+import org.usfirst.frc4388.utility.MMTalonPIDController;
+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.RigidTransform2d;
+import org.usfirst.frc4388.utility.Rotation2d;
+import org.usfirst.frc4388.utility.SoftwarePIDController;
+import org.usfirst.frc4388.utility.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 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<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
+
+	private CANTalonEncoder leftDrive1;
+	private WPI_TalonSRX leftDrive2;
+//	private WPI_TalonSRX leftDrive3;
+	
+	private CANTalonEncoder rightDrive1;
+	private WPI_TalonSRX rightDrive2;
+//	private WPI_TalonSRX rightDrive3;
+
+	private DifferentialDrive m_drive;
+	
+	//PID encoder and motor
+	private CANTalonEncoder elevatorRight;
+	private WPI_TalonSRX elevatorLeft;
+
+	//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 MMTalonPIDController mmStraightController;
+	private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24);  
+	
+	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.04;
+	private boolean isCalibrating = false;
+	private double gyroOffsetDeg = 0;
+
+	public Drive() {
+		try {
+			leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
+			leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID);
+//			leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID);
+			
+			rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder);
+			rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID);
+//			rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID);
+			
+			//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.clearStickyFaults(0);
+			leftDrive1.setInverted(false);//false on comp 2108, false on microbot
+			leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
+			leftDrive1.setSafetyEnabled(false);
+			//leftDrive1.setCurrentLimit(15);
+			//leftDrive1.enableCurrentLimit(true);
+			leftDrive1.setNeutralMode(NeutralMode.Brake);
+			leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
+			leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
+			leftDrive1.configNominalOutputForward(+1.0f, 0);
+			leftDrive1.configNominalOutputReverse(-1.0f, 0);
+
+			
+//	        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.setNeutralMode(NeutralMode.Brake);
+			leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID());
+	
+
+			
+			//rightDrive1.clearStickyFaults();
+			//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
+			//rightDrive1.setNominalClosedLoopVoltage(12.0);
+			rightDrive1.clearStickyFaults(0);
+			rightDrive1.setInverted(true);//true on comp 2108, false on microbot
+			rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
+			rightDrive1.setSafetyEnabled(false);
+			rightDrive1.setNeutralMode(NeutralMode.Brake);
+			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(true);//True on comp 2108, false on microbot
+			rightDrive2.setSafetyEnabled(false);
+			rightDrive2.setNeutralMode(NeutralMode.Brake);
+			rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
+
+			
+
+							
+			motorControllers.add(leftDrive1);
+			motorControllers.add(rightDrive1);
+
+			//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);
+		}
+		catch (Exception e) {
+			System.err.println("An error occurred in the DriveTrain constructor");
+		}
+	}
+
+	@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 setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
+		double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
+		mmStraightController.setPID(mmStraightPIDParams);
+		mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); 
+		setControlMode(DriveControlMode.MOTION_MAGIC);
+	}
+	
+	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 = leftDrive1.getPositionWorld();
+        double right_distance = rightDrive1.getPositionWorld();
+        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(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(ControlMode.PercentOutput, 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(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(ControlMode.PercentOutput, 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(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(ControlMode.PercentOutput, 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);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			leftDrive1.set(ControlMode.Position, 0);
+			rightDrive1.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			rightDrive1.set(ControlMode.Position, 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.MOTION_MAGIC) {
+				isFinished = mmStraightController.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;
+		}
+		m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
+				m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
+		
+		if (useGyroLock) {
+			double yawError = gyroLockAngleDeg - getGyroAngleDeg();
+			m_steerOutput = kPGyro * yawError;
+		}
+		
+		m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.85);
+		// 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(ControlMode.PercentOutput, leftPercentOutput);
+		rightDrive1.set(ControlMode.PercentOutput, 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) {
+		//mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers);
+		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 leftDrive1.getPositionWorld();
+    }
+
+    public double getRightPositionWorld() {
+    	return rightDrive1.getPositionWorld();
+    }
+
+	public void updateStatus(Robot.OperationMode operationMode) {
+		if (operationMode == Robot.OperationMode.TEST) {
+			try {
+				SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
+				SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0));
+				SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0));
+				SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12);
+				SmartDashboard.putNumber("Left Vel Ft-Sec", 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", getGyroAngleDeg());
+				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);
+			}
+			catch (Exception e) {
+				System.err.println("Drivetrain update status error");
+			}
+		}
+		else {
+			
+		}
+	}	
+	
+}

src/org/usfirst/frc4388/robot/subsystems/Elevator.java

@@ -0,0 +1,402 @@
+package org.usfirst.frc4388.robot.subsystems;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.controller.XboxController;
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+import org.usfirst.frc4388.utility.PIDParams;
+import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.SensorCollection;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class Elevator extends Subsystem implements ControlLoopable
+{
+	//PID encoder and motor
+	private CANTalonEncoder elevatorRight;
+	private WPI_TalonSRX elevatorLeft;
+
+	//PID controller Max Scale
+	private SoftwarePIDPositionController pidPositionControllerMaxScale;
+	//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
+	private PIDParams PositionPMaxScale;
+	
+	//PID controller Max Scale
+	private SoftwarePIDPositionController pidPositionControllerLowScale;
+	//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
+	private PIDParams PositionPLowScale;
+	
+	//PID controller Max Scale
+	private SoftwarePIDPositionController pidPositionControllerSwitch;
+	//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
+	private PIDParams PositionPSwitch;
+	
+	//PID controller Max Scale
+	private SoftwarePIDPositionController pidPositionControllerLowest;
+	//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
+	private PIDParams PositionPLowest;
+
+	//PID target
+	private double targetPPosition;
+	
+	//Encoder ticks to inches for encoders
+	public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
+	
+	//control mode for joystick control
+	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
+	
+	private double periodMs;
+	
+	//Non Linear Joystick
+	public static final double STICK_DEADBAND = 0.02;
+	public static final double MOVE_NON_LINEARITY = 1.0;
+	private int moveNonLinear = 0;
+	private double moveScale = 1.0;
+	private double moveTrim = 0.0;
+	
+	private boolean isFinished;
+	private DifferentialDrive m_drive;
+	
+	//private LimitSwitchSource limitSwitch = new DigitalInput(1);
+	LimitSwitchSource limitSwitchSource;
+	
+	public boolean pressed;
+	SensorCollection isPressed;
+	
+	// Motor controllers
+	//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
+    
+    public Elevator()
+    {    	
+    	try
+    	{
+			//PID elevator encoder and talon
+			elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
+			elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
+			
+			//Setting right elevator motor as follower
+    		elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
+    		elevatorLeft.setInverted(false);
+    		elevatorRight.setInverted(false);
+    		
+    		//Limit Switch Left
+    		elevatorLeft.overrideLimitSwitchesEnable(true);
+    		elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+    		elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+    		
+    		//Limit Switch Right
+    		elevatorRight.overrideLimitSwitchesEnable(true);
+			elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+    		elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+
+    	}
+    	catch(Exception e)
+    	{
+    		System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor");
+    	}
+    }
+    
+    private double nonlinearStickCalcPositive(double move, double moveNonLinearity) 
+    {
+		return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
+	}
+    
+    private double nonlinearStickCalcNegative(double move, double moveNonLinearity) 
+    {
+		return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
+	}
+    
+    private boolean inDeadZone(double input) 
+    {
+		boolean inDeadZone;
+		if (Math.abs(input) < STICK_DEADBAND)
+		{
+			inDeadZone = true;
+		} 
+		else 
+		{
+			inDeadZone = false;
+		}
+		
+		return inDeadZone;
+	}
+    
+    private double limitValue(double value) 
+    {
+		if (value > 1.0)
+		{
+			value = 1.0;
+		}
+		else if (value < -1.0)
+		{
+			value = -1.0;
+		}
+		return value;
+	}
+    
+    public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) 
+    {
+		if (inDeadZone(move))
+		{
+			return 0;
+		}
+
+		move += trim;
+		move *= scale;
+		move = limitValue(move);
+
+		int iterations = Math.abs(nonLinearFactor);
+		for (int i = 0; i < iterations; i++)
+		{
+			if (nonLinearFactor > 0)
+			{
+				move = nonlinearStickCalcPositive(move, wheelNonLinearity);
+			} 
+			else
+			{
+				move = nonlinearStickCalcNegative(move, wheelNonLinearity);
+			}
+		}
+		return move;
+	}
+    
+    public void setElevatorMode()
+    {
+    	setControlMode(DriveControlMode.JOYSTICK);
+    }
+    
+    public void resetElevatorEncoder()
+    {
+    	elevatorRight.setSelectedSensorPosition(0, 0, 0);
+    }
+    
+    public void moveElevatorXbox()
+    {
+    	double moveElevatorInput;
+    	double elevatorSafeZone = 1.2; 
+    	
+    	double elevatorPos = getEncoderElevatorPosition();
+    	
+    	moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
+    	
+    	//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
+    	
+    	boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
+    	boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
+    	
+    	if(elevatorTuningPressed == true)
+      	{     		
+     		elevatorRight.set(moveElevatorInput * (0.5));
+      	}
+     	else if(elevatorTuningPressed == false)
+     	{
+     		if(elevatorPos <= elevatorSafeZone /*&& elevatorPos >= 0*/)
+     		{
+     			elevatorRight.set(moveElevatorInput * 0.65);
+     		}
+     		else if(elevatorPos > elevatorSafeZone)
+     		{
+     			elevatorRight.set(moveElevatorInput);
+     			
+     			/*
+     			if(holdButtonPressed == true)
+             	{
+             		elevatorRight.set(-0.43 * (0.2));
+             	}
+             	else if(holdButtonPressed == false)
+             	{
+             		elevatorRight.set(moveElevatorInput * 0.75);
+             	}
+             	*/
+     		}
+     		/*
+     		else if(elevatorPos < 0)
+     		{
+             	elevatorRight.set(moveElevatorInput * 0.75);
+     		}
+     		*/
+      	}
+     	
+     	System.out.println(elevatorPos);		//-6.9 to 1.9   total: 8.8 range
+    }
+    
+	//PID encoder position
+	public double getEncoderElevatorPosition()
+	{
+		return elevatorRight.getPositionWorld();
+	}
+	
+	public synchronized void setControlMode(DriveControlMode controlMode) 
+	{
+ 		this.controlMode = controlMode;
+ 		
+ 		isFinished = false;
+	}
+	/*
+	public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError)
+	{
+		double elevatorTargetPos = 0;
+		this.targetPPosition = elevatorTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);      ///////TARGET POSITION WHERE??
+		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
+	}
+	
+	public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError)
+	{
+		double elevatorTargetPos = 0;
+		this.targetPPosition = elevatorTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
+		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
+	}
+	
+	public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError)
+	{
+		double elevatorTargetPos = 0;
+		this.targetPPosition = elevatorTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
+		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
+	}
+	
+	public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError)
+	{
+		double elevatorTargetPos = 0;
+		this.targetPPosition = elevatorTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
+		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
+	}
+	*/
+	public void holdInPos()
+	{
+		elevatorRight.set(-0.43 * 0.2);
+	}
+	
+	public void stopMotors()
+	{
+		elevatorRight.set(0);
+	}
+	
+	public void isSwitchPressed()
+	{
+		pressed = false;
+		isPressed = elevatorRight.getSensorCollection();
+		
+		if(isPressed.isFwdLimitSwitchClosed() == true)
+		{
+			Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
+			pressed = true;
+		}
+		else
+		{
+			Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
+			pressed = false;
+		}
+		
+		//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
+	}
+	
+	
+	
+    public double getelevatorPositionWorld() {
+    	return elevatorRight.getPositionWorld();
+    }
+	@Override
+	public void controlLoopUpdate() 
+	{
+		if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) 
+		{
+			moveElevatorXbox();
+		}
+		else if (!isFinished)
+		{
+			//PID control mode
+			if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
+			{
+				isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition());
+			}
+			else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
+			{
+				isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition());
+			}
+			else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
+			{
+				isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition());
+			}
+			else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
+			{
+				isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
+			}
+		}
+	}
+
+	@Override
+    public void initDefaultCommand()
+    {
+    }
+
+    @Override
+    public void periodic() 
+    {
+    	isSwitchPressed();
+    }
+    
+	@Override
+	public void setPeriodMs(long periodMs)
+	{
+		//PID controller
+		pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight);
+		pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight);
+		pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight);
+		pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight);
+		
+		this.periodMs = periodMs;
+	}
+	
+	public synchronized boolean isFinished() 
+	{
+		return isFinished;
+	}
+	
+	public double getPeriodMs()
+	{
+		return periodMs;
+	}
+	
+	public void updateStatus(Robot.OperationMode operationMode) 
+	{
+		if (operationMode == Robot.OperationMode.TEST) 
+		{
+			try 
+			{
+				//SmartDashboard.putData(pressed);
+			}
+			catch (Exception e) 
+			{
+				System.err.println("Drivetrain update status error");
+			}
+		}
+	}
+	public void rawMoveSteer(double move, double steer) {
+		m_drive.arcadeDrive(move, steer, false);
+	}
+	
+}
+

src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java

@@ -0,0 +1,120 @@
+
+package org.usfirst.frc4388.robot.subsystems;
+
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import org.usfirst.frc4388.robot.OI;
+import org.usfirst.frc4388.robot.Robot;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.SensorCollection;
+
+
+
+/**
+ *
+ */
+public class ElevatorAuton extends Subsystem {
+
+	private WPI_TalonSRX ElevatorRight;
+	private WPI_TalonSRX ElevatorLeft;
+	public static final double RAISE_ELEVATOR_SPEED = 1;
+	public static final double LOWER_ELEVATOR_SPEED = -1;
+	public static final double STOP_ELEVATOR_SPEED = 0;
+	LimitSwitchSource limitSwitchSource;
+	public boolean pressed;
+	SensorCollection isPressed;
+	/////^^^^^^^^^ replace this line with the modes we need
+	
+	
+	private boolean isFinished;
+	//private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK;
+	private double PeriodMs;
+	
+	
+	
+	
+	public ElevatorAuton() {
+		try {
+			ElevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR1_ID);
+			ElevatorRight = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
+			ElevatorRight.setInverted(false);
+			ElevatorLeft.setInverted(false);
+			
+			
+		
+			
+			//carriageLeft.set(CurrentLimit, value);
+			
+			ElevatorRight.set(ControlMode.Follower, ElevatorLeft.getDeviceID());
+		}
+		catch (Exception e) {
+			System.err.println("An error occurred in the elevator constructor");
+			
+			
+		}
+    }
+	
+	public void setRaiseSpeed(double raiseSpeed) {
+			ElevatorLeft.set(-raiseSpeed);
+    }
+		
+      //  public synchronized void setControlMode(CarriageControlMode controlMode) {
+     	//	this.controlMode = controlMode;
+    	//	if (controlMode == CarriageControlMode.JOYSTICK) {
+    			
+    		//	carriageLeft.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+    		//	carriageRight.set(ControlMode.PercentOutput, 0);	
+    	//	}
+ //   }
+
+    	
+
+		@Override
+		/*public void controlLoopUpdate() {
+			pressed = false;
+    		isPressed = ElevatorLeft.getSensorCollection();
+			
+			elevator_input = Robot.oi.getOperatorJoystick().getRawAxis(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
+			System.err.println(isPressed.isFwdLimitSwitchClosed());
+			if (isPressed.isFwdLimitSwitchClosed() == true) {
+				ElevatorLeft.set(0);
+				ElevatorRight.set(0);
+				pressed = true;
+			} else {
+
+				ElevatorLeft.set(elevator_input);
+				ElevatorRight.set(elevator_input);
+				pressed = false;
+			}
+		}
+		*/
+
+	    public void periodic() {
+	   
+	    }
+
+
+		@Override
+	//	public void setPeriodMs(long periodMs) {
+		//	this.PeriodMs = periodMs;
+	//	}
+    	public void initDefaultCommand() {
+    	}
+    	public void isFinished() {
+    	}
+    	
+    	
+
+		
+}
+

src/org/usfirst/frc4388/robot/subsystems/LED.java

@@ -0,0 +1,36 @@
+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 LED extends Subsystem {
+    
+		
+	private DoubleSolenoid Light;
+
+	
+	public LED() {
+		try {
+			Light = new DoubleSolenoid(1,2,3);					
+		} 
+		catch (Exception e) {
+			System.err.println("An error occurred in the LED constructor");
+		}
+	}
+	
+	public void setLight(boolean state) {
+		if (state==true) {
+			Light.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			Light.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+
+
+	public void initDefaultCommand() {
+    }
+}

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

@@ -0,0 +1,46 @@
+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 Intake;
+
+	
+	public Pnumatics() {
+		try {
+			speedShift = new DoubleSolenoid(1,0,1);					
+			Intake = new DoubleSolenoid(1,4,5 ); 
+		} 
+		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 setIntake(boolean state) {
+		if (state==true) {
+			Intake.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			Intake.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+
+	public void initDefaultCommand() {
+    }
+}
+

src/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java

@@ -0,0 +1,228 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+import java.util.Optional;
+import java.util.Set;
+
+import org.usfirst.frc4388.robot.Constants;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * Implements an adaptive pure pursuit controller. See:
+ * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4
+ * .pdf
+ * 
+ * Basically, we find a spot on the path we'd like to follow and calculate the
+ * wheel speeds necessary to make us land on that spot. The target spot is a
+ * specified distance ahead of us, and we look further ahead the greater our
+ * tracking error.
+ */
+public class AdaptivePurePursuitController {
+    private static final double kEpsilon = 1E-9;
+
+    double mFixedLookahead;
+    Path mPath;
+    RigidTransform2d.Delta mLastCommand;
+    double mLastTime;
+    double mMaxAccel;
+    double mDt;
+    boolean mReversed;
+    double mPathCompletionTolerance;
+
+	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	
+	public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
+			//motorController.setF(pidParams.kF);
+			//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
+			//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
+			//motorController.setProfile(0);
+			motorController.config_kP(0, pidParams.kP, 0);
+			motorController.config_kI(0, pidParams.kI, 0);
+			motorController.config_kD(0, pidParams.kD, 0);
+			motorController.config_kF(0, pidParams.kF, 0);
+			motorController.configNominalOutputForward(+0.0f, 0);
+			motorController.configNominalOutputReverse(-0.0f, 0);
+			motorController.configPeakOutputForward(+1.0f, 0);
+			motorController.configPeakOutputReverse(-1.0f, 0);
+			motorController.selectProfileSlot(0, 0);
+		}
+	}
+	
+    public void setPath(double fixed_lookahead, double max_accel, Path path,
+            boolean reversed, double path_completion_tolerance) {
+        mFixedLookahead = fixed_lookahead;
+        mMaxAccel = max_accel;
+        mPath = path;
+        mDt = periodMs;
+        mLastCommand = null;
+        mReversed = reversed;
+        mPathCompletionTolerance = path_completion_tolerance;
+
+        // Set up the motion profile 
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Speed);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Velocity, 0);
+		}
+    }
+
+    public boolean isDone() {
+        double remainingLength = mPath.getRemainingLength();
+        return remainingLength <= mPathCompletionTolerance;
+    }
+
+	public boolean controlLoopUpdate(RigidTransform2d robot_pose) {
+        RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp());
+        Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command);
+
+        // Scale the command to respect the max velocity limits
+        double max_vel = 0.0;
+        max_vel = Math.max(max_vel, Math.abs(setpoint.left));
+        max_vel = Math.max(max_vel, Math.abs(setpoint.right));
+        if (max_vel > Constants.kPathFollowingMaxVel) {
+            double scaling = Constants.kPathFollowingMaxVel / max_vel;
+            setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling);
+        }
+
+        // Update the controllers Kf and set point.
+		for (CANTalonEncoder motorController : motorControllers) {
+			if (motorController.isRight()) {
+				motorController.setVelocityWorld(-setpoint.right);
+			}
+			else {
+				motorController.setVelocityWorld(-setpoint.left);
+			}
+		}
+		
+		return isDone();
+	}
+
+	public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) {
+        RigidTransform2d pose = robot_pose;
+        if (mReversed) {
+            pose = new RigidTransform2d(robot_pose.getTranslation(),
+                    robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
+        }
+
+        double distance_from_path = mPath.update(robot_pose.getTranslation());
+        if (this.isDone()) {
+            return new RigidTransform2d.Delta(0, 0, 0);
+        }
+
+        PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(),
+                distance_from_path + mFixedLookahead);
+        Optional<Circle> circle = joinPath(pose, lookahead_point.translation);
+
+        double speed = lookahead_point.speed;
+        if (mReversed) {
+            speed *= -1;
+        }
+        // Ensure we don't accelerate too fast from the previous command
+        double dt = now - mLastTime;
+        if (mLastCommand == null) {
+            mLastCommand = new RigidTransform2d.Delta(0, 0, 0);
+            dt = mDt;
+        }
+        double accel = (speed - mLastCommand.dx) / dt;
+        if (accel < -mMaxAccel) {
+            speed = mLastCommand.dx - mMaxAccel * dt;
+        } else if (accel > mMaxAccel) {
+            speed = mLastCommand.dx + mMaxAccel * dt;
+        }
+
+        // Ensure we slow down in time to stop
+        // vf^2 = v^2 + 2*a*d
+        // 0 = v^2 + 2*a*d
+        double remaining_distance = mPath.getRemainingLength();
+        double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance);
+        if (Math.abs(speed) > max_allowed_speed) {
+            speed = max_allowed_speed * Math.signum(speed);
+        }
+        final double kMinSpeed = 4.0;
+        if (Math.abs(speed) < kMinSpeed) {
+            // Hack for dealing with problems tracking very low speeds with
+            // Talons
+            speed = kMinSpeed * Math.signum(speed);
+        }
+
+        RigidTransform2d.Delta rv;
+        if (circle.isPresent()) {
+            rv = new RigidTransform2d.Delta(speed, 0,
+                    (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius);
+        } else {
+            rv = new RigidTransform2d.Delta(speed, 0, 0);
+        }
+        mLastTime = now;
+        mLastCommand = rv;
+        return rv;
+    }
+
+    public Set<String> getMarkersCrossed() {
+        return mPath.getMarkersCrossed();
+    }
+
+    public static class Circle {
+        public final Translation2d center;
+        public final double radius;
+        public final boolean turn_right;
+
+        public Circle(Translation2d center, double radius, boolean turn_right) {
+            this.center = center;
+            this.radius = radius;
+            this.turn_right = turn_right;
+        }
+    }
+
+    public static Optional<Circle> joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) {
+        double x1 = robot_pose.getTranslation().getX();
+        double y1 = robot_pose.getTranslation().getY();
+        double x2 = lookahead_point.getX();
+        double y2 = lookahead_point.getY();
+
+        Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point);
+        double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin()
+                - pose_to_lookahead.getY() * robot_pose.getRotation().cos();
+        if (Math.abs(cross_product) < kEpsilon) {
+            return Optional.empty();
+        }
+
+        double dx = x1 - x2;
+        double dy = y1 - y2;
+        double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos();
+        double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin();
+
+        double cross_term = mx * dx + my * dy;
+
+        if (Math.abs(cross_term) < kEpsilon) {
+            // Points are colinear
+            return Optional.empty();
+        }
+
+        return Optional.of(new Circle(
+                new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term),
+                        (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)),
+                .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0));
+    }
+
+}

src/org/usfirst/frc4388/utility/BHRMathUtils.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc4388.utility;
+
+public class BHRMathUtils {
+
+	public static double normalizeAngle0To360(double currentAccumAngle) {
+		// reduce the angle  
+		double normalizedAngle = currentAccumAngle % 360; 
+
+		// force it to be the positive remainder, so that 0 <= angle < 360  
+		normalizedAngle = (normalizedAngle + 360) % 360;  
+		
+		return normalizedAngle;
+	}
+
+	public static double adjustAccumAngleToDesired(double currentAccumAngle, double desiredAngle0To360) {
+		double normalizedAngle = normalizeAngle0To360(currentAccumAngle); 
+
+		if ( Math.abs(normalizedAngle - desiredAngle0To360) <= 180) {
+			double deltaAngle = normalizedAngle - desiredAngle0To360;
+			return currentAccumAngle - deltaAngle;
+		}
+		else {
+			double deltaAngle = desiredAngle0To360 - normalizedAngle;
+			if (deltaAngle < 0) {
+				deltaAngle += 360;
+			}
+			else {
+				deltaAngle -= 360;
+			}
+			return currentAccumAngle + deltaAngle;
+		}
+	}
+
+	public static double adjustAccumAngleToClosest180(double currentAccumAngle) {
+		double normalizedAngle = Math.abs(normalizeAngle0To360(currentAccumAngle)); 
+
+		if (normalizedAngle < 90 || normalizedAngle > 270) {
+			return adjustAccumAngleToDesired(currentAccumAngle, 0);
+		}
+		else {
+			return adjustAccumAngleToDesired(currentAccumAngle, 180);
+		}
+	}
+		
+	public static void main(String[] args) {
+		System.out.println("Accum angle to desired 721 to 45 = " + adjustAccumAngleToDesired(721,  45));
+	}
+}

src/org/usfirst/frc4388/utility/CANTalonEncoder.java

@@ -0,0 +1,63 @@
+package org.usfirst.frc4388.utility;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class CANTalonEncoder extends WPI_TalonSRX
+{
+	private double encoderTicksToWorld;
+	private boolean isRight = true;
+	
+	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
+		this(deviceNumber, encoderTicksToWorld, false, feedbackDevice);
+	}
+
+	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
+		super(deviceNumber);
+		//this.setFeedbackDevice(feedbackDevice);
+		this.configSelectedFeedbackSensor(feedbackDevice, 0, 0);
+		this.encoderTicksToWorld = encoderTicksToWorld;
+		this.isRight = isRight;
+	}
+
+    public boolean isRight() {
+		return isRight;
+	}
+
+	public void setRight(boolean isRight) {
+		this.isRight = isRight;
+	}
+
+	public double convertEncoderTicksToWorld(double encoderTicks) {
+    	return encoderTicks / encoderTicksToWorld;
+    }
+
+    public double convertEncoderWorldToTicks(double worldValue) {
+    	return worldValue * encoderTicksToWorld;
+    }
+    
+    public void setWorld(double worldValue) {
+    	//this.set(convertEncoderWorldToTicks(worldValue));
+    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue));		//TODO: change to explicit mode set?
+    }
+    
+    public void setPositionWorld(double worldValue) {
+    	//this.setPosition(convertEncoderWorldToTicks(worldValue));
+    	this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0);	//TODO: verify
+    }
+    
+    public double getPositionWorld() {
+    	//return convertEncoderTicksToWorld(this.getPosition());
+    	return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0));	//TODO: verify want 0="Primary closed-loop"
+    }
+    
+    public void setVelocityWorld(double worldValue) {
+    	//this.set(convertEncoderWorldToTicks(worldValue) * 0.1);
+    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1);	//TODO: change to explicit mode set?
+    }
+    
+    public double getVelocityWorld() {
+    	//return convertEncoderTicksToWorld(this.getSpeed() / 0.1);
+    	return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1);	//TODO: verify want 0="Primary closed-loop"
+    }
+}

src/org/usfirst/frc4388/utility/ConstantsBase.java

@@ -0,0 +1,199 @@
+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.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 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);
+                    }
+                } 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() - 1;
+        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();
+        }
+    }
+
+}

src/org/usfirst/frc4388/utility/ControlLoopable.java

@@ -0,0 +1,7 @@
+package org.usfirst.frc4388.utility;
+
+public interface ControlLoopable 
+{
+	public void controlLoopUpdate();
+	public void setPeriodMs(long periodMs);
+}

src/org/usfirst/frc4388/utility/ControlLooper.java

@@ -0,0 +1,79 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.Timer;
+import java.util.TimerTask;
+import java.util.Vector;
+
+/**
+ * ControlLooper.java
+ * <p>
+ * Runs several ControlLoopables simultaneously with one timing loop.
+ * Useful for running a bunch of control loops
+ * with only one Thread worth of overhead.
+ * 
+ * Shamelessly stolen from team 254 2015 code and then slightly modified
+ *
+ * @author Tom Bottiglieri
+ */
+
+public class ControlLooper 
+{
+	private Timer controlLoopTimer;	
+    private Vector<ControlLoopable> loopables = new Vector<ControlLoopable>();
+	private long periodMs;
+	private String name;
+
+	public ControlLooper(String name, long periodMs) {
+		this.name = name;
+		this.periodMs = periodMs;
+	}
+	
+	private class ControlLoopTask extends TimerTask {
+		private ControlLooper controlLooper;
+
+		public ControlLoopTask(ControlLooper controlLooper) {
+			if (controlLooper == null) {
+				throw new NullPointerException("Given control looper was null");
+			}
+			this.controlLooper = controlLooper;
+		}
+
+		@Override
+		public void run() {
+			controlLooper.controlLoopUpdate();
+		}
+		
+	}
+	
+	public String getName() {
+		return name;
+	}
+	
+	public void start() {
+		if (controlLoopTimer == null) {
+			controlLoopTimer = new Timer();
+			controlLoopTimer.schedule(new ControlLoopTask(this), 0L, periodMs);
+		}
+	}
+	
+	public void stop() {
+		if (controlLoopTimer != null) {
+			controlLoopTimer.cancel();
+			controlLoopTimer = null;
+		}
+	}
+		
+	private void controlLoopUpdate() {
+        for (int i = 0; i < loopables.size(); ++i) {
+        	ControlLoopable c = loopables.elementAt(i);
+            if (c != null) {
+            	c.controlLoopUpdate();
+            }
+        }
+	}
+	
+    public void addLoopable(ControlLoopable c) {
+        loopables.addElement(c);
+        c.setPeriodMs(periodMs);
+    }
+}

src/org/usfirst/frc4388/utility/FalconLinePlot.java

@@ -0,0 +1,894 @@
+package org.usfirst.frc4388.utility;
+
+import java.awt.AWTException;
+import java.awt.BasicStroke;
+import java.awt.Color;
+import java.awt.Dimension;
+import java.awt.FontMetrics;
+import java.awt.Graphics;
+import java.awt.Graphics2D;
+import java.awt.Image;
+import java.awt.Rectangle;
+import java.awt.RenderingHints;
+import java.awt.Robot;
+import java.awt.Stroke;
+import java.awt.Toolkit;
+import java.awt.datatransfer.Clipboard;
+import java.awt.datatransfer.ClipboardOwner;
+import java.awt.datatransfer.DataFlavor;
+import java.awt.datatransfer.Transferable;
+import java.awt.datatransfer.UnsupportedFlavorException;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+import java.awt.event.MouseAdapter;
+import java.awt.event.MouseEvent;
+import java.awt.geom.AffineTransform;
+import java.awt.geom.Ellipse2D;
+import java.awt.geom.Line2D;
+import java.awt.image.BufferedImage;
+import java.io.IOException;
+import java.text.DecimalFormat;
+import java.util.LinkedList;
+
+import javax.swing.JFrame;
+import javax.swing.JMenuItem;
+import javax.swing.JPanel;
+import javax.swing.JPopupMenu;
+
+/**
+ * This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user 
+ * to plot multiple graphs on one figure, control axis dimensions, and specify colors.
+ * 
+ * This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If
+ * a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this
+ * class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user
+ * control over as much as possible, so they can generate the perfect chart.
+ * 
+ * The plotter also features the ability to capture screen shots directly from the right-click menu, this allows
+ * the user to copy and paste plots into reports or other documents rather quickly.
+ * 
+ * This class holds an interface similar to that of Matlab. 
+ * 
+ * This class currently only supports scatterd line charts.
+ * 
+ * @author Kevin Harrilal
+ * @email kevin@team2168.org
+ * @version 1
+ * @date 9 Sept 2014
+ *
+ */
+ 
+class FalconLinePlot extends JPanel implements ClipboardOwner{
+  
+    
+	private static final long serialVersionUID = 3205256608145459434L;
+	private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge
+    private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge
+    
+    private double upperXtic;
+    private double lowerXtic;
+    private double upperYtic;
+    private double lowerYtic;
+    private boolean yGrid;
+    private boolean xGrid;
+    
+    private double yMax;
+    private double yMin;
+    private double xMax;
+    private double xMin;
+    
+    private int yticCount;
+    private int xticCount;
+    private double xTicStepSize;
+    private double yTicStepSize;
+    
+    boolean userSetYTic;
+    boolean userSetXTic;
+    
+    private String xLabel;
+    private String yLabel;
+    private String titleLabel;
+    protected static int count = 0;
+    
+    JPopupMenu menu = new JPopupMenu("Popup");
+    
+    //Link List to hold all different plots on one graph.
+    private LinkedList<xyNode> link; 
+    
+
+    /**
+     * Constructor which Plots only Y-axis data.
+     * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
+     */
+    public FalconLinePlot(double[] yData)
+    {
+    	this(null,yData,Color.red);
+    }
+    
+    public FalconLinePlot(double[] yData,Color lineColor, Color marker)
+    {
+    	this(null,yData,lineColor,marker);
+    }
+    
+    /**
+     * Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length.
+     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
+     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
+     */
+    public FalconLinePlot(double[] xData, double[] yData)
+    {
+    	this(xData,yData,Color.red,null);
+    }
+    
+    /**
+     * Constructor which Plots chart based on provided x and y axis data. 
+     * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
+     * is the second dimension.
+     */
+    public FalconLinePlot(double[][] data)
+    {
+    	this(getXVector(data),getYVector(data),Color.red,null);
+    }
+    
+/**
+ * Constructor which plots charts based on provided x and y axis data in a single two dimensional array.
+ * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
+ * is the second dimension.
+ * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
+ * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
+ * not have datapoint markers.
+ */
+    public FalconLinePlot(double[][] data, Color lineColor, Color markerColor)
+    {
+    	this(getXVector(data),getYVector(data),lineColor,markerColor);
+    }
+    
+    /**
+     * Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line.
+     * Data markers are not displayed.
+     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
+     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
+     * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
+     */
+    public FalconLinePlot(double[] xData, double[] yData,Color lineColor)
+    {
+    	this(xData,yData,lineColor,null);
+    }
+    
+
+    
+    /**
+     * Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user 
+     * can also specify the color of the adjoining line and the color of the datapoint maker.
+     * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
+     * @param yData is an array of double representing the Y-axis values of the data to be plotted.
+     * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
+     * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
+     * not have datapoint markers.
+     */
+    public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor)
+    {
+    	xLabel = "X axis";
+    	yLabel = "Y axis";
+    	titleLabel = "Title";
+    	
+       upperXtic = -Double.MAX_VALUE;
+       lowerXtic = Double.MAX_VALUE;
+       upperYtic = -Double.MAX_VALUE;
+       lowerYtic = Double.MAX_VALUE;
+       xticCount = -Integer.MAX_VALUE;
+       
+       this.userSetXTic = false;
+       this.userSetYTic = false;
+    	
+    	link = new LinkedList<xyNode>();
+    	
+    	addData(xData, yData, lineColor,markerColor);
+    	
+    	count ++;
+    	JFrame g = new JFrame("Figure " + count);
+        g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+        g.add(this);
+        g.setSize(600,400);
+        g.setLocationByPlatform(true);
+        g.setVisible(true);
+         
+        menu(g,this);	
+    }
+    
+    /**
+     * Adds a plot to an existing figure.  
+     * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
+     * @param color is the color the user wishes to be displayed for the line connecting each datapoint
+     */
+    
+    public void addData(double[] y, Color lineColor)
+    {
+    	addData(y, lineColor, null);
+    }
+    
+    public void addData(double[] y, Color lineColor, Color marker)
+    {
+    	//cant add y only data unless all other data is y only data
+    	for(xyNode data: link)
+    		if(data.x != null)
+    			throw new Error ("All previous chart series need to have only Y data arrays");
+    	
+    	addData(null,y,lineColor, marker);
+    }
+    
+    public void addData(double[] x, double[] y, Color lineColor)
+    {
+    	addData(x,y,lineColor,null);
+    }
+    
+    
+    public void addData(double[][] data, Color lineColor)
+    {
+    	addData(getXVector(data),getYVector(data),lineColor,null);
+    }
+    
+    public void addData(double[][] data, Color lineColor, Color marker)
+    {
+    	addData(getXVector(data),getYVector(data),lineColor,marker);
+    }
+    
+    public void addData(double[] x, double[] y, Color lineColor, Color marker)
+    { 	
+    	xyNode Data = new xyNode();
+    	    	
+    	//copy y array into node
+    	Data.y = new double[y.length];
+    	Data.lineColor = lineColor;
+    	
+    	if(marker == null)
+    		Data.lineMarker = false;
+    	else
+    	{
+    		Data.lineMarker = true;
+    		Data.markerColor = marker;
+    	}
+    	for(int i=0; i<y.length; i++)
+    		Data.y[i] = y[i];
+    	
+    	//if X is not null, copy x
+    	if(x != null)
+    	{
+        	//cant add x, and y data unless all other data has x and y data
+        	
+        	for(xyNode data: link)
+        		if(data.x == null)
+        			throw new Error ("All previous chart series need to have both X and Y data arrays");
+    		
+    		if(x.length != y.length)
+    			throw new Error("X dimension must match Y dimension");
+    		
+    		Data.x = new double[x.length];
+    		
+    		for(int i=0; i<x.length; i++)
+        		Data.x[i] = x[i];
+    		
+    	}
+    	link.add(Data);
+    }
+ 
+    /**
+     * Main method which paints the panel and shows the figure.
+     * 
+     */
+    @Override
+    protected void paintComponent(Graphics g) {
+        super.paintComponent(g);
+        Graphics2D g2 =  (Graphics2D)g;
+        g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
+        
+        int w = getWidth();
+        int h = getHeight();
+    
+        // Draw X and Y lines axis.
+        Line2D yaxis = new Line2D.Double(xPAD, yPAD, xPAD, h-yPAD);
+        Line2D.Double xaxis = new Line2D.Double(xPAD, h-yPAD, w-xPAD, h-yPAD);
+        g2.draw(yaxis); 
+        g2.draw(xaxis);
+        
+        //find Max Y limits
+        getMinMax(link);
+        
+        //draw ticks
+        drawYTickRange(g2, yaxis, 15, yMax, yMin);
+        drawXTickRange(g2, xaxis, 15, xMax, xMin);
+        
+        //plot all data
+        plot(g2);
+        
+        //draw x and y labels
+        setXLabel(g2, xLabel);
+        setYLabel(g2, yLabel);
+        setTitle(g2, titleLabel);
+        
+    }
+    
+    void setXTic(double lowerBound, double upperBound, double stepSize)
+    {
+    	this.userSetXTic = true;
+    	
+    	this.upperXtic=upperBound;
+    	this.lowerXtic=lowerBound;
+    	this.xTicStepSize = stepSize;
+    }
+    
+    public void setYTic(double lowerBound, double upperBound, double stepSize)
+    {
+    	this.userSetYTic = true;
+    	
+    	this.upperYtic=upperBound;
+    	this.lowerYtic=lowerBound;
+    	this.yTicStepSize = stepSize;
+    }
+    
+    private void plot(Graphics2D g2)
+    {
+    	
+    	int w = super.getWidth();
+    	int h = super.getHeight();
+    	
+    	Color tempC = g2.getColor();
+    	
+    	//loop through list and plot each
+    	for(int i=0; i<link.size(); i++)
+    	{
+	        // Draw lines.
+	        double xScale = (double)(w - 2*xPAD)/(upperXtic-lowerXtic);
+	        double yScale = (double)(h - 2*yPAD)/(upperYtic-lowerYtic);
+	        
+	        for(int j = 0; j < link.get(i).y.length-1; j++) 
+	        {
+	        	double x1;
+	        	double x2;
+	        	
+	            if(link.get(i).x==null)
+	            {
+	            	x1 = xPAD + j*xScale;
+	            	x2 = xPAD + (j+1)*xScale;
+	            }
+	            else
+	            {
+	            	x1 = xPAD + xScale*link.get(i).x[j] + lowerXtic*xScale;
+	            	x2 = xPAD + xScale*link.get(i).x[j+1] + lowerXtic*xScale;
+	            }
+	            
+	            double y1 = h - yPAD - yScale*link.get(i).y[j] + lowerYtic*yScale;
+	            
+	            double y2 = h - yPAD - yScale*link.get(i).y[j+1] + lowerYtic*yScale;
+	            g2.setPaint(link.get(i).lineColor);
+	            g2.draw(new Line2D.Double(x1, y1, x2, y2));
+	            
+	            if(link.get(i).lineMarker)
+	            {
+	            	g2.setPaint(link.get(i).markerColor);
+	            	g2.fill(new Ellipse2D.Double(x1-2, y1-2, 4, 4));
+	            	g2.fill(new Ellipse2D.Double(x2-2, y2-2, 4, 4));
+	            }
+	      
+	        }
+	        	        	
+    	}
+    	
+    	g2.setColor(tempC);
+    }
+    
+    /**
+     * need to optimize for loops
+     * @param list
+     */
+    private void getMinMax(LinkedList<xyNode> list)
+    {
+    	for(xyNode node: list)
+    	{
+    		double tempYMax = getMax(node.y);
+    		double tempYMin = getMin(node.y);
+    		
+    		if(tempYMin<yMin)
+    			yMin = tempYMin;
+    		
+    		if(tempYMax>yMax)
+    			yMax=tempYMax;
+    		
+    		if(xticCount < node.y.length)
+				xticCount = node.y.length;
+    		
+    		
+    		if(node.x != null)
+    		{
+        		double tempXMax = getMax(node.x);
+        		double tempXMin = getMin(node.x);
+        		
+        		if(tempXMin<xMin)
+        			xMin = tempXMin;
+        		
+        		if(tempXMax>xMax)
+        			xMax=tempXMax;
+        		
+    		}
+    		else
+    		{
+    			xMax=node.y.length-1;
+    			xMin=0;
+    			
+    		}
+    		
+    	}
+    	
+    }
+ 
+    private double getMax(double[] data) {
+        double max = -Double.MAX_VALUE;
+        for(int i = 0; i < data.length; i++) {
+            if(data[i] > max)
+                max = data[i];
+        }
+        return max;
+    }
+    
+    private double getMin(double[] data) {
+        double min = Double.MAX_VALUE;
+        for(int i = 0; i < data.length; i++) {
+            if(data[i] < min)
+                min = data[i];
+        }
+        return min;
+    }
+    
+    public void setYLabel(String s)
+    {
+    	yLabel = s;
+    }
+    
+    public void setXLabel(String s)
+    {
+    	xLabel = s;
+    }
+    
+    public void setTitle(String s)
+    {
+    	titleLabel = s;
+    }
+    
+    private void setYLabel(Graphics2D g2, String s)
+    {
+    	FontMetrics fm = getFontMetrics(getFont());
+    	int width = fm.stringWidth(s);
+    	
+    	AffineTransform temp = g2.getTransform();
+    	
+    	    AffineTransform at = new AffineTransform();
+    	    at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2);
+    	    g2.setTransform(at);
+    	    
+    	    //draw string in center of y axis
+    	    g2.drawString(s, 10, 7+getHeight()/2+width/2);
+
+        g2.setTransform(temp);
+    	
+    }
+    
+    private void setXLabel(Graphics2D g2, String s)
+    {
+    	FontMetrics fm = getFontMetrics(getFont());
+    	int width = fm.stringWidth(s);
+    	
+    	g2.drawString(s, getWidth()/2-(width/2), getHeight()-10);
+    }
+    
+    private void setTitle(Graphics2D g2, String s)
+    {
+    	FontMetrics fm = getFontMetrics(getFont());
+    	
+    	String[] line = s.split("\n");
+    	
+    	int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2);
+    	
+    	for (int i=0; i<line.length; i++)
+    	{
+    		
+    		int width = fm.stringWidth(line[i]);
+            g2.drawString(line[i], getWidth()/2-(width/2),  height);
+            height +=fm.getHeight();
+            
+    	}
+   
+    }
+
+    
+    public void yGridOn()
+    {
+    	yGrid=true;
+    	//super.repaint();
+    }
+    
+    public void yGridOff()
+    {
+    	yGrid=false;
+    	//super.repaint();
+    }
+    
+    public void xGridOn()
+    {
+    	xGrid=true;
+    	//super.repaint();
+    }
+    
+    public void xGridOff()
+    {
+    	xGrid=false;
+    	//super.repaint();
+    }
+    
+    private void drawYTickRange(Graphics2D g2, Line2D yaxis, int tickCount, double Max, double Min)
+    {
+    	if(!userSetYTic)
+    	{
+    	double range = Max - Min;
+    	
+    	
+    	//calculate max Y and min Y tic Range
+    	double unroundedTickSize = range/(tickCount-1);
+    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
+    	double pow10x = Math.pow(10, x);
+    	yTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
+    	
+    	//determine min and max tick label 
+        if(Min<0)
+        	lowerYtic = yTicStepSize * Math.floor(Min/yTicStepSize);
+        else
+        	lowerYtic = yTicStepSize * Math.ceil(Min/yTicStepSize);
+        
+        if(Max<0)
+        	upperYtic = yTicStepSize * Math.floor(1+Max/yTicStepSize);
+        else
+        	upperYtic = yTicStepSize * Math.ceil(1+Max/yTicStepSize);
+        
+    	}
+    	
+        
+        double x0 = yaxis.getX1();
+        double y0 = yaxis.getY1();
+        double xf = yaxis.getX2();
+        double yf = yaxis.getY2();
+        
+        //calculate stepsize between ticks and length of Y axis using distance formula
+        int roundedTicks = (int) ((upperYtic - lowerYtic) / yTicStepSize);
+        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
+        
+        double upper = upperYtic;
+        for (int i = 0; i<=roundedTicks; i++)
+        {
+        	double newY = y0;
+        	
+        	//calculate width of number for proper drawing
+        	String number = new DecimalFormat("#.#").format(upper);
+        	FontMetrics fm = getFontMetrics(getFont());
+        	int width = fm.stringWidth(number);
+   
+        	g2.draw(new Line2D.Double(x0,newY, x0-10,newY));
+        	g2.drawString(number, (float)x0-15-width, (float)newY+5); 
+        	 
+        	//add grid lines to chart
+        	if(yGrid && i!=roundedTicks)
+        	{
+
+        		Stroke tempS = g2.getStroke();
+        		Color tempC = g2.getColor();
+        		
+        		g2.setColor (Color.lightGray);
+          	    g2.setStroke (new BasicStroke(
+          	      1f, 
+          	      BasicStroke.CAP_ROUND, 
+          	      BasicStroke.JOIN_ROUND, 
+          	      1f, 
+          	      new float[] {5f}, 
+          	      0f));
+          	    
+          	    g2.draw(new Line2D.Double(xPAD, newY, getWidth()-xPAD, newY));
+        		
+          	    g2.setColor(tempC);
+          	    g2.setStroke(tempS);
+        		
+        	}
+        	
+        	  upper = upper - yTicStepSize;
+        	y0 = newY + distance;
+        	
+        }
+    }
+    
+    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min)
+    {
+    	drawXTickRange(g2, xaxis, tickCount, Max, Min, 1);
+    }
+    
+    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min, double skip)
+    {
+    	if(!userSetXTic)
+    	{
+    	double range = Max - Min;
+    	
+    	//calculate max Y and min Y tic Range
+    	double unroundedTickSize = range/(tickCount-1);
+    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
+    	double pow10x = Math.pow(10, x);
+    	xTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
+    	
+    	//determine min and max tick label 
+        if(Min<0)
+        	lowerXtic = xTicStepSize * Math.floor(Min/xTicStepSize);
+        else
+        	lowerXtic = xTicStepSize * Math.ceil(Min/xTicStepSize);
+        
+        if(Max<0)
+        	upperXtic = xTicStepSize * Math.floor(1+Max/xTicStepSize);
+        else
+        	upperXtic = xTicStepSize * Math.ceil(1+Max/xTicStepSize);
+    	}
+        
+        
+        double x0 = xaxis.getX1();
+        double y0 = xaxis.getY1();
+        double xf = xaxis.getX2();
+        double yf = xaxis.getY2();
+        
+        //calculate stepsize between ticks and length of Y axis using distance formula
+        int roundedTicks = (int) ((upperXtic - lowerXtic) / xTicStepSize);
+        
+        
+        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
+        
+        double lower = lowerXtic;
+        for (int i = 0; i<=roundedTicks; i++)
+        {
+        	double newX = x0;
+        	
+        	//calculate width of number for proper drawing
+        	String number = new DecimalFormat("#.#").format(lower);
+        	FontMetrics fm = getFontMetrics( getFont() );
+        	int width = fm.stringWidth(number);
+        	
+   
+        	g2.draw(new Line2D.Double(newX,yf, newX,yf+10));
+        	
+        	//dont label every x tic to prevent clutter
+        	if(i%skip==0)
+       		g2.drawString(number, (float)(newX-(width/2.0)), (float)yf+25); 
+        	
+        	//add grid lines to chart
+        	if(xGrid && i!=0)
+        	{
+        		Stroke tempS = g2.getStroke();
+        		Color tempC = g2.getColor();
+        		
+        		g2.setColor (Color.lightGray);
+          	    g2.setStroke (new BasicStroke(
+          	      1f, 
+          	      BasicStroke.CAP_ROUND, 
+          	      BasicStroke.JOIN_ROUND, 
+          	      1f, 
+          	      new float[] {5f}, 
+          	      0f));
+          	    
+          	    g2.draw(new Line2D.Double(newX, yPAD, newX, getHeight()-yPAD));
+        		
+          	    g2.setColor(tempC);
+          	    g2.setStroke(tempS);
+        		
+        	}
+        	
+        	 
+        	lower = lower + xTicStepSize;
+        	x0 = newX + distance;
+        }
+    }
+    
+    public void updateData(int series, double[][] data)
+    {
+    	//add Data to link list
+    	addData(data,null,null);
+    	
+    	//copy data from new to old and line styles from list to new list.
+    	
+    	link.get(series).x = link.getLast().x.clone();
+    	link.get(series).y = link.getLast().y.clone();
+    	
+    	//remove last data
+    	link.removeLast();
+    
+    	
+    	
+    }
+    
+	public static double[] getXVector(double[][] arr)
+	{
+		double[] temp = new double[arr.length];
+
+		for(int i=0; i<temp.length; i++)
+			temp[i] = arr[i][0];
+
+		return temp;		
+	}
+
+	public static double[] getYVector(double[][] arr)
+	{
+		double[] temp = new double[arr.length];
+
+		for(int i=0; i<temp.length; i++)
+			temp[i] = arr[i][1];
+
+		return temp;		
+	}
+	
+	/**********Class for Linked List************/
+    private class xyNode
+    {
+    	double[] x;
+    	double[] y;
+    	Color lineColor;
+    	
+    	boolean lineMarker;
+    	Color markerColor;
+    	
+    	public xyNode()
+    	{
+    		x=null;
+    		y=null;
+    		
+    		lineMarker = false;
+    	}
+    }
+	
+    /****Methods to Support Right Click Menu****/
+    @Override
+	public void lostOwnership(Clipboard    clip, Transferable transferable) 
+	{
+		//We must keep the object we placed on the system clipboard
+		//until this method is called.
+	}
+
+    
+    
+    private void menu(JFrame g, final FalconLinePlot p )
+    {
+
+    	g.addMouseListener(new PopupTriggerListener());
+    	
+    	JMenuItem item = new JMenuItem("Copy Figure");
+
+        item.addActionListener(new ActionListener() 
+        {
+          public void actionPerformed(ActionEvent e) 
+          {
+            
+            BufferedImage i = new BufferedImage(p.getSize().width, p.getSize().height,BufferedImage.TRANSLUCENT);
+            p.setOpaque(false);
+            p.paint(i.createGraphics()); 
+            TransferableImage trans = new TransferableImage( i );
+			Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
+			c.setContents( trans, p);
+          }
+        });
+        
+        menu.add(item);
+    	
+        item = new JMenuItem("Desktop ScreenShot");
+        item.addActionListener(new ActionListener() 
+        {
+          public void actionPerformed(ActionEvent e) 
+          {
+            System.out.println("Copy files to clipboard");
+            
+            try {
+                Robot robot = new Robot();
+                Dimension screenSize  = Toolkit.getDefaultToolkit().getScreenSize();
+                Rectangle screen = new Rectangle( screenSize );
+                BufferedImage i = robot.createScreenCapture( screen );
+                TransferableImage trans = new TransferableImage( i );
+                Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
+                c.setContents( trans, p);
+            }
+            catch ( AWTException x ) {
+                x.printStackTrace();
+                System.exit( 1 );
+            }
+            
+          }
+        });
+        
+        menu.add(item);
+    }
+
+    class PopupTriggerListener extends MouseAdapter {
+    	public void mousePressed(MouseEvent ev) {
+    		if (ev.isPopupTrigger()) {
+    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
+    		}
+    	}
+
+    	public void mouseReleased(MouseEvent ev) {
+    		if (ev.isPopupTrigger()) {
+    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
+    		}
+    	}
+
+    	public void mouseClicked(MouseEvent ev) {
+
+    	}
+    }
+
+    private class TransferableImage implements Transferable {
+
+        Image i;
+
+        public TransferableImage( Image i ) {
+            this.i = i;
+        }
+
+        public Object getTransferData( DataFlavor flavor )
+        throws UnsupportedFlavorException, IOException {
+            if ( flavor.equals( DataFlavor.imageFlavor ) && i != null ) {
+                return i;
+            }
+            else {
+                throw new UnsupportedFlavorException( flavor );
+            }
+        }
+
+        public DataFlavor[] getTransferDataFlavors() {
+            DataFlavor[] flavors = new DataFlavor[ 1 ];
+            flavors[ 0 ] = DataFlavor.imageFlavor;
+            return flavors;
+        }
+
+        public boolean isDataFlavorSupported( DataFlavor flavor ) {
+            DataFlavor[] flavors = getTransferDataFlavors();
+            for ( int i = 0; i < flavors.length; i++ ) {
+                if ( flavor.equals( flavors[ i ] ) ) {
+                    return true;
+                }
+            }
+
+            return false;
+        }
+    }
+    
+    /******TEST MAIN METHOD*******/
+    public static void main(String[] args) {
+    	
+  	  double[] data = {
+  		        -235, 14, 18, 03, 60, 150, 74, 87, 54, 77,
+  		        61, 55, 48, 60, 49, 36, 38, 27, 20, 18,5
+  		    };
+  	  
+  	  double[] data2 = {
+		        -4, 124, 128, 33, -1, 1, 74, 87, 54, 77,
+		        61, 55, 48, 60, 40, 36, 38, 27, 20, 18,5
+		    };
+  	  
+  	  double[] test = {0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 2.0,
+  			  2.2,2.4,2.6,2.8,3.0,3.2,3.4,3.6,3.8,4.0,4.2
+  	  };
+  	
+  	
+  	
+  	FalconLinePlot fig2 = new FalconLinePlot(data,Color.red, Color.blue);
+  	
+  	fig2.yGridOn();
+  	fig2.xGridOn();
+  	fig2.setYLabel("This is a new");
+  	
+  	fig2.addData(data2, Color.blue);
+  	
+  	FalconLinePlot fig1 =  new FalconLinePlot(test,data);
+  	
+  }
+    
+    
+}
+   

src/org/usfirst/frc4388/utility/Interpolable.java

@@ -0,0 +1,27 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * 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);
+}

src/org/usfirst/frc4388/utility/InterpolatingTreeMap.java

@@ -0,0 +1,91 @@
+package org.usfirst.frc4388.utility;
+
+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;
+        }
+    }
+}

src/org/usfirst/frc4388/utility/InverseInterpolable.java

@@ -0,0 +1,25 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * 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);
+}

src/org/usfirst/frc4388/utility/Kinematics.java

@@ -0,0 +1,59 @@
+package org.usfirst.frc4388.utility;
+
+import org.usfirst.frc4388.robot.Constants;
+
+/**
+ * Provides forward and inverse kinematics equations for the robot modeling the
+ * wheelbase as a differential drive (with a corrective factor to account for
+ * the inherent skidding of the center 4 wheels quasi-kinematically).
+ */
+
+public class Kinematics {
+    private static final double kEpsilon = 1E-9;
+
+    /**
+     * Forward kinematics using only encoders, rotation is implicit (less
+     * accurate than below, but useful for predicting motion)
+     */
+    public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) {
+        double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2;
+        double delta_v = (right_wheel_delta - left_wheel_delta) / 2;
+        double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter;
+        return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation);
+    }
+
+    /**
+     * Forward kinematics using encoders and explicitly measured rotation (ex.
+     * from gyro)
+     */
+    public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta,
+            double delta_rotation_rads) {
+        return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads);
+    }
+
+    /** Append the result of forward kinematics to a previous pose. */
+    public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta,
+            double right_wheel_delta, Rotation2d current_heading) {
+        RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta,
+                current_pose.getRotation().inverse().rotateBy(current_heading).getRadians());
+        return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro));
+    }
+
+    public static class DriveVelocity {
+        public final double left;
+        public final double right;
+
+        public DriveVelocity(double left, double right) {
+            this.left = left;
+            this.right = right;
+        }
+    }
+
+    public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) {
+        if (Math.abs(velocity.dtheta) < kEpsilon) {
+            return new DriveVelocity(velocity.dx, velocity.dx);
+        }
+        double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor);
+        return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v);
+    }
+}

src/org/usfirst/frc4388/utility/MMTalonPIDController.java

@@ -0,0 +1,137 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+
+import com.ctre.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<CANTalonEncoder> 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<CANTalonEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+	
+	private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) {
+		return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
+			//motorController.setF(pidParams.kF);
+			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kF(0, pidParams.kF, 0);	//TODO: verify want parameter slot 0, with no timeout
+		}
+	}
+	
+	public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
+		controlMode = MMControlMode.STRAIGHT;
+		this.startGyroAngle = desiredAngle;
+		this.useGyroLock = useGyroLock;
+		this.targetValue = targetValue;
+		
+		// Set up the motion profile 
+		for (CANTalonEncoder motorController : motorControllers) {
+			if (resetEncoder) {
+				//motorController.setPosition(0);
+				motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			}
+			//motorController.setMotionMagicCruiseVelocity(maxVelocity);
+			//motorController.setMotionMagicAcceleration(maxAcceleration);
+			//motorController.set(targetValue);
+			//motorController.changeControlMode(TalonControlMode.MotionMagic);
+			motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0);
+			motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0);
+			motorController.set(ControlMode.MotionMagic, targetValue);
+		}
+	}
+			
+	public void setZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(angle);
+			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		}
+	}
+
+	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 (CANTalonEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					//motorController.set(rightTarget);
+			    	motorController.set(motorController.getControlMode(), rightTarget);	//TODO: change to explicit mode set?
+				}
+				else {
+					//motorController.set(leftTarget);
+			    	motorController.set(motorController.getControlMode(), leftTarget);	//TODO: change to explicit mode set?
+				}
+			}
+		}
+		
+		return false;
+	}
+}

src/org/usfirst/frc4388/utility/MPSoftwarePIDController.java

@@ -0,0 +1,156 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+public class MPSoftwarePIDController
+{	
+	public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC };
+
+	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected 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<CANTalonEncoder> 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);
+		// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
+		// for (CANTalonEncoder motorController : motorControllers) {
+		//     motorController.changeControlMode(TalonControlMode.PercentVbus);
+		// }
+		
+		prevError = 0.0;
+		totalError = 0.0;
+	}
+	
+	//public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) {
+	//	this.turnType = turnType;
+	//	this.useGyroLock = true;
+	//	
+	//	// Set up the motion profile 
+	//	mp = MotionProfileCache.getInstance().getMP(key);
+	//	this.startGyroAngle = mp.getStartDistance();
+	//	this.targetGyroAngle = mp.getTargetDistance();
+	//	
+	//	for (CANTalonEncoder motorController : motorControllers) {
+	//		motorController.changeControlMode(TalonControlMode.PercentVbus);
+	//	}
+	//	
+	//	prevError = 0.0;
+	//	totalError = 0.0;
+	//}
+	
+	public 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 (CANTalonEncoder motorController : motorControllers) {
+				//motorController.set(output);
+				motorController.set(ControlMode.PercentOutput, output);
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
+			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 (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
+			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 (turnType == MPSoftwareTurnType.LEFT_ARC) {
+			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 (turnType == MPSoftwareTurnType.RIGHT_ARC) {
+			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);					
+				}
+			}
+		}
+
+		return false;
+	}
+	
+	public MotionProfilePoint getCurrentPoint() {
+		return mpPoint;
+	}
+}

src/org/usfirst/frc4388/utility/MPTalonPIDController.java

@@ -0,0 +1,233 @@
+
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+public class MPTalonPIDController
+{	
+	protected static enum MPControlMode { STRAIGHT, TURN };
+	public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
+
+	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected 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;
+	
+	public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
+			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
+		}
+	}
+	
+	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 (CANTalonEncoder motorController : motorControllers) {
+			if (resetEncoder) {
+				//motorController.setPosition(0);
+				motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			}
+			//motorController.set(mp.getStartDistance());
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.set(ControlMode.Position, mp.getStartDistance());
+		}
+	}
+	
+	//public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
+	//	controlMode = MPControlMode.STRAIGHT;
+	//	this.startGyroAngle = desiredAngle;
+	//	this.useGyroLock = useGyroLock;
+	//	
+	//	// Set up the motion profile 
+	//	mp = MotionProfileCache.getInstance().getMP(key);
+	//	for (CANTalonEncoder motorController : motorControllers) {
+	//		if (resetEncoder) {
+	//			motorController.setPosition(0);
+	//		}
+	//		motorController.set(mp.getStartDistance());
+	//		motorController.changeControlMode(TalonControlMode.Position);
+	//	}
+	//}
+	
+	public 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 (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+		
+		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 (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(angle);
+			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		}
+	}
+
+	public 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;
+			
+				KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
+				KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position;
+			}
+			
+			// Update the controllers Kf and set point.
+			for (CANTalonEncoder motorController : motorControllers) {
+				if (motorController.isRight()) {
+					//motorController.setF(KfRight);
+					motorController.config_kF(0, KfRight, 0);	//TODO: verify
+					motorController.setWorld(-mpPoint.position);
+				}
+				else {
+					//motorController.setF(KfLeft);
+					motorController.config_kF(0, KfLeft, 0);	//TODO: verify
+					motorController.setWorld(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 (CANTalonEncoder motorController : motorControllers) {
+				if (turnType == MPTalonTurnType.TANK) {
+					if (motorController.isRight()) {
+						//motorController.setF(KfRight);
+						motorController.config_kF(0, KfRight, 0);	//TODO: verify
+						motorController.setWorld(-mpPoint.position);
+					}
+					else {
+						//motorController.setF(KfLeft);
+						motorController.config_kF(0, KfLeft, 0);	//TODO: verify
+						motorController.setWorld(mpPoint.position);
+					}
+				}
+				else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) {
+					if (!motorController.isRight()) {
+						//motorController.setF(KfLeft);
+						motorController.config_kF(0, KfLeft, 0);	//TODO: verify
+						motorController.setWorld(mpPoint.position);
+					}
+				}
+				else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) {
+					if (motorController.isRight()) {
+						//motorController.setF(KfRight);
+						motorController.config_kF(0, KfRight, 0);	//TODO: verify
+						motorController.setWorld(-mpPoint.position);
+					}
+				}
+			}
+		}
+		
+		return false;
+	}
+	
+	public MotionProfilePoint getCurrentPoint() {
+		return mpPoint;
+	}
+}

src/org/usfirst/frc4388/utility/MPTalonPIDPathController.java

@@ -0,0 +1,113 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import jaci.pathfinder.Trajectory.Segment;
+
+public class MPTalonPIDPathController
+{	
+	protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected long periodMs;
+	protected PIDParams pidParams;	
+	protected PathGenerator path;
+	protected double startGyroAngle;
+	protected double targetGyroAngle;
+	protected double trackDistance;
+	
+	public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
+			motorController.config_kP(0, pidParams.kP, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kI(0, pidParams.kI, 0);	//TODO: verify want parameter slot 0, with no timeout
+			motorController.config_kD(0, pidParams.kD, 0);	//TODO: verify want parameter slot 0, with no timeout
+		}
+	}
+	
+	public void setMPPathTarget(PathGenerator path) {
+		this.path = path;
+		path.resetCounter();
+		
+		// Set up the motion profile 
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+	
+	public void setZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(angle);
+			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		}
+	}
+
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		Segment leftPoint = path.getLeftPoint();
+		Segment rightPoint = path.getRightPoint();
+		
+		// Check if we are finished
+		if (leftPoint == null) {
+			return true;
+		}
+		
+		// Calculate the motion profile feed forward and gyro feedback terms
+		double KfLeft = 0.0;
+		double KfRight = 0.0;
+
+		// Update the set points and Kf gains
+		double gyroDelta = -path.getHeading() - currentGyroAngle;
+		if (Math.abs(leftPoint.position) > 0.001) {
+			KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position;
+			KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position;
+		}
+		
+		// Update the controllers Kf and set point.
+		for (CANTalonEncoder motorController : motorControllers) {
+			if (motorController.isRight()) {
+				//motorController.setF(KfRight);
+				motorController.config_kF(0, KfRight, 0);	//TODO: verify
+				motorController.setWorld(rightPoint.position);
+			}
+			else {
+				//motorController.setF(KfLeft);
+				motorController.config_kF(0, KfLeft, 0);	//TODO: verify
+				motorController.setWorld(leftPoint.position);
+			}
+		}
+		
+		path.incrementCounter();
+		
+		return false;
+	}
+}

src/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java

@@ -0,0 +1,111 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import jaci.pathfinder.Trajectory.Segment;
+
+public class MPTalonPIDPathVelocityController
+{	
+	protected ArrayList<CANTalonEncoder> 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<CANTalonEncoder> motorControllers) 
+	{
+		this.motorControllers = motorControllers;
+		this.periodMs = periodMs;
+		setPID(pidParams);
+	}
+    
+	public void setPID(PIDParams pidParams) {
+		this.pidParams = pidParams;
+		
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
+			//motorController.setF(pidParams.kF);
+			//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
+			//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
+			//motorController.setProfile(0);
+			motorController.config_kP(0, pidParams.kP, 0);
+			motorController.config_kI(0, pidParams.kI, 0);
+			motorController.config_kD(0, pidParams.kD, 0);
+			motorController.config_kF(0, pidParams.kF, 0);
+			motorController.configNominalOutputForward(+0.0f, 0);
+			motorController.configNominalOutputReverse(-0.0f, 0);
+			motorController.configPeakOutputForward(+1.0f, 0);
+			motorController.configPeakOutputReverse(-1.0f, 0);
+			motorController.selectProfileSlot(0, 0);
+		}
+	}
+	
+	public void setMPPathTarget(PathGenerator path) {
+		this.path = path;
+		path.resetCounter();
+		
+		// Set up the motion profile 
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Speed);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Velocity, 0);
+		}
+	}
+	
+	public void setZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(0);
+			//motorController.set(0);
+			//motorController.changeControlMode(TalonControlMode.Position);
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+			motorController.set(ControlMode.Position, 0);
+		}
+	}
+
+	public void resetZeroPosition() {
+		for (CANTalonEncoder motorController : motorControllers) {
+			motorController.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
+		}
+	}
+
+	public void resetZeroPosition(double angle) {
+		for (CANTalonEncoder motorController : motorControllers) {
+			//motorController.setPosition(angle);
+			motorController.setSelectedSensorPosition((int)angle, 0, 0);	//TODO URGENT: convert angle to raw sensor position
+		}
+	}
+
+	public boolean controlLoopUpdate(double currentGyroAngle) {
+		Segment leftPoint = path.getLeftPoint();
+		Segment rightPoint = path.getRightPoint();
+		
+		// Check if we are finished
+		if (leftPoint == null) {
+			return true;
+		}
+		
+		// Calculate the motion profile feed forward and gyro feedback terms
+		double rightVelocity = rightPoint.velocity;
+		double leftVelocity = leftPoint.velocity;
+		
+		// Update the controllers Kf and set point.
+		for (CANTalonEncoder motorController : motorControllers) {
+			if (motorController.isRight()) {
+				motorController.setVelocityWorld(rightVelocity);
+			}
+			else {
+				motorController.setVelocityWorld(leftVelocity);
+			}
+		}
+		
+		path.incrementCounter();
+		
+		return false;
+	}
+}

src/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, 96, 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");
+	}
+}

src/org/usfirst/frc4388/utility/MotionProfilePoint.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc4388.utility;
+
+public class MotionProfilePoint {
+	public double time;
+	public double position;
+	public double velocity;
+	public double acceleration;
+	
+	public void initialize(double startPosition) {
+		time = 0;
+		position = startPosition;
+		velocity = 0;
+		acceleration = 0;
+	}
+}

src/org/usfirst/frc4388/utility/PIDParams.java

@@ -0,0 +1,62 @@
+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)
+    {
+    	this.kP = kP;
+    }
+    
+    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;
+    }
+}

src/org/usfirst/frc4388/utility/Path.java

@@ -0,0 +1,260 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Set;
+
+import org.usfirst.frc4388.utility.Path.Waypoint;
+
+/**
+ * A Path is a recording of the path that the robot takes. Path objects consist
+ * of a List of Waypoints that the robot passes by. Using multiple Waypoints in
+ * a Path object and the robot's current speed, the code can extrapolate future
+ * Waypoints and predict the robot's motion. It can also dictate the robot's
+ * motion along the set path.
+ */
+public class Path {
+    protected static final double kSegmentCompletePercentage = .99;
+
+    protected List<Waypoint> mWaypoints;
+    protected List<PathSegment> mSegments;
+    protected Set<String> mMarkersCrossed;
+
+    /**
+     * A point along the Path, which consists of the location, the speed, and a
+     * string marker (that future code can identify). Paths consist of a List of
+     * Waypoints.
+     */
+    public static class Waypoint {
+        public final Translation2d position;
+        public final double speed;
+        public final Optional<String> marker;
+
+        public Waypoint(Translation2d position, double speed) {
+            this.position = position;
+            this.speed = speed;
+            this.marker = Optional.empty();
+        }
+
+        public Waypoint(Translation2d position, double speed, String marker) {
+            this.position = position;
+            this.speed = speed;
+            this.marker = Optional.of(marker);
+        }
+    }
+
+    public Path(List<Waypoint> waypoints) {
+        mMarkersCrossed = new HashSet<String>();
+        mWaypoints = waypoints;
+        mSegments = new ArrayList<PathSegment>();
+        for (int i = 0; i < waypoints.size() - 1; ++i) {
+            mSegments.add(
+                    new PathSegment(waypoints.get(i).position, waypoints.get(i + 1).position, waypoints.get(i).speed));
+        }
+        // The first waypoint is already complete
+        if (mWaypoints.size() > 0) {
+            Waypoint first_waypoint = mWaypoints.get(0);
+            if (first_waypoint.marker.isPresent()) {
+                mMarkersCrossed.add(first_waypoint.marker.get());
+            }
+            mWaypoints.remove(0);
+        }
+    }
+
+    /**
+     * @param An
+     *            initial position
+     * @return Returns the distance from the position to the first point on the
+     *         path
+     */
+    public double update(Translation2d position) {
+        double rv = 0.0;
+        for (Iterator<PathSegment> it = mSegments.iterator(); it.hasNext();) {
+            PathSegment segment = it.next();
+            PathSegment.ClosestPointReport closest_point_report = segment.getClosestPoint(position);
+            if (closest_point_report.index >= kSegmentCompletePercentage) {
+                it.remove();
+                if (mWaypoints.size() > 0) {
+                    Waypoint waypoint = mWaypoints.get(0);
+                    if (waypoint.marker.isPresent()) {
+                        mMarkersCrossed.add(waypoint.marker.get());
+                    }
+                    mWaypoints.remove(0);
+                }
+            } else {
+                if (closest_point_report.index > 0.0) {
+                    // Can shorten this segment
+                    segment.updateStart(closest_point_report.closest_point);
+                }
+                // We are done
+                rv = closest_point_report.distance;
+                // ...unless the next segment is closer now
+                if (it.hasNext()) {
+                    PathSegment next = it.next();
+                    PathSegment.ClosestPointReport next_closest_point_report = next.getClosestPoint(position);
+                    if (next_closest_point_report.index > 0
+                            && next_closest_point_report.index < kSegmentCompletePercentage
+                            && next_closest_point_report.distance < rv) {
+                        next.updateStart(next_closest_point_report.closest_point);
+                        rv = next_closest_point_report.distance;
+                        mSegments.remove(0);
+                        if (mWaypoints.size() > 0) {
+                            Waypoint waypoint = mWaypoints.get(0);
+                            if (waypoint.marker.isPresent()) {
+                                mMarkersCrossed.add(waypoint.marker.get());
+                            }
+                            mWaypoints.remove(0);
+                        }
+                    }
+                }
+                break;
+            }
+        }
+        return rv;
+    }
+
+    public Set<String> getMarkersCrossed() {
+        return mMarkersCrossed;
+    }
+
+    public double getRemainingLength() {
+        double length = 0.0;
+        for (int i = 0; i < mSegments.size(); ++i) {
+            length += mSegments.get(i).getLength();
+        }
+        return length;
+    }
+
+    /**
+     * @param The
+     *            robot's current position
+     * @param A
+     *            specified distance to predict a future waypoint
+     * @return A segment of the robot's predicted motion with start/end points
+     *         and speed.
+     */
+    public PathSegment.Sample getLookaheadPoint(Translation2d position, double lookahead_distance) {
+        if (mSegments.size() == 0) {
+            return new PathSegment.Sample(new Translation2d(), 0);
+        }
+
+        // Check the distances to the start and end of each segment. As soon as
+        // we find a point > lookahead_distance away, we know the right point
+        // lies somewhere on that segment.
+        Translation2d position_inverse = position.inverse();
+        if (position_inverse.translateBy(mSegments.get(0).getStart()).norm() >= lookahead_distance) {
+            // Special case: Before the first point, so just return the first
+            // point.
+            return new PathSegment.Sample(mSegments.get(0).getStart(), mSegments.get(0).getSpeed());
+        }
+        for (int i = 0; i < mSegments.size(); ++i) {
+            PathSegment segment = mSegments.get(i);
+            double distance = position_inverse.translateBy(segment.getEnd()).norm();
+            if (distance >= lookahead_distance) {
+                // This segment contains the lookahead point
+                Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(segment, position,
+                        lookahead_distance);
+                if (intersection_point.isPresent()) {
+                    return new PathSegment.Sample(intersection_point.get(), segment.getSpeed());
+                } else {
+                    System.out.println("ERROR: No intersection point?");
+                }
+            }
+        }
+        // Special case: After the last point, so extrapolate forward.
+        PathSegment last_segment = mSegments.get(mSegments.size() - 1);
+        PathSegment new_last_segment = new PathSegment(last_segment.getStart(), last_segment.interpolate(10000),
+                last_segment.getSpeed());
+        Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(new_last_segment, position,
+                lookahead_distance);
+        if (intersection_point.isPresent()) {
+            return new PathSegment.Sample(intersection_point.get(), last_segment.getSpeed());
+        } else {
+            System.out.println("ERROR: No intersection point anywhere on line?");
+            return new PathSegment.Sample(last_segment.getEnd(), last_segment.getSpeed());
+        }
+    }
+
+    static Optional<Translation2d> getFirstCircleSegmentIntersection(PathSegment segment, Translation2d center,
+            double radius) {
+        double x1 = segment.getStart().getX() - center.getX();
+        double y1 = segment.getStart().getY() - center.getY();
+        double x2 = segment.getEnd().getX() - center.getX();
+        double y2 = segment.getEnd().getY() - center.getY();
+        double dx = x2 - x1;
+        double dy = y2 - y1;
+        double dr_squared = dx * dx + dy * dy;
+        double det = x1 * y2 - x2 * y1;
+
+        double discriminant = dr_squared * radius * radius - det * det;
+        if (discriminant < 0) {
+            // No intersection
+            return Optional.empty();
+        }
+
+        double sqrt_discriminant = Math.sqrt(discriminant);
+        Translation2d pos_solution = new Translation2d(
+                (det * dy + (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
+                (-det * dx + Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
+        Translation2d neg_solution = new Translation2d(
+                (det * dy - (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
+                (-det * dx - Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
+
+        // Choose the one between start and end that is closest to start
+        double pos_dot_product = segment.dotProduct(pos_solution);
+        double neg_dot_product = segment.dotProduct(neg_solution);
+        if (pos_dot_product < 0 && neg_dot_product >= 0) {
+            return Optional.of(neg_solution);
+        } else if (pos_dot_product >= 0 && neg_dot_product < 0) {
+            return Optional.of(pos_solution);
+        } else {
+            if (Math.abs(pos_dot_product) <= Math.abs(neg_dot_product)) {
+                return Optional.of(pos_solution);
+            } else {
+                return Optional.of(neg_solution);
+            }
+        }
+    }
+    
+    public static void addCircleArc(List<Waypoint> waypoints, double radius, double angleDeg, int numPoints, String endMarker ) {
+    	Waypoint last = waypoints.get(waypoints.size() - 1);
+    	
+    	double centerX = last.position.x_;
+    	double centerY = radius;
+    	
+    	double deltaAngle = angleDeg / numPoints;
+    	double currentAngle = deltaAngle;
+    	for (int i = 0; i < numPoints; i++ ) {
+    		double x = radius * Math.sin(Math.toRadians(currentAngle)) + centerX;
+    		double y = centerY - radius * Math.cos(Math.toRadians(currentAngle));
+    		
+    		if (i == numPoints - 1 && endMarker != null) {
+    			Waypoint point = new Waypoint(new Translation2d(x, y), last.speed, endMarker);
+    			waypoints.add(point);
+    		}
+    		else {
+    			Waypoint point = new Waypoint(new Translation2d(x, y), last.speed);
+    			waypoints.add(point);
+    		}
+    		
+    		currentAngle += deltaAngle;
+    	}
+    }
+    
+	public static void main(String[] args) {
+		
+        List<Waypoint> waypoints = new ArrayList<>();
+        waypoints.add(new Waypoint(new Translation2d(0, 0), 40.0));
+        waypoints.add(new Waypoint(new Translation2d(-35, 0), 40.0));
+        Path.addCircleArc(waypoints, 30.0, -45.0, 10, "hopperSensorOn");
+        waypoints.add(new Waypoint(new Translation2d(-85, 30), 40.0));
+        
+        for (int i = 0; i < waypoints.size(); i++) {
+        	Waypoint curPoint = waypoints.get(i);
+        	System.out.println("x = " + curPoint.position.x_ + ", y = " + curPoint.position.y_);
+        }
+	}
+}

src/org/usfirst/frc4388/utility/PathGenerator.java

@@ -0,0 +1,232 @@
+package org.usfirst.frc4388.utility;
+
+import java.awt.Color;
+
+import org.usfirst.frc4388.robot.subsystems.Drive;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Trajectory;
+import jaci.pathfinder.Trajectory.Segment;
+import jaci.pathfinder.Waypoint;
+import jaci.pathfinder.modifiers.TankModifier;
+
+public class PathGenerator {
+	
+	private Segment[] centerPoints;
+	private Segment[] leftPoints;
+	private Segment[] rightPoints;
+	private int curIndex = 0;
+	
+	public PathGenerator(Waypoint[] points, double timeStep, double maxVelocity, double maxAccel, double maxJerk) {
+
+		boolean negativeFlag = false;
+		for (int i = 0; i < points.length; i++) {
+        	if (points[i].x < 0) {
+        		negativeFlag = true;
+        	}
+        }
+		if (negativeFlag == true) {
+			for (int i = 0; i < points.length; i++) {
+	        	points[i].x = -(points[i].x);
+	        }
+		}
+		
+        Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_LOW, timeStep, maxVelocity, maxAccel, maxJerk);
+        Trajectory trajectory = Pathfinder.generate(points, config);
+        centerPoints = trajectory.segments;       
+
+	    TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES);
+	    leftPoints = modifier.getLeftTrajectory().segments;
+	    rightPoints = modifier.getRightTrajectory().segments; 
+
+        if (negativeFlag == true) {
+			for (int i = 0; i < centerPoints.length; i++) {
+				Segment curSeg = centerPoints[i];
+				curSeg.x = -(curSeg.x);
+				curSeg.y = -(curSeg.y);
+				curSeg.jerk = -(curSeg.jerk);
+				curSeg.acceleration = -(curSeg.acceleration);
+				curSeg.velocity = -(curSeg.velocity);
+				curSeg.position = -(curSeg.position);
+				curSeg.heading = -(curSeg.heading);
+				curSeg = leftPoints[i];
+				curSeg.x = -(curSeg.x);
+				curSeg.y = -(curSeg.y);
+				curSeg.jerk = -(curSeg.jerk);
+				curSeg.acceleration = -(curSeg.acceleration);
+				curSeg.velocity = -(curSeg.velocity);
+				curSeg.position = -(curSeg.position);
+				curSeg.heading = -(curSeg.heading);
+				curSeg = rightPoints[i];
+				curSeg.x = -(curSeg.x);
+				curSeg.y = -(curSeg.y);
+				curSeg.jerk = -(curSeg.jerk);
+				curSeg.acceleration = -(curSeg.acceleration);
+				curSeg.velocity = -(curSeg.velocity);
+				curSeg.position = -(curSeg.position);
+				curSeg.heading = -(curSeg.heading);
+			}
+		}
+		
+//      TankModifier2 modifier2 = new TankModifier2();
+//      modifier2.modify(centerPoints, Drive.TRACK_WIDTH_INCHES);
+//      leftPoints = modifier2.getLeftSegments();
+//      rightPoints = modifier2.getRightSegments();
+	}
+	
+	public Segment getLeftPoint() {
+		return (curIndex < leftPoints.length) ? leftPoints[curIndex] : null;
+	}
+	
+	public Segment getRightPoint() {
+		return (curIndex < rightPoints.length) ? rightPoints[curIndex] : null;
+	}
+	
+	public Segment getCenterPoint() {
+		return (curIndex < centerPoints.length) ? centerPoints[curIndex] : null;
+	}
+	
+	public Double getHeading() {
+		if (curIndex < centerPoints.length) {
+			double heading = Math.toDegrees(centerPoints[curIndex].heading);
+			if (heading > 180) {
+				heading -= 360;
+			}
+			else if (heading < -180) {
+				heading += 360;
+			}
+			return heading;
+		}
+		return null;
+	}
+	
+	public void incrementCounter() {
+		curIndex++;
+	}
+	
+	public void resetCounter() {
+		curIndex =0;
+	}
+	
+	public Segment[] getCenterPoints() {
+		return centerPoints;
+	}
+	
+	public Segment[] getLeftPoints() {
+		return leftPoints;
+	}
+	
+	public Segment[] getRightPoints() {
+		return rightPoints;
+	}
+	
+    public static void main(String[] args) {
+        Waypoint[] points = new Waypoint[] {
+                new Waypoint(0, 0, 0),
+                new Waypoint(78, 20, Pathfinder.d2r(32))
+        };
+
+        PathGenerator path = new PathGenerator(points, 0.01, 120, 200.0, 700.0);
+        Segment[] centerSegments = path.getCenterPoints();
+        Segment[] leftSegments = path.getLeftPoints();
+        Segment[] rightSegments = path.getRightPoints();
+       
+        double[] heading = new double[centerSegments.length];
+        double[] centerX = new double[centerSegments.length];
+        double[] centerY = new double[centerSegments.length];
+        double[] centerAccel = new double[centerSegments.length];
+        double[] centerVelocity = new double[centerSegments.length];
+        double[] centerPosition = new double[centerSegments.length];
+        double[] leftX = new double[leftSegments.length];
+        double[] leftY = new double[leftSegments.length];
+        double[] leftAccel = new double[leftSegments.length];
+        double[] leftVelocity = new double[leftSegments.length];
+        double[] leftPosition = new double[leftSegments.length];
+        double[] rightX = new double[rightSegments.length];
+        double[] rightY = new double[rightSegments.length];
+        double[] rightAccel = new double[rightSegments.length];
+        double[] rightVelocity = new double[rightSegments.length];
+        double[] rightPosition = new double[rightSegments.length];
+        
+        path.resetCounter();
+        for (int i = 0; i < centerSegments.length; i++) {
+        	heading[i] = path.getHeading();
+        	centerX[i] = path.getCenterPoint().x;
+        	centerY[i] = path.getCenterPoint().y;
+        	centerAccel[i] = path.getCenterPoint().acceleration;
+        	centerVelocity[i] = path.getCenterPoint().velocity;
+        	centerPosition[i] = path.getCenterPoint().position;
+        	leftX[i] = path.getLeftPoint().x;
+        	leftY[i] = path.getLeftPoint().y;
+        	leftAccel[i] = path.getLeftPoint().acceleration;
+        	leftVelocity[i] = path.getLeftPoint().velocity;
+        	leftPosition[i] = path.getLeftPoint().position;
+        	rightX[i] = path.getRightPoint().x;
+        	rightY[i] = path.getRightPoint().y;
+        	rightAccel[i] = path.getRightPoint().acceleration;
+        	rightVelocity[i] = path.getRightPoint().velocity;
+        	rightPosition[i] = path.getRightPoint().position;
+        	path.incrementCounter();
+        }
+        
+		FalconLinePlot fig4 = new FalconLinePlot(centerX);
+		fig4.yGridOn();
+		fig4.xGridOn();
+		fig4.setYLabel("X (in)");
+		fig4.setXLabel("time");
+		fig4.setTitle("Position Profile X");
+		fig4.addData(rightX, Color.magenta);
+		fig4.addData(leftX, Color.blue);
+
+		FalconLinePlot fig0 = new FalconLinePlot(centerY);
+		fig0.yGridOn();
+		fig0.xGridOn();
+		fig0.setYLabel("Y (in)");
+		fig0.setXLabel("time");
+		fig0.setTitle("Position Profile Y");
+		fig0.addData(rightY, Color.magenta);
+		fig0.addData(leftY, Color.blue);
+
+		FalconLinePlot fig1 = new FalconLinePlot(centerPosition);
+		fig1.yGridOn();
+		fig1.xGridOn();
+		fig1.setYLabel("Position (in)");
+		fig1.setXLabel("time (seconds)");
+		fig1.setTitle("Position Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
+		fig1.addData(rightPosition, Color.magenta);
+		fig1.addData(leftPosition, Color.blue);
+
+		FalconLinePlot fig2 = new FalconLinePlot(centerVelocity, Color.green, Color.green);
+		fig2.yGridOn();
+		fig2.xGridOn();
+		fig2.setYLabel("Velocity (in/sec)");
+		fig2.setXLabel("time (seconds)");
+		fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
+		fig2.addData(rightVelocity, Color.magenta);
+		fig2.addData(leftVelocity, Color.blue);
+
+		FalconLinePlot fig3 = new FalconLinePlot(centerAccel, Color.green, Color.green);
+		fig3.yGridOn();
+		fig3.xGridOn();
+		fig3.setYLabel("Accel (in/sec^2)");
+		fig3.setXLabel("time (seconds)");
+		fig3.setTitle("Accel Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
+		fig3.addData(rightAccel, Color.magenta);
+		fig3.addData(leftAccel, Color.blue);
+
+		FalconLinePlot fig5 = new FalconLinePlot(heading);
+		fig5.yGridOn();
+		fig5.xGridOn();
+		fig5.setYLabel("Heading");
+		fig5.setXLabel("time");
+		fig5.setTitle("Heading Profile");
+
+		FalconLinePlot fig6 = new FalconLinePlot(centerX, centerY);
+		fig6.yGridOn();
+		fig6.xGridOn();
+		fig6.setYLabel("Y");
+		fig6.setXLabel("X");
+		fig6.setTitle("XY Profile");
+  }
+
+}

src/org/usfirst/frc4388/utility/PathSegment.java

@@ -0,0 +1,89 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * A PathSegment consists of two Translation2d objects (the start and end
+ * points) as well as the speed of the robot.
+ *
+ */
+public class PathSegment {
+    protected static final double kEpsilon = 1E-9;
+
+    public static class Sample {
+        public final Translation2d translation;
+        public final double speed;
+
+        public Sample(Translation2d translation, double speed) {
+            this.translation = translation;
+            this.speed = speed;
+        }
+    }
+
+    protected double mSpeed;
+    protected Translation2d mStart;
+    protected Translation2d mEnd;
+    protected Translation2d mStartToEnd; // pre-computed for efficiency
+    protected double mLength; // pre-computed for efficiency
+
+    public static class ClosestPointReport {
+        public double index; // Index of the point on the path segment (not
+                             // clamped to [0, 1])
+        public double clamped_index; // As above, but clamped to [0, 1]
+        public Translation2d closest_point; // The result of
+                                            // interpolate(clamped_index)
+        public double distance; // The distance from closest_point to the query
+                                // point
+    }
+
+    public PathSegment(Translation2d start, Translation2d end, double speed) {
+        mEnd = end;
+        mSpeed = speed;
+        updateStart(start);
+    }
+
+    public void updateStart(Translation2d new_start) {
+        mStart = new_start;
+        mStartToEnd = mStart.inverse().translateBy(mEnd);
+        mLength = mStartToEnd.norm();
+    }
+
+    public double getSpeed() {
+        return mSpeed;
+    }
+
+    public Translation2d getStart() {
+        return mStart;
+    }
+
+    public Translation2d getEnd() {
+        return mEnd;
+    }
+
+    public double getLength() {
+        return mLength;
+    }
+
+    // Index is on [0, 1]
+    public Translation2d interpolate(double index) {
+        return mStart.interpolate(mEnd, index);
+    }
+
+    public double dotProduct(Translation2d other) {
+        Translation2d start_to_other = mStart.inverse().translateBy(other);
+        return mStartToEnd.getX() * start_to_other.getX() + mStartToEnd.getY() * start_to_other.getY();
+    }
+
+    public ClosestPointReport getClosestPoint(Translation2d query_point) {
+        ClosestPointReport rv = new ClosestPointReport();
+        if (mLength > kEpsilon) {
+            double dot_product = dotProduct(query_point);
+            rv.index = dot_product / (mLength * mLength);
+            rv.clamped_index = Math.min(1.0, Math.max(0.0, rv.index));
+            rv.closest_point = interpolate(rv.index);
+        } else {
+            rv.index = rv.clamped_index = 0.0;
+            rv.closest_point = new Translation2d(mStart);
+        }
+        rv.distance = rv.closest_point.inverse().translateBy(query_point).norm();
+        return rv;
+    }
+}

src/org/usfirst/frc4388/utility/RigidTransform2d.java

@@ -0,0 +1,131 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * Represents a 2d pose (rigid transform) containing translational and
+ * rotational elements.
+ * 
+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
+ */
+public class RigidTransform2d implements Interpolable<RigidTransform2d> {
+    private final static double kEps = 1E-9;
+
+    // Movement along an arc at constant curvature and velocity. We can use
+    // ideas from "differential calculus" to create new RigidTransform2d's from
+    // a Delta.
+    public static class Delta {
+        public final double dx;
+        public final double dy;
+        public final double dtheta;
+
+        public Delta(double dx, double dy, double dtheta) {
+            this.dx = dx;
+            this.dy = dy;
+            this.dtheta = dtheta;
+        }
+    }
+
+    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 fromVelocity(Delta 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));
+    }
+
+    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);
+    }
+
+    /**
+     * Do linear interpolation of this transform (there are more accurate ways
+     * using constant curvature, but this is good enough).
+     */
+    @Override
+    public RigidTransform2d interpolate(RigidTransform2d other, double x) {
+        if (x <= 0) {
+            return new RigidTransform2d(this);
+        } else if (x >= 1) {
+            return new RigidTransform2d(other);
+        }
+        return new RigidTransform2d(translation_.interpolate(other.translation_, x),
+                rotation_.interpolate(other.rotation_, x));
+    }
+
+    @Override
+    public String toString() {
+        return "T:" + translation_.toString() + ", R:" + rotation_.toString();
+    }
+}

src/org/usfirst/frc4388/utility/Rotation2d.java

@@ -0,0 +1,124 @@
+package org.usfirst.frc4388.utility;
+
+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 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 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 (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);
+    }
+
+    /**
+     * 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);
+    }
+
+    @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)";
+    }
+}

src/org/usfirst/frc4388/utility/SoftwarePIDController.java

@@ -0,0 +1,114 @@
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class SoftwarePIDController
+{	
+	protected ArrayList<CANTalonEncoder> 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<CANTalonEncoder> 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;
+		
+		// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
+		// for (CANTalonEncoder motorController : motorControllers) {
+		//     motorController.changeControlMode(TalonControlMode.PercentVbus);
+		// }
+		
+		prevError = 0.0;
+		totalError = 0.0;
+	
+	}
+	
+	public boolean controlLoopUpdate() {
+		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 (CANTalonEncoder motorController : motorControllers) {
+				//motorController.set(output);
+				motorController.set(ControlMode.PercentOutput, output);
+			}
+		}
+		else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
+			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 (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
+			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);
+				}
+			}
+		}
+
+		return false;
+	}	
+}

src/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java

@@ -0,0 +1,85 @@
+
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class SoftwarePIDPositionController
+{	
+	//protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected WPI_TalonSRX motorController;
+	protected PIDParams pidParams;
+	
+	protected PIDParams PValue;
+	
+	protected double targetEncoderPosition;
+	
+	protected double minTurnOutput = 0.002;
+	protected double maxError;
+	protected double minError;
+	protected double maxPrevError; ///??
+	protected double prevError = 0.0; // the prior error (used to compute velocity)
+	protected double totalError = 0.0; // the sum of the errors for use in the integral calc
+	
+	public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) 
+	{
+		this.motorController = elevatorLeft;
+		setP(PValue);
+	}
+    
+	public void setP(PIDParams PValue) 
+	{
+		this.PValue = PValue;
+	}
+	
+	public void setPIDPositionTarget(double targetPosition, double maxError, double minError)
+	{
+		this.targetEncoderPosition = targetPosition;
+		
+		this.maxError = maxError;
+		this.minError = minError;
+		//this.maxPrevError = maxPrevError;
+		
+		prevError = 0.0;
+		totalError = 0.0;
+	}
+	
+	public boolean controlLoopUpdate() 
+	{
+		return controlLoopUpdate(0);
+	}
+	
+	public boolean controlLoopUpdate(double currentEncoderPosition) 
+	{
+		// Calculate the motion profile feed forward and error feedback terms
+		double error = targetEncoderPosition - currentEncoderPosition;
+		//double deltaLastError = (error - prevError);
+		
+		//Updating the error
+		//totalError += error;
+		
+		// Check if we are finished
+		if (Math.abs(error) < maxError && Math.abs(error) > minError) 
+		{
+			//Robot.elevator.holdInPos();
+			
+			return true;
+		}
+		
+		double output =  pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError;
+		
+		prevError = error;
+		
+		// Update the controllers set point.
+		motorController.set(ControlMode.PercentOutput, output);
+		
+		return false;
+	}
+}

src/org/usfirst/frc4388/utility/Translation2d.java

@@ -0,0 +1,104 @@
+package org.usfirst.frc4388.utility;
+
+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 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_;
+    }
+
+    /**
+     * 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 getX() {
+        return x_;
+    }
+
+    public double getY() {
+        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());
+    }
+
+    /**
+     * 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_);
+    }
+
+    @Override
+    public String toString() {
+        final DecimalFormat fmt = new DecimalFormat("#0.000");
+        return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")";
+    }
+}