From 5d8f5d6578b45a5bb1d7b11916b4349b4caa3ef2 Mon Sep 17 00:00:00 2001
From: Gabe Mitnick <gabe.mitnick@gmail.com>
Date: Sun, 11 Mar 2018 15:48:29 -0700
Subject: [PATCH 01/36] make lift wiggle room and number of stages preferences

and remove repetition
---
 .../team199/Robot2018/commands/AutoLift.java  | 166 ++++++++--------
 .../team199/Robot2018/subsystems/Lift.java    |  23 ++-
 .../Robot2018/subsystems/LiftInterface.java   |  50 ++---
 .../frc/team199/Robot2018/PIDConstSim.java    | 178 ++++++++----------
 4 files changed, 214 insertions(+), 203 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
index 764551b..da41c15 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
@@ -12,103 +12,105 @@
 /**
  *
  */
-public class AutoLift extends Command implements PIDOutput{
+public class AutoLift extends Command implements PIDOutput {
 	/**
-	 * All distances are measured from bottom of cube and + 3 inches for wiggle room for dropping cubes
-	 * Also, actual distances are divided by 3 because according to cad, the lift will have a 1:3 ratio from winch
-	 * to actual height.
+	 * All distances are measured from bottom of cube and + 3 inches for wiggle room
+	 * for dropping cubes Also, actual distances are divided by 3 because according
+	 * to cad, the lift will have a 1:3 ratio from winch to actual height.
 	 */
-	
-	/**
-	 * Distance to switch
-	 * 18.75 inches in starting position (this measurement is the fence that surrounds the switch)
-	 * 21.75 / 3 for ratio = 7.25
-	 */
-	private final double SWITCH_DIST = 7.25;
-	/**
-	 * Distance to scale
-	 * 5 feet starting
-	 * 63 / 3 = 21
-	 */
-	private final double SCALE_DIST = 21;
-	/**
-	 * Distance to bar
-	 * 87 / 3 = 29
-	 * 7 feet starting; bar distance should be changed because I'm not aware how climber mech will be positioned
-	 */
-	private final double BAR_DIST = 29;
+
+	private final int NUM_STAGES;
+	private final double WIGGLE_ROOM;
+	private final double SWITCH_DIST;
+	private final double SCALE_DIST;
+	private final double BAR_DIST;
+
 	private double desiredDist = 0;
 	private double currDist;
 	private LiftInterface lift;
 	private Position desiredPos;
-	
+
 	private PIDController liftController;
 
-    public AutoLift(Position stage, LiftInterface lift) {
-    	this.lift = lift;
-    	requires(Robot.lift);
-    	currDist = lift.getHeight();
-    	switch(stage) {
-    	case GROUND:
-    		desiredDist = -currDist;
-    		break;
-    	case SWITCH:
-    		desiredDist = SWITCH_DIST - currDist;
-    		break;
-    	case SCALE:
-    		desiredDist = SCALE_DIST - currDist;
-    		break;
-    	case BAR:
-    		desiredDist = BAR_DIST - currDist;
-    		break;
-    	}
-    	
-    	liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
-				Robot.getConst("LiftkD", 0),RobotMap.liftEnc, this);
-    	
-    	desiredPos = stage;
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
+	public AutoLift(Position stage, LiftInterface lift) {
+		this.lift = lift;
+		requires(Robot.lift);
+
+		currDist = lift.getHeight();
+
+		// calculate constant measurements
+
+		NUM_STAGES = lift.getNumStages();
+		WIGGLE_ROOM = lift.getWiggleRoom();
+		// distance to switch 18.75 inches in starting position
+		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;
+		// distance to scale 5 feet starting 63 / 3 = 21
+		SCALE_DIST = (60.0 + WIGGLE_ROOM) / NUM_STAGES;
+		// 7 feet starting; bar distance should be changed because I'm not aware how
+		// climber mech will be positioned
+		BAR_DIST = (84.0 + WIGGLE_ROOM) / NUM_STAGES;
+
+		switch (stage) {
+		case GROUND:
+			desiredDist = -currDist;
+			break;
+		case SWITCH:
+			desiredDist = SWITCH_DIST - currDist;
+			break;
+		case SCALE:
+			desiredDist = SCALE_DIST - currDist;
+			break;
+		case BAR:
+			desiredDist = BAR_DIST - currDist;
+			break;
+		}
+
+		liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
+				Robot.getConst("LiftkD", 0), RobotMap.liftEnc, this);
+
+		desiredPos = stage;
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
 		// input is in inches
-		//liftController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
+		// liftController.setInputRange(-Robot.getConst("Max High Speed", 204),
+		// Robot.getConst("Max High Speed", 204));
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
 		liftController.setOutputRange(-1.0, 1.0);
 		liftController.setContinuous(false);
-		//liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
+		// liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
 		liftController.setSetpoint(desiredDist);
 		liftController.enable();
-    }
-
-    // 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(liftController.onTarget()) {
-    		lift.setCurrPosition(desiredPos);
-    		return true;
-    	}
-    	return false;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    	liftController.disable();
-    	liftController.free();
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    	end();
-    }
-    
-    @Override
+	}
+
+	// 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 (liftController.onTarget()) {
+			lift.setCurrPosition(desiredPos);
+			return true;
+		}
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	protected void end() {
+		liftController.disable();
+		liftController.free();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+		end();
+	}
+
+	@Override
 	public void pidWrite(double output) {
 		lift.runMotor(output);
 	}
-    
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 4eb76db..af2a227 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -18,6 +18,14 @@ public class Lift extends Subsystem implements LiftInterface {
 	private final Encoder liftEnc = RobotMap.liftEnc;
 	private Position currPosition = Position.GROUND;
 
+	private final int NUM_STAGES;
+	private final double WIGGLE_ROOM;
+
+	public Lift() {
+		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
+		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0);
+	}
+
 	/**
 	 * Set the default command for a subsystem here.
 	 */
@@ -51,7 +59,7 @@ public void setCurrPosition(Position newPosition) {
 	 * Uses (insert sensor here) to detect the current lift position
 	 */
 	public double getHeight() {
-		return liftEnc.getDistance() * 3;
+		return liftEnc.getDistance() * NUM_STAGES;
 	}
 
 	/**
@@ -94,4 +102,17 @@ public void resetEnc() {
 		liftEnc.reset();
 	}
 
+	/**
+	 * Gets the number of stages variable
+	 */
+	public int getNumStages() {
+		return NUM_STAGES;
+	}
+
+	/**
+	 * Gets the extra distance above the switch or scale we want to lift in inches
+	 */
+	public double getWiggleRoom() {
+		return WIGGLE_ROOM;
+	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
index 8ba11d4..6e6e006 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
@@ -1,58 +1,66 @@
 package org.usfirst.frc.team199.Robot2018.subsystems;
 
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
-
 public interface LiftInterface {
-	
+
 	/**
 	 * Set the default command for a subsystem here.
-	 * */
+	 */
 	public void initDefaultCommand();
-	
+
 	public enum Position {
-		GROUND,
-		SWITCH,
-		SCALE,
-		BAR
+		GROUND, SWITCH, SCALE, BAR
 	}
-	
+
 	/**
-	 * Uses (insert sensor here) to detect the current lift position 
+	 * Uses (insert sensor here) to detect the current lift position
 	 */
 	public double getHeight();
-	
+
 	/**
 	 * stops the lift
 	 */
 	public void stopLift();
-	
+
 	/**
 	 * gets current motor values
 	 */
 	public double getLiftSpeed();
-	
+
 	/**
 	 * Runs lift motors at specified speed
-	 * @param speed - desired speed to run at
+	 * 
+	 * @param speed
+	 *            - desired speed to run at
 	 */
 	public void runMotor(double speed);
-	
+
 	/**
 	 * Returns the position the lift is currently at
+	 * 
 	 * @return pos - current position
 	 */
 	public Position getCurrPos();
-	
+
 	/**
 	 * Resets the encoder
 	 */
 	public void resetEnc();
-	
+
 	/**
 	 * Sets the current position in the lift subsystem
-	 * @param newPosition - the new position meant to be set
+	 * 
+	 * @param newPosition
+	 *            - the new position meant to be set
 	 */
 	public void setCurrPosition(Position newPosition);
-	
-	
+
+	/**
+	 * Gets the number of stages variable
+	 */
+	public int getNumStages();
+
+	/**
+	 * Gets the extra distance above the switch or scale we want to lift in inches
+	 */
+	public double getWiggleRoom();
 }
diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java
index 6359c69..8e96322 100644
--- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java
+++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java
@@ -1,18 +1,13 @@
 package org.usfirst.frc.team199.Robot2018;
 
-import org.junit.jupiter.api.*;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.junit.runner.RunWith;
-import org.mockito.invocation.InvocationOnMock;
-import org.mockito.stubbing.Answer;
-
 import static org.junit.Assert.assertTrue;
 import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.when;
 
-import java.util.Arrays;
-import java.util.Collection;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.mockito.invocation.InvocationOnMock;
+import org.mockito.stubbing.Answer;
 
 import edu.wpi.first.wpilibj.HLUsageReporting;
 import edu.wpi.first.wpilibj.PIDController;
@@ -21,32 +16,29 @@
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.internal.HardwareTimer;
-import org.junit.runners.Parameterized;
-import org.junit.runners.Parameterized.Parameters;
-import org.junit.runners.Parameterized.Parameter;
 
 //@RunWith(value = Parameterized.class)
 class PIDConstSim {
 	static final double initkP = 10;
 	static final double initkI = 6;
 	static final double initkD = 6;
-	
-	//@Parameter(value = )
+
+	// @Parameter(value = )
 	double liftkP = initkP;
 	double liftkI = initkI;
 	double liftkD = initkD;
-	
+
 	private final double targetOne = 24;
 	private final double targetTwo = 60;
 	private final double targetThree = 45;
 	// 1 in in meters
 	private final double tolerance = 0.0254;
-	
+
 	double position = 0;
 	double velocity_ = 0;
 	double voltage_ = 0;
 	double offset_ = -0.1;
-	
+
 	// Stall Torque in N m
 	static final double kStallTorque = 2.41;
 	// Stall Current in Amps
@@ -63,8 +55,7 @@ class PIDConstSim {
 	// Resistance of the motor
 	static final double kResistance = 12.0 / kStallCurrent;
 	// Motor velocity constant
-	static final double Kv = ((kFreeSpeed / 60.0 * 2.0 * Math.PI) /
-	             (12.0 - kResistance * kFreeCurrent));
+	static final double Kv = ((kFreeSpeed / 60.0 * 2.0 * Math.PI) / (12.0 - kResistance * kFreeCurrent));
 	// Torque constant
 	static final double Kt = (kNumMotors * kStallTorque) / kStallCurrent;
 	// Gear ratio
@@ -75,61 +66,46 @@ class PIDConstSim {
 
 	// Control loop time step
 	static final double kDt = 0.01;
-	    
+
 	double current_time = 0;
-	
-	  // V = I * R + omega / Kv
-	  // torque = Kt * I
-	
-	
+
+	// V = I * R + omega / Kv
+	// torque = Kt * I
+
 	// (name = "{index}: testAdd({0}+{1}) = {2}")
-	/*@Parameters
-    public static double[][] data() {
-    	// array is P I and D finalants in order
-        //double [][][] PIDfinalTable = new double[5][5][5];
-		double [][] PIDfinalTable = new double[3][5];
-        double diffInter3 = initkD / 10;
-        double diffInter2 = initkI / 10;
-        double diffInter1 = initkP / 10;
-        for(int i = 0; i < 3; i++) {
-        	for(int j = -2; j < 3; j++) {
-        		double res;
-        		if(i == 0)
-        			res = initkP + j * diffInter1;
-        		else if(i == 1)
-        			res = initkI + j * diffInter2;
-        		else
-        			res = initkD + j * diffInter3;
-        		PIDfinalTable[i][j] = res;
-        	}
-        }
-    	//return (Iterable<Double[]>)Arrays.asList(PIDfinalTable);
-        return PIDfinalTable;
-    }
-    */
-	
+	/*
+	 * @Parameters public static double[][] data() { // array is P I and D finalants
+	 * in order //double [][][] PIDfinalTable = new double[5][5][5]; double [][]
+	 * PIDfinalTable = new double[3][5]; double diffInter3 = initkD / 10; double
+	 * diffInter2 = initkI / 10; double diffInter1 = initkP / 10; for(int i = 0; i <
+	 * 3; i++) { for(int j = -2; j < 3; j++) { double res; if(i == 0) res = initkP +
+	 * j * diffInter1; else if(i == 1) res = initkI + j * diffInter2; else res =
+	 * initkD + j * diffInter3; PIDfinalTable[i][j] = res; } } //return
+	 * (Iterable<Double[]>)Arrays.asList(PIDfinalTable); return PIDfinalTable; }
+	 */
+
 	private double GetAcceleration(double voltage) {
-	    return -Kt * kG * kG / (Kv * kResistance * kr * kr * kMass) * velocity_ +
-	           kG * Kt / (kResistance * kr * kMass) * voltage;
-	  }
-	
+		return -Kt * kG * kG / (Kv * kResistance * kr * kr * kMass) * velocity_
+				+ kG * Kt / (kResistance * kr * kMass) * voltage;
+	}
+
 	private void simulateTime(double time, double voltage) {
 		final double starting_time = time;
-	    while (time > 0) {
-	      final double current_dt = Math.min(time, 0.001);
-	      //System.out.println("vel before: " + velocity_);
-	      position += current_dt * velocity_;
-	      double acc = GetAcceleration(voltage);
-	      //System.out.println("acc: " + acc);
-	      velocity_ += current_dt * acc;
-	      //System.out.println("vel after: " + velocity_);
-	      time -= 0.001;
-	      //EXPECT_LE(position, ElevatorLoop::kMaxHeight + 0.01);
-	      //EXPECT_GE(position, ElevatorLoop::kMinHeight - 0.01);
-	    }
-	    current_time += starting_time;
+		while (time > 0) {
+			final double current_dt = Math.min(time, 0.001);
+			// System.out.println("vel before: " + velocity_);
+			position += current_dt * velocity_;
+			double acc = GetAcceleration(voltage);
+			// System.out.println("acc: " + acc);
+			velocity_ += current_dt * acc;
+			// System.out.println("vel after: " + velocity_);
+			time -= 0.001;
+			// EXPECT_LE(position, ElevatorLoop::kMaxHeight + 0.01);
+			// EXPECT_GE(position, ElevatorLoop::kMinHeight - 0.01);
+		}
+		current_time += starting_time;
 	}
-	
+
 	@BeforeEach
 	void setUp() {
 		// Since VelocityPIDController extends PIDController and PIDController calls
@@ -140,50 +116,50 @@ void setUp() {
 		HardwareTimer tim = mock(HardwareTimer.class);
 		Timer.Interface timerInstance = mock(Timer.Interface.class);
 		when(tim.newTimer()).thenReturn(timerInstance);
-		when(timerInstance.get()).thenAnswer(
-			     new Answer() {
-			         public Object answer(InvocationOnMock invocation) {
-			             //return getTime();
-			        	 return current_time;
-			         }
-			 });
-		
+		when(timerInstance.get()).thenAnswer(new Answer() {
+			public Object answer(InvocationOnMock invocation) {
+				// return getTime();
+				return current_time;
+			}
+		});
+
 		Timer.SetImplementation(tim);
 		HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class);
 		HLUsageReporting.SetImplementation(usageReporter);
 	}
-	
-	class PIDSim{
+
+	class PIDSim {
 		public PIDSim() {
-			//position = 0;
+			// position = 0;
 		}
-		
+
 		public double pidGet() {
 			return position;
 		}
-		
+
 		public void pidWrite(double output) {
-			//System.out.println("voltage: " + output);
+			// System.out.println("voltage: " + output);
 			simulateTime(kDt, output * 12);
 			System.out.println("position: " + position);
 		}
 	}
-	
-	class PIDSimController extends PIDController{
-		
+
+	class PIDSimController extends PIDController {
+
 		public PIDSimController(double p, double i, double d, PIDSource src, PIDOutput out) {
-			super(p,i,d,src,out);
+			super(p, i, d, src, out);
 		}
-		
+
 		public void calc() {
 			this.calculate();
 		}
-		
+
 	}
-	
-	class PIDSimSource implements PIDSource{
+
+	class PIDSimSource implements PIDSource {
 		private PIDSim sim;
 		private PIDSourceType type;
+
 		public PIDSimSource(PIDSim sim) {
 			this.sim = sim;
 		}
@@ -197,25 +173,28 @@ public PIDSourceType getPIDSourceType() {
 		public double pidGet() {
 			return sim.pidGet();
 		}
-		
+
 		@Override
 		public void setPIDSourceType(PIDSourceType pidSource) {
 			// TODO Auto-generated method stub
 			type = pidSource;
 		}
 	}
-	
-	class PIDSimOutput implements PIDOutput{
+
+	class PIDSimOutput implements PIDOutput {
 		private PIDSim sim;
+
 		public PIDSimOutput(PIDSim sim) {
 			this.sim = sim;
 		}
+
 		@Override
 		public void pidWrite(double output) {
 			sim.pidWrite(output);
 		}
 	}
 
+	@Test
 	void test24() {
 		PIDSim sim = new PIDSim();
 		PIDSimSource src = new PIDSimSource(sim);
@@ -225,8 +204,8 @@ void test24() {
 		liftController.setSetpoint(0.6);
 		liftController.setOutputRange(-1.0, 1.0);
 		liftController.enable();
-		//1 second?
-		for(int i = 0; i < 100; i++) {
+		// 1 second?
+		for (int i = 0; i < 100; i++) {
 			liftController.calc();
 		}
 		System.out.println("Test" + targetOne + ": " + sim.pidGet());
@@ -235,6 +214,7 @@ void test24() {
 		assertTrue(Math.abs(0.6 - sim.pidGet()) < tolerance);
 	}
 
+	@Test
 	void test60() {
 		PIDSim sim = new PIDSim();
 		PIDSimSource src = new PIDSimSource(sim);
@@ -244,8 +224,8 @@ void test60() {
 		liftController.setSetpoint(1.524);
 		liftController.setOutputRange(-1.0, 1.0);
 		liftController.enable();
-		//1 second?
-		for(int i = 0; i < 200; i++) {
+		// 1 second?
+		for (int i = 0; i < 200; i++) {
 			liftController.calc();
 		}
 		System.out.println("" + sim.pidGet());
@@ -263,8 +243,8 @@ void test45() {
 		liftController.setSetpoint(1.143);
 		liftController.setOutputRange(-1.0, 1.0);
 		liftController.enable();
-		//1 second?
-		for(int i = 0; i < 100; i++) {
+		// 1 second?
+		for (int i = 0; i < 100; i++) {
 			liftController.calc();
 		}
 		System.out.println("" + sim.pidGet());

From 1f762ad5662945ab66b823d6a1b7652a964c95b4 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Sun, 11 Mar 2018 15:59:51 -0700
Subject: [PATCH 02/36] Fixes to FindTurnTimeConstant and PIDTurn.

---
 .../usfirst/frc/team199/Robot2018/Robot.java  |  3 +-
 .../commands/FindTurnTimeConstant.java        | 51 ++++++++++++-------
 .../team199/Robot2018/commands/PIDTurn.java   | 22 ++++----
 3 files changed, 48 insertions(+), 28 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 045fda7..ba9e39b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -245,7 +245,8 @@ public void teleopPeriodic() {
 		// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
 		//
 
-		System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
+		// System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(),
+		// RobotMap.dtRight.get());
 
 		SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
 		SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java
index eb8d6d9..2bc12e7 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java
@@ -20,20 +20,20 @@ public class FindTurnTimeConstant extends Command {
 	private SmartDashboardInterface sd;
 	private AHRS gyro;
 	private Robot robot;
-	private Timer tim = new Timer();
-	private ArrayList<DataPoint> dataPoints = new ArrayList<DataPoint>();
+	private Timer tim;
+	private ArrayList<DataPoint> dataPoints;
 
 	private class DataPoint {
 		public double seconds;
-		public double degreesPerSecond;
+		public double radiansPerSecond;
 
-		public DataPoint(double seconds, double degreesPerSecond) {
+		public DataPoint(double seconds, double radiansPerSecond) {
 			this.seconds = seconds;
-			this.degreesPerSecond = degreesPerSecond;
+			this.radiansPerSecond = radiansPerSecond;
 		}
 
 		public String toString() {
-			return "Time: " + seconds + " seconds; rotational velocity: " + degreesPerSecond + " degrees per second";
+			return "Time: " + seconds + " seconds; rotational velocity: " + radiansPerSecond + " radians per second";
 		}
 	}
 
@@ -53,12 +53,18 @@ public FindTurnTimeConstant(Robot robot, DrivetrainInterface dt, AHRS gyro, Smar
 	// Called just before this Command runs the first time
 	protected void initialize() {
 		System.out.println("FindTurnTimeConstant is in init()");
-		if (!robot.isTest()) {
-			System.out.println("but you're not in test mode.");
-			return;
-		}
+		// if (!robot.isTest()) {
+		// System.out.println("but you're not in test mode.");
+		// return;
+		// }
+
+		// this should be done in low gear
+		Robot.dt.shiftGears(false);
+
+		dataPoints = new ArrayList<DataPoint>();
 
 		System.out.println("and you're in test mode");
+		tim = new Timer();
 		tim.start();
 		// spin at full speed
 		dt.arcadeDrive(0, 1);
@@ -70,6 +76,7 @@ protected void execute() {
 		DataPoint newMeasurement = new DataPoint(tim.get(), gyro.getRate());
 		System.out.println(newMeasurement);
 		dataPoints.add(newMeasurement);
+		dt.arcadeDrive(0, 1);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
@@ -78,11 +85,16 @@ protected boolean isFinished() {
 		// true if the last and second-to-last rotational velocities measured are within
 		// 5 degrees per second squared of each other. or if it's not a test, finish
 		// immediately
-		if (numberOfDataPoints < 2) {
+
+		// if we're still in the first 51 cycles (don't have enough data), then not
+		// finished. also, we're probably still accelerating if we haven't reached 3
+		// radians per second (the max will be around 5).
+		if (numberOfDataPoints < 51 || Math.abs(dataPoints.get(numberOfDataPoints - 1).radiansPerSecond) < 3.0) {
 			return false;
 		}
-		return (Math.abs(dataPoints.get(numberOfDataPoints - 1).degreesPerSecond
-				- dataPoints.get(numberOfDataPoints - 2).degreesPerSecond) < 5) || !robot.isTest();
+
+		return (Math.abs(dataPoints.get(numberOfDataPoints - 1).radiansPerSecond
+				- dataPoints.get(numberOfDataPoints - 51).radiansPerSecond) < 0.02);
 	}
 
 	// Called once after isFinished returns true
@@ -90,14 +102,17 @@ protected void end() {
 		// stop moving
 		dt.arcadeDrive(0, 0);
 
-		DataPoint lastDataPoint = dataPoints.get(dataPoints.size() - 1);
-		System.out.println("Finished when " + lastDataPoint);
-		// the rotvel of the data point we're looking for to find the time constant
-		double magicValue = lastDataPoint.degreesPerSecond * (1 - (1 / Math.E));
+		double lastRadiansPerSecond = dataPoints.get(dataPoints.size() - 1).radiansPerSecond;
+		System.out.println("Finished at " + lastRadiansPerSecond + " radians per second");
+		Robot.putConst("Max Turn Radians Per Second", lastRadiansPerSecond);
+
+		// the radians per second of the data point we're looking for to find the time
+		// constant
+		double magicValue = lastRadiansPerSecond * (1 - (1 / Math.E));
 
 		for (DataPoint datum : dataPoints) {
 			// if we've past the magic value, we're done
-			if (Math.abs(datum.degreesPerSecond) > Math.abs(magicValue)) {
+			if (Math.abs(datum.radiansPerSecond) > Math.abs(magicValue)) {
 				System.out.println("The magic value is " + datum);
 				Robot.putConst("TurnTimeConstant", datum.seconds);
 				// stop checking data
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
index c665b04..b4dae99 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
@@ -68,15 +68,7 @@ public PIDTurn(double target, double[] point, DrivetrainInterface dt, SmartDashb
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
-		// calculates the maximum turning speed in degrees/sec based on the max linear
-		// speed in inches/s and the distance (inches) between sides of the DT
-		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
-		double r = Robot.getConst("TurnPidR", 3.0);
-		double kP = r / Robot.rmap.getDrivetrainTimeConstant() / maxTurnSpeed;
-		double kI = 0;
-		double kD = r / maxTurnSpeed;
-		double kF = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
-		turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
+		turnController = new PIDController(0, 0, 0, 0, ahrs, this);
 		// tim = new Timer();
 		sd.putData("Turn PID", turnController);
 	}
@@ -156,6 +148,18 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	protected void initialize() {
+		// calculate pid constants
+
+		// max turn speed from FindTurnTimeConstant, converted to degrees
+		double maxTurnSpeed = Robot.getConst("Max Turn Radians Per Second", 4.0) * 180 / Math.PI;
+		double updateTime = sd.getConst("Default PID Update Time", 0.05);
+		double r = Robot.getConst("TurnPidR", 3.0);
+		
+		double kP = r / Robot.getConst("TurnTimeConstant", 0.2) / maxTurnSpeed;
+		double kI = 0;
+		double kD = r / (maxTurnSpeed * updateTime);
+		double kF = 1 / (maxTurnSpeed * updateTime);
+		turnController.setPID(kP, kI, kD, kF);
 
 		if (!turnToPoint) {
 			if (absoluteRotation) {

From 03f815a3cef13bce47cc0ae2b21d12d806086f18 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Sun, 11 Mar 2018 16:03:26 -0700
Subject: [PATCH 03/36] Insignificant shuffleboard changes.

---
 shuffleboard.json | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index d6775cd..558854a 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -87,7 +87,7 @@
             "content": {
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
-              "_title": "SmartDashboard/Drivetrain/Left VPID Targ",
+              "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
               "Visible time": 30.0,
               "SmartDashboard/Drivetrain/Left VPID Targ visible": true
             }
@@ -100,7 +100,7 @@
             "content": {
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
-              "_title": "SmartDashboard/Drivetrain/Right VPID Targ",
+              "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
               "Visible time": 30.0,
               "SmartDashboard/Drivetrain/Right VPID Targ visible": true
             }
@@ -385,7 +385,7 @@
             "content": {
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
-              "_title": "SmartDashboard/Drivetrain/Left VPID Targ",
+              "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
               "Visible time": 10.0,
               "SmartDashboard/Drivetrain/Left VPID Targ visible": true
             }
@@ -398,7 +398,7 @@
             "content": {
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
-              "_title": "SmartDashboard/Drivetrain/Right VPID Targ",
+              "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
               "Visible time": 10.0,
               "SmartDashboard/Drivetrain/Right VPID Targ visible": true
             }

From 93926ff08fedcb34b76a2eb5d351dd5b413d5a12 Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Mon, 12 Mar 2018 20:56:55 -0700
Subject: [PATCH 04/36] new lift setup

made Lift extend PIDSubsystem and implemented the necessary methods for
that (need to do spd --> voltage method though)
made new command (UpdateLiftPosition) for continuous teleop PID -->
default Lift command
need to make commands for auto
---
 .../usfirst/frc/team199/Robot2018/Robot.java  |  8 ++-
 .../commands/UpdateLiftPosition.java          | 64 +++++++++++++++++
 .../team199/Robot2018/subsystems/Lift.java    | 68 ++++++++++++++-----
 3 files changed, 121 insertions(+), 19 deletions(-)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 045fda7..6405f16 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -59,6 +59,7 @@ public class Robot extends IterativeRobot {
 	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
 	public static SmartDashboardInterface sd = new SmartDashboardInterface() {
+		@Override
 		public double getConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			if (!pref.containsKey("Const/" + key)) {
@@ -71,6 +72,7 @@ public double getConst(String key, double def) {
 			return pref.getDouble("Const/" + key, def);
 		}
 
+		@Override
 		public void putConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			pref.putDouble("Const/" + key, def);
@@ -79,14 +81,17 @@ public void putConst(String key, double def) {
 			}
 		}
 
+		@Override
 		public void putData(String string, PIDController controller) {
 			SmartDashboard.putData(string, controller);
 		}
 
+		@Override
 		public void putNumber(String string, double d) {
 			SmartDashboard.putNumber(string, d);
 		}
 
+		@Override
 		public void putBoolean(String string, boolean b) {
 			SmartDashboard.putBoolean(string, b);
 		}
@@ -130,7 +135,8 @@ public void robotInit() {
 		climber = new Climber();
 		climberAssist = new ClimberAssist();
 		intakeEject = new IntakeEject();
-		lift = new Lift();
+		lift = new Lift("Lift", getConst("LiftkP", 0.1), getConst("LiftkI", 0), getConst("LiftkD", 0),
+				getConst("LiftkF", 0.1));
 		dt = new Drivetrain(sd);
 		oi = new OI(this);
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
new file mode 100644
index 0000000..f636774
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -0,0 +1,64 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command should be the default command for the Lift subsystem
+ */
+public class UpdateLiftPosition extends Command {
+
+	private Lift lift;
+	private double targ;
+
+	public UpdateLiftPosition(Lift lift) {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		this.lift = lift;
+		targ = 0;
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		/**
+		 * May need to setInputRange(0, maxHeight);
+		 */
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	@Override
+	protected void execute() {
+		int angle = Robot.oi.manipulator.getPOV();
+		/**
+		 * reference AutoLift for how to calculate targ (below) based on the given
+		 * Position enum
+		 */
+		if (angle == 0) {
+			// targ = GROUND
+		} else {
+			// targ = SWITCH
+		}
+		lift.setSetpoint(targ);
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	@Override
+	protected boolean isFinished() {
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	@Override
+	protected void end() {
+		lift.disable();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	@Override
+	protected void interrupted() {
+	}
+}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index af2a227..7e62e4b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -2,17 +2,16 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
-import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
+import org.usfirst.frc.team199.Robot2018.commands.UpdateLiftPosition;
 
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 /**
  *
  */
-public class Lift extends Subsystem implements LiftInterface {
+public class Lift extends PIDSubsystem implements LiftInterface {
 
 	private final SpeedControllerGroup liftMotors = RobotMap.liftMotors;
 	private final Encoder liftEnc = RobotMap.liftEnc;
@@ -21,28 +20,19 @@ public class Lift extends Subsystem implements LiftInterface {
 	private final int NUM_STAGES;
 	private final double WIGGLE_ROOM;
 
-	public Lift() {
+	public Lift(String name, double kP, double kI, double kD, double kF) {
+		super(name, kP, kI, kD, kF);
 		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0);
+		enable();
 	}
 
 	/**
 	 * Set the default command for a subsystem here.
 	 */
+	@Override
 	public void initDefaultCommand() {
-		// Set the default command for a subsystem here.
-		// setDefaultCommand(new MySpecialCommand());
-		int angle = Robot.oi.manipulator.getPOV();
-
-		Command com = null;
-
-		if (angle == 0) {
-			com = new AutoLift(Position.GROUND, this);
-			setDefaultCommand(com);
-		} else if (angle == 90 || angle == 180 || angle == 270) {
-			com = new AutoLift(Position.SWITCH, this);
-			setDefaultCommand(com);
-		}
+		setDefaultCommand(new UpdateLiftPosition(this));
 	}
 
 	/**
@@ -51,6 +41,7 @@ public void initDefaultCommand() {
 	 * @param newPosition
 	 *            - the new position meant to be set
 	 */
+	@Override
 	public void setCurrPosition(Position newPosition) {
 		currPosition = newPosition;
 	}
@@ -58,6 +49,7 @@ public void setCurrPosition(Position newPosition) {
 	/**
 	 * Uses (insert sensor here) to detect the current lift position
 	 */
+	@Override
 	public double getHeight() {
 		return liftEnc.getDistance() * NUM_STAGES;
 	}
@@ -65,6 +57,7 @@ public double getHeight() {
 	/**
 	 * stops the lift
 	 */
+	@Override
 	public void stopLift() {
 		liftMotors.stopMotor();
 	}
@@ -72,6 +65,7 @@ public void stopLift() {
 	/**
 	 * gets current motor values
 	 */
+	@Override
 	public double getLiftSpeed() {
 		return liftMotors.get();
 	}
@@ -82,6 +76,7 @@ public double getLiftSpeed() {
 	 * @param speed
 	 *            - desired speed to run at
 	 */
+	@Override
 	public void runMotor(double output) {
 		liftMotors.set(output);
 	}
@@ -91,6 +86,7 @@ public void runMotor(double output) {
 	 * 
 	 * @return pos - current position
 	 */
+	@Override
 	public Position getCurrPos() {
 		return currPosition;
 	}
@@ -98,6 +94,7 @@ public Position getCurrPos() {
 	/**
 	 * Resets the encoder
 	 */
+	@Override
 	public void resetEnc() {
 		liftEnc.reset();
 	}
@@ -105,6 +102,7 @@ public void resetEnc() {
 	/**
 	 * Gets the number of stages variable
 	 */
+	@Override
 	public int getNumStages() {
 		return NUM_STAGES;
 	}
@@ -112,7 +110,41 @@ public int getNumStages() {
 	/**
 	 * Gets the extra distance above the switch or scale we want to lift in inches
 	 */
+	@Override
 	public double getWiggleRoom() {
 		return WIGGLE_ROOM;
 	}
+
+	/**
+	 * Uses AMT103 Encoder to detect the current lift height with respect to the
+	 * lift's min height (inches)
+	 */
+	@Override
+	protected double returnPIDInput() {
+		// return getHeight(); //Use this instead? What's the difference?
+		return liftEnc.getDistance();
+	}
+
+	/**
+	 * Runs the lift motors at the value that the pid loop calculated. Used
+	 * internally by PIDSubsystem.
+	 */
+	@Override
+	protected void usePIDOutput(double output) {
+		double out = output;
+		double spd = liftEnc.getRate();
+		out += convertSpdToVoltage(spd);
+		runMotor(out);
+	}
+
+	/**
+	 * Takes a value for the current lift speed and translates it to the amount of
+	 * voltage to motors need to supply.
+	 * 
+	 * @param speed
+	 *            - the current lift speed (in/s)
+	 */
+	public double convertSpdToVoltage(double speed) {
+		return 0;
+	}
 }

From ae3344f912469cae9d062551ca087b35574da3ab Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 00:08:27 -0700
Subject: [PATCH 05/36] lots of Lift functionalities added

implemented LiftToPosition, MoveLift, MoveLiftWithPID
added buttons for the new commands (w/ some finagling for LiftToPosition
params)
see https://trello.com/c/7OOvkOJG/167-implement-pid-on-lift-encoder for
details in checklist
---
 .../org/usfirst/frc/team199/Robot2018/OI.java | 21 ++++++-
 .../usfirst/frc/team199/Robot2018/Robot.java  | 20 +++++-
 .../Robot2018/SmartDashboardInterface.java    |  2 +
 .../team199/Robot2018/commands/AutoLift.java  |  6 +-
 .../Robot2018/commands/LiftToPosition.java    | 63 +++++++++++++++++++
 .../commands/{RunLift.java => MoveLift.java}  | 23 ++++---
 .../Robot2018/commands/MoveLiftWithPID.java   | 62 ++++++++++++++++++
 .../commands/UpdateLiftPosition.java          | 60 +++++++++++++-----
 .../team199/Robot2018/subsystems/Lift.java    | 55 ++++++++++++++--
 .../Robot2018/subsystems/LiftInterface.java   | 30 +++++++--
 10 files changed, 300 insertions(+), 42 deletions(-)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
 rename Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/{RunLift.java => MoveLift.java} (70%)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index e8d42ba..2c65103 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -10,12 +10,14 @@
 import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
 import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
 import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
+import org.usfirst.frc.team199.Robot2018.commands.LiftToPosition;
+import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
+import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
 import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
 import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
-import org.usfirst.frc.team199.Robot2018.commands.RunLift;
 import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
@@ -23,6 +25,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
 import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -48,6 +51,9 @@ public class OI {
 	private JoystickButton resetEncButton;
 	private JoystickButton moveLiftUpButton;
 	private JoystickButton moveLiftDownButton;
+	private JoystickButton moveLiftPIDUpButton;
+	private JoystickButton moveLiftPIDDownButton;
+	private JoystickButton testLiftPID;
 	private JoystickButton findTurnTimeConstantButton;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
@@ -92,6 +98,15 @@ public OI(Robot robot) {
 		findTurnTimeConstantButton
 				.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
 
+		moveLiftPIDUpButton = new JoystickButton(leftJoy, getButton("Run Lift PID Up", 3));
+		moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
+		moveLiftPIDDownButton = new JoystickButton(leftJoy, getButton("Run Lift PID Down", 4));
+		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
+
+		testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
+		testLiftPID.whenPressed(
+				new LiftToPosition(Robot.lift, Robot.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
+
 		rightJoy = new Joystick(1);
 		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
 		shiftHighGearButton.whenPressed(new ShiftHighGear());
@@ -104,9 +119,9 @@ public OI(Robot robot) {
 		updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
 
 		moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
-		moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
+		moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true));
 		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
-		moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
+		moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
 
 		manipulator = new Joystick(2);
 		if (manipulator.getButtonCount() == 0) {
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 6405f16..9a1bda3 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -72,6 +72,19 @@ public double getConst(String key, double def) {
 			return pref.getDouble("Const/" + key, def);
 		}
 
+		@Override
+		public String getString(String key, String def) {
+			Preferences pref = Preferences.getInstance();
+			if (!pref.containsKey("String/" + key)) {
+				pref.putString("String/" + key, def);
+				if (pref.getString("String/ + key", def) != def) {
+					System.err.println("pref Key" + "String/" + key + "already taken by a different type");
+					return def;
+				}
+			}
+			return pref.getString("String/" + key, def);
+		}
+
 		@Override
 		public void putConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
@@ -109,6 +122,10 @@ public static double getConst(String key, double def) {
 		return sd.getConst(key, def);
 	}
 
+	public static String getString(String key, String def) {
+		return sd.getString(key, def);
+	}
+
 	public static void putConst(String key, double def) {
 		sd.putConst(key, def);
 	}
@@ -135,8 +152,7 @@ public void robotInit() {
 		climber = new Climber();
 		climberAssist = new ClimberAssist();
 		intakeEject = new IntakeEject();
-		lift = new Lift("Lift", getConst("LiftkP", 0.1), getConst("LiftkI", 0), getConst("LiftkD", 0),
-				getConst("LiftkF", 0.1));
+		lift = new Lift();
 		dt = new Drivetrain(sd);
 		oi = new OI(this);
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java
index 7dba315..fc36820 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java
@@ -12,4 +12,6 @@ public interface SmartDashboardInterface {
 	public void putNumber(String string, double d);
 
 	public void putBoolean(String string, boolean b);
+
+	public String getString(String key, String def);
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
index da41c15..604ef31 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
@@ -3,7 +3,7 @@
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
 import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
@@ -28,11 +28,11 @@ public class AutoLift extends Command implements PIDOutput {
 	private double desiredDist = 0;
 	private double currDist;
 	private LiftInterface lift;
-	private Position desiredPos;
+	private LiftHeight desiredPos;
 
 	private PIDController liftController;
 
-	public AutoLift(Position stage, LiftInterface lift) {
+	public AutoLift(LiftHeight stage, LiftInterface lift) {
 		this.lift = lift;
 		requires(Robot.lift);
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
new file mode 100644
index 0000000..1e2da63
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
@@ -0,0 +1,63 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class LiftToPosition extends Command {
+
+	private Lift lift;
+	private LiftHeight pos;
+
+	public LiftToPosition(Lift lift, String goal) {
+		requires(Robot.lift);
+		this.lift = lift;
+		pos = LiftHeight.toLH(goal);
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		// There's no way to reset the Lift's PID constants after constructing it, so if
+		// you change them you must Restart Robot Code in DS.
+		double setpoint = lift.getDesiredDistFromPos(pos);
+		lift.setSetpoint(setpoint);
+		System.out.println("Target Height: " + setpoint);
+		lift.enable();
+		System.out.println("Enabled");
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	@Override
+	protected void execute() {
+		System.out.println("Current Height: " + lift.getHeight());
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	@Override
+	protected boolean isFinished() {
+		return lift.onTarget();
+	}
+
+	// Called once after isFinished returns true
+	@Override
+	protected void end() {
+		lift.disable();
+		System.out.println("Disabled");
+		// keep stopLift() after disable() so they don't conflict
+		lift.stopLift();
+		System.out.println("Stopped");
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	@Override
+	protected void interrupted() {
+		end();
+	}
+}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
similarity index 70%
rename from Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java
rename to Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
index 1f5baae..99fabc5 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
@@ -1,24 +1,22 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
- *
+ * Use this command to test the Lift without PID
  */
-public class RunLift extends Command {
+public class MoveLift extends Command {
 
-	private LiftInterface lift;
-	private final double SPEED = 0.05;
+	private Lift lift;
+	private double SPEED;
 	private int dir;
 
-	public RunLift(LiftInterface lift, boolean up) {
-		// Use requires() here to declare subsystem dependencies
-		// eg. requires(chassis);
-		this.lift = lift;
+	public MoveLift(Lift lift, boolean up) {
 		requires(Robot.lift);
+		this.lift = lift;
 		if (up)
 			dir = 1;
 		else
@@ -26,26 +24,33 @@ public RunLift(LiftInterface lift, boolean up) {
 	}
 
 	// Called just before this Command runs the first time
+	@Override
 	protected void initialize() {
+		lift.disable();
+		SPEED = Robot.getConst("Lift Speed", 0.05);
 	}
 
 	// Called repeatedly when this Command is scheduled to run
+	@Override
 	protected void execute() {
 		lift.runMotor(SPEED * dir);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
+	@Override
 	protected boolean isFinished() {
 		return false;
 	}
 
 	// Called once after isFinished returns true
+	@Override
 	protected void end() {
 		lift.stopLift();
 	}
 
 	// Called when another command which requires one or more of the same
 	// subsystems is scheduled to run
+	@Override
 	protected void interrupted() {
 		end();
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
new file mode 100644
index 0000000..a1689ad
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
@@ -0,0 +1,62 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Use this command to test the Lift with PID
+ */
+public class MoveLiftWithPID extends Command {
+
+	private Lift lift;
+	private double setpoint;
+	private int dir;
+
+	public MoveLiftWithPID(Lift lift, boolean up) {
+		requires(Robot.lift);
+		this.lift = lift;
+		if (up)
+			dir = 1;
+		else
+			dir = -1;
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		setpoint = 0;
+		lift.setSetpoint(setpoint);
+		lift.enable();
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	@Override
+	protected void execute() {
+		setpoint += dir * Robot.getConst("Lift Move Increment", 1); // inches
+		lift.setSetpoint(setpoint);
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	@Override
+	protected boolean isFinished() {
+		// always false bc whileHeld
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	@Override
+	protected void end() {
+		lift.disable();
+		// keep stopLift() after disable() so they don't conflict
+		lift.stopLift();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	@Override
+	protected void interrupted() {
+		end();
+	}
+}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index f636774..1fab7d7 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -2,6 +2,7 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -11,37 +12,64 @@
 public class UpdateLiftPosition extends Command {
 
 	private Lift lift;
-	private double targ;
+
+	private double currDist;
+	private double desiredDist = 0;
+	private LiftHeight desiredPos;
+
+	private boolean manipulatorPluggedIn;
 
 	public UpdateLiftPosition(Lift lift) {
-		// Use requires() here to declare subsystem dependencies
-		// eg. requires(chassis);
+		requires(Robot.lift);
 		this.lift = lift;
-		targ = 0;
 	}
 
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
+		try {
+			Robot.oi.manipulator.getRawAxis(1);
+		} catch (NullPointerException e) {
+			System.err.println("[ERROR] Manipulator not plugged in.");
+			manipulatorPluggedIn = false;
+		}
+
+		// I put this enable in here as well as in the Lift constructor bc not sure if
+		// this interrupted() method (and therefore end() and disable() methods) will be
+		// called when an auto command takes overs temporarily. May be repetitive, so...
 		/**
-		 * May need to setInputRange(0, maxHeight);
+		 * @TODO figure out if this needs to be here and/or in Lift constructor or not
 		 */
+		lift.enable();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		int angle = Robot.oi.manipulator.getPOV();
-		/**
-		 * reference AutoLift for how to calculate targ (below) based on the given
-		 * Position enum
-		 */
-		if (angle == 0) {
-			// targ = GROUND
-		} else {
-			// targ = SWITCH
+		if (manipulatorPluggedIn) {
+			int angle = Robot.oi.manipulator.getPOV();
+
+			// 180 degrees is down and -1 is "nothing," so basically default to GROUND,
+			// anything else is SWITCH
+			if (angle != 180 && angle != -1) {
+				desiredPos = LiftHeight.SWITCH;
+				// NOTE: if full lift functionality does become a thing, need to add a couple
+				// more if-elses here to account for those enum values
+			} else {
+				desiredPos = LiftHeight.GROUND;
+			}
+
+			currDist = lift.getHeight();
+			/**
+			 * @TODO: AutoLift sets desiredDist to the delta dist (set - current), but
+			 *        shouldn't you set the setpoint to your actual setpoint? not to the
+			 *        current error? Uncomment line 70 if using delta. Or, use
+			 *        setRelativeSetpoint(something here).
+			 */
+			desiredDist = lift.getDesiredDistFromPos(desiredPos);
+			// desiredDist -= currDist;
+			lift.setSetpoint(desiredDist);
 		}
-		lift.setSetpoint(targ);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
@@ -53,6 +81,7 @@ protected boolean isFinished() {
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
+		lift.stopLift();
 		lift.disable();
 	}
 
@@ -60,5 +89,6 @@ protected void end() {
 	// subsystems is scheduled to run
 	@Override
 	protected void interrupted() {
+		end();
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 7e62e4b..b3fb54c 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -15,15 +15,35 @@ public class Lift extends PIDSubsystem implements LiftInterface {
 
 	private final SpeedControllerGroup liftMotors = RobotMap.liftMotors;
 	private final Encoder liftEnc = RobotMap.liftEnc;
-	private Position currPosition = Position.GROUND;
+	private LiftHeight currPosition = LiftHeight.GROUND;
 
 	private final int NUM_STAGES;
 	private final double WIGGLE_ROOM;
+	private final double SWITCH_DIST;
+	private final double SCALE_DIST;
+	private final double BAR_DIST;
+
+	public Lift() {
+		super("Lift", Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0), Robot.getConst("LiftkD", 0),
+				Robot.getConst("LiftkF", 0.1));
+
+		/**
+		 * @TODO figure out Lift Max Height
+		 */
+		setInputRange(0, Robot.getConst("Lift Max Height", 0));
+		setOutputRange(-1, 1);
 
-	public Lift(String name, double kP, double kI, double kD, double kF) {
-		super(name, kP, kI, kD, kF);
 		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
-		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0);
+		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
+
+		// calculate constant measurements
+		// distance to switch 18.75 inches in starting position
+		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;
+		// distance to scale 5 feet starting 63 / 3 = 21
+		SCALE_DIST = (60.0 + WIGGLE_ROOM) / NUM_STAGES;
+		// 7 feet starting; bar distance should be changed because I'm not aware how
+		// climber mech will be positioned
+		BAR_DIST = (84.0 + WIGGLE_ROOM) / NUM_STAGES;
 		enable();
 	}
 
@@ -35,6 +55,29 @@ public void initDefaultCommand() {
 		setDefaultCommand(new UpdateLiftPosition(this));
 	}
 
+	public double getDesiredDistFromPos(LiftHeight pos) {
+		double desiredDist;
+		switch (pos) {
+		case GROUND:
+			desiredDist = 0;
+			break;
+		case SWITCH:
+			desiredDist = SWITCH_DIST;
+			break;
+		case SCALE:
+			desiredDist = SCALE_DIST;
+			break;
+		case BAR:
+			desiredDist = BAR_DIST;
+			break;
+		default:
+			desiredDist = 0;
+			break;
+		}
+
+		return desiredDist;
+	}
+
 	/**
 	 * Sets the current position in the lift subsystem
 	 * 
@@ -42,7 +85,7 @@ public void initDefaultCommand() {
 	 *            - the new position meant to be set
 	 */
 	@Override
-	public void setCurrPosition(Position newPosition) {
+	public void setCurrPosition(LiftHeight newPosition) {
 		currPosition = newPosition;
 	}
 
@@ -87,7 +130,7 @@ public void runMotor(double output) {
 	 * @return pos - current position
 	 */
 	@Override
-	public Position getCurrPos() {
+	public LiftHeight getCurrPos() {
 		return currPosition;
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
index 6e6e006..c67d210 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
@@ -7,8 +7,30 @@ public interface LiftInterface {
 	 */
 	public void initDefaultCommand();
 
-	public enum Position {
-		GROUND, SWITCH, SCALE, BAR
+	public enum LiftHeight {
+		GROUND, SWITCH, SCALE, BAR;
+
+		public static LiftHeight toLH(String str) {
+			LiftHeight lh = null;
+			switch (str) {
+			case "GROUND":
+				lh = GROUND;
+				break;
+			case "SWITCH":
+				lh = SWITCH;
+				break;
+			case "SCALE":
+				lh = SCALE;
+				break;
+			case "BAR":
+				lh = BAR;
+				break;
+			default:
+				lh = GROUND;
+				break;
+			}
+			return lh;
+		}
 	}
 
 	/**
@@ -39,7 +61,7 @@ public enum Position {
 	 * 
 	 * @return pos - current position
 	 */
-	public Position getCurrPos();
+	public LiftHeight getCurrPos();
 
 	/**
 	 * Resets the encoder
@@ -52,7 +74,7 @@ public enum Position {
 	 * @param newPosition
 	 *            - the new position meant to be set
 	 */
-	public void setCurrPosition(Position newPosition);
+	public void setCurrPosition(LiftHeight newPosition);
 
 	/**
 	 * Gets the number of stages variable

From 421d2d649add97f512e084cd09132ca36889d31e Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 00:13:13 -0700
Subject: [PATCH 06/36] added lift button functionalities

these buttons should be used mostly for testing purposes right now but can be changed later for competition functionality if necessary
---
 docs/controllers.txt | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/docs/controllers.txt b/docs/controllers.txt
index 364e3c3..50ed1de 100644
--- a/docs/controllers.txt
+++ b/docs/controllers.txt
@@ -2,9 +2,9 @@
 	buttons
 		1	slow forward-backward movement in arcade, or all movement in tank
 		2	toggle drive type (arcade/tank) [ShiftDriveType]
-		3
-		4
-		5
+		3	move lift up w/ incremental PID [MoveLiftWithPID...true]
+		4	move lift down w/ incremental PID [MoveLiftWithPID...false]
+		5	use to test PID, targ height changed in SD [LiftToPosition]
 		6
 		7	move with PID by inches from SmartDashboard "Move Targ" [PIDMove]
 		8	turn with PID by degrees from SmartDashboard "Turn Targ" [PIDTurn]
@@ -26,8 +26,8 @@
 		7
 		8	update the PIDController constants with SmartDashboard values [UpdatePIDConstants]
 		9	set the left and right encoder distance per pulse with SmartDashboard values [SetDistancePerPulse]
-		10	
-		11	
+		10	move lift up while held at very slow speed [MoveLift...true]
+		11	move lift down while held at very slow speed [MoveLift...false]
 	axes
 		0	X axis	arcade turn (if default)
 		1	Y axis	tank right, if NOT default: arcade forward

From 2c19666211136e25756c710904a45b048e9b535f Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 18:46:23 -0700
Subject: [PATCH 07/36] changed all PR comments

fixed enable/disable/stop (also added enable to the end() of MoveLift so
it's ready and live when not using that non-PID command anymore
changed getString in OI to be SmartDashboard. instead of Robot. but left
my getString methods in there bc not doing any harm
left end() in interrupted bc not really doing any harm
got rid of spd --> voltage method and replaced it with a constant from
Robot.getConst
---
 .../src/org/usfirst/frc/team199/Robot2018/OI.java   |  4 ++--
 .../team199/Robot2018/commands/LiftToPosition.java  | 11 ++---------
 .../frc/team199/Robot2018/commands/MoveLift.java    |  1 +
 .../team199/Robot2018/commands/MoveLiftWithPID.java |  4 ----
 .../Robot2018/commands/UpdateLiftPosition.java      |  2 --
 .../frc/team199/Robot2018/subsystems/Lift.java      | 13 +------------
 6 files changed, 6 insertions(+), 29 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index 2c65103..85552d9 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -104,8 +104,8 @@ public OI(Robot robot) {
 		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
 
 		testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
-		testLiftPID.whenPressed(
-				new LiftToPosition(Robot.lift, Robot.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
+		testLiftPID.whenPressed(new LiftToPosition(Robot.lift,
+				SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
 
 		rightJoy = new Joystick(1);
 		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
index 1e2da63..5e4bafb 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
@@ -23,13 +23,11 @@ public LiftToPosition(Lift lift, String goal) {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		// There's no way to reset the Lift's PID constants after constructing it, so if
-		// you change them you must Restart Robot Code in DS.
+		lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
+				Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
 		double setpoint = lift.getDesiredDistFromPos(pos);
 		lift.setSetpoint(setpoint);
 		System.out.println("Target Height: " + setpoint);
-		lift.enable();
-		System.out.println("Enabled");
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -47,11 +45,6 @@ protected boolean isFinished() {
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
-		lift.disable();
-		System.out.println("Disabled");
-		// keep stopLift() after disable() so they don't conflict
-		lift.stopLift();
-		System.out.println("Stopped");
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
index 99fabc5..97f4194 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
@@ -46,6 +46,7 @@ protected boolean isFinished() {
 	@Override
 	protected void end() {
 		lift.stopLift();
+		lift.enable();
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
index a1689ad..5f31529 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
@@ -28,7 +28,6 @@ public MoveLiftWithPID(Lift lift, boolean up) {
 	protected void initialize() {
 		setpoint = 0;
 		lift.setSetpoint(setpoint);
-		lift.enable();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -48,9 +47,6 @@ protected boolean isFinished() {
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
-		lift.disable();
-		// keep stopLift() after disable() so they don't conflict
-		lift.stopLift();
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index 1fab7d7..1ea5240 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -81,8 +81,6 @@ protected boolean isFinished() {
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
-		lift.stopLift();
-		lift.disable();
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index b3fb54c..809ebbb 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -176,18 +176,7 @@ protected double returnPIDInput() {
 	protected void usePIDOutput(double output) {
 		double out = output;
 		double spd = liftEnc.getRate();
-		out += convertSpdToVoltage(spd);
+		out += Robot.getConst("Lift: Necessary Voltage", 0);
 		runMotor(out);
 	}
-
-	/**
-	 * Takes a value for the current lift speed and translates it to the amount of
-	 * voltage to motors need to supply.
-	 * 
-	 * @param speed
-	 *            - the current lift speed (in/s)
-	 */
-	public double convertSpdToVoltage(double speed) {
-		return 0;
-	}
 }

From df46666f8d6e2a93e40d5258a7be8013826d49e0 Mon Sep 17 00:00:00 2001
From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com>
Date: Tue, 13 Mar 2018 19:01:33 -0700
Subject: [PATCH 08/36] add eject commands

---
 .../Robot2018/commands/EjectToExchange.java   | 32 ++++-------------
 .../Robot2018/commands/EjectToScale.java      | 35 +++++--------------
 .../Robot2018/commands/EjectToSwitch.java     | 35 +++++--------------
 3 files changed, 24 insertions(+), 78 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
index 9782f56..b6a8913 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
@@ -1,36 +1,16 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
  *
  */
-public class EjectToExchange extends Command {
+public class EjectToExchange extends CommandGroup {
 
     public EjectToExchange() {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
-    }
-
-    // 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() {
+        addSequential(new LiftToPosition(Robot.lift, "GROUND"));
+        addSequential(new OuttakeCube());
     }
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
index b8e8dfa..9bc6d5d 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
@@ -1,36 +1,19 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
  *
  */
-public class EjectToScale extends Command {
+public class EjectToScale extends CommandGroup {
 
     public EjectToScale() {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
-    }
-
-    // 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() {
+    		addSequential(new LiftToPosition(Robot.lift, "SCALE"));
+		addSequential(new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+		addSequential(new OuttakeCube());
+		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+		addSequential(new LiftToPosition(Robot.lift, "SCALE"));
     }
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
index a0df40b..46612f2 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
@@ -1,36 +1,19 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
  *
  */
-public class EjectToSwitch extends Command {
+public class EjectToSwitch extends CommandGroup {
 
     public EjectToSwitch() {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
-    }
-
-    // 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() {
+    		addSequential(new LiftToPosition(Robot.lift, "SWITCH"));
+    		addSequential(new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+    		addSequential(new OuttakeCube());
+    		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+    		addSequential(new LiftToPosition(Robot.lift, "GROUND"));
     }
 }

From 36c90ed0ed8da11ad6bd21e06c06ac0038724d5e Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 19:17:29 -0700
Subject: [PATCH 09/36] trello card checklist changes

https://trello.com/c/7OOvkOJG/167-implement-pid-on-lift-encoder
made absolute setpoints
used getHeight for everything
deleted AutoLift bc obsolete
deleted another missed unecessary enable
---
 .../team199/Robot2018/commands/AutoLift.java  | 116 ------------------
 .../commands/UpdateLiftPosition.java          |  18 ---
 .../team199/Robot2018/subsystems/Lift.java    |   4 +-
 3 files changed, 2 insertions(+), 136 deletions(-)
 delete mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
deleted file mode 100644
index 604ef31..0000000
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
+++ /dev/null
@@ -1,116 +0,0 @@
-package org.usfirst.frc.team199.Robot2018.commands;
-
-import org.usfirst.frc.team199.Robot2018.Robot;
-import org.usfirst.frc.team199.Robot2018.RobotMap;
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
-
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class AutoLift extends Command implements PIDOutput {
-	/**
-	 * All distances are measured from bottom of cube and + 3 inches for wiggle room
-	 * for dropping cubes Also, actual distances are divided by 3 because according
-	 * to cad, the lift will have a 1:3 ratio from winch to actual height.
-	 */
-
-	private final int NUM_STAGES;
-	private final double WIGGLE_ROOM;
-	private final double SWITCH_DIST;
-	private final double SCALE_DIST;
-	private final double BAR_DIST;
-
-	private double desiredDist = 0;
-	private double currDist;
-	private LiftInterface lift;
-	private LiftHeight desiredPos;
-
-	private PIDController liftController;
-
-	public AutoLift(LiftHeight stage, LiftInterface lift) {
-		this.lift = lift;
-		requires(Robot.lift);
-
-		currDist = lift.getHeight();
-
-		// calculate constant measurements
-
-		NUM_STAGES = lift.getNumStages();
-		WIGGLE_ROOM = lift.getWiggleRoom();
-		// distance to switch 18.75 inches in starting position
-		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;
-		// distance to scale 5 feet starting 63 / 3 = 21
-		SCALE_DIST = (60.0 + WIGGLE_ROOM) / NUM_STAGES;
-		// 7 feet starting; bar distance should be changed because I'm not aware how
-		// climber mech will be positioned
-		BAR_DIST = (84.0 + WIGGLE_ROOM) / NUM_STAGES;
-
-		switch (stage) {
-		case GROUND:
-			desiredDist = -currDist;
-			break;
-		case SWITCH:
-			desiredDist = SWITCH_DIST - currDist;
-			break;
-		case SCALE:
-			desiredDist = SCALE_DIST - currDist;
-			break;
-		case BAR:
-			desiredDist = BAR_DIST - currDist;
-			break;
-		}
-
-		liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
-				Robot.getConst("LiftkD", 0), RobotMap.liftEnc, this);
-
-		desiredPos = stage;
-	}
-
-	// Called just before this Command runs the first time
-	protected void initialize() {
-		// input is in inches
-		// liftController.setInputRange(-Robot.getConst("Max High Speed", 204),
-		// Robot.getConst("Max High Speed", 204));
-		// output in "motor units" (arcade and tank only accept values [-1, 1]
-		liftController.setOutputRange(-1.0, 1.0);
-		liftController.setContinuous(false);
-		// liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
-		liftController.setSetpoint(desiredDist);
-		liftController.enable();
-	}
-
-	// 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 (liftController.onTarget()) {
-			lift.setCurrPosition(desiredPos);
-			return true;
-		}
-		return false;
-	}
-
-	// Called once after isFinished returns true
-	protected void end() {
-		liftController.disable();
-		liftController.free();
-	}
-
-	// Called when another command which requires one or more of the same
-	// subsystems is scheduled to run
-	protected void interrupted() {
-		end();
-	}
-
-	@Override
-	public void pidWrite(double output) {
-		lift.runMotor(output);
-	}
-}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index 1ea5240..6b24eb1 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -12,8 +12,6 @@
 public class UpdateLiftPosition extends Command {
 
 	private Lift lift;
-
-	private double currDist;
 	private double desiredDist = 0;
 	private LiftHeight desiredPos;
 
@@ -33,14 +31,6 @@ protected void initialize() {
 			System.err.println("[ERROR] Manipulator not plugged in.");
 			manipulatorPluggedIn = false;
 		}
-
-		// I put this enable in here as well as in the Lift constructor bc not sure if
-		// this interrupted() method (and therefore end() and disable() methods) will be
-		// called when an auto command takes overs temporarily. May be repetitive, so...
-		/**
-		 * @TODO figure out if this needs to be here and/or in Lift constructor or not
-		 */
-		lift.enable();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -59,15 +49,7 @@ protected void execute() {
 				desiredPos = LiftHeight.GROUND;
 			}
 
-			currDist = lift.getHeight();
-			/**
-			 * @TODO: AutoLift sets desiredDist to the delta dist (set - current), but
-			 *        shouldn't you set the setpoint to your actual setpoint? not to the
-			 *        current error? Uncomment line 70 if using delta. Or, use
-			 *        setRelativeSetpoint(something here).
-			 */
 			desiredDist = lift.getDesiredDistFromPos(desiredPos);
-			// desiredDist -= currDist;
 			lift.setSetpoint(desiredDist);
 		}
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 809ebbb..c850880 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -90,7 +90,7 @@ public void setCurrPosition(LiftHeight newPosition) {
 	}
 
 	/**
-	 * Uses (insert sensor here) to detect the current lift position
+	 * Uses AMT103 Encoder to detect the current lift position
 	 */
 	@Override
 	public double getHeight() {
@@ -165,7 +165,7 @@ public double getWiggleRoom() {
 	@Override
 	protected double returnPIDInput() {
 		// return getHeight(); //Use this instead? What's the difference?
-		return liftEnc.getDistance();
+		return getHeight();
 	}
 
 	/**

From 0c1f8ccfbfc525f0e1db9840bfd2abdd441aa1bc Mon Sep 17 00:00:00 2001
From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com>
Date: Tue, 13 Mar 2018 19:25:31 -0700
Subject: [PATCH 10/36] lower to ground after scale eject

---
 .../usfirst/frc/team199/Robot2018/commands/EjectToScale.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
index 9bc6d5d..801e392 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
@@ -14,6 +14,6 @@ public EjectToScale() {
 		addSequential(new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
 		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
-		addSequential(new LiftToPosition(Robot.lift, "SCALE"));
+		addSequential(new LiftToPosition(Robot.lift, "GROUND"));
     }
 }

From 833cdda758fe954758eb106e6fee48e6c20b0dac Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 19:37:59 -0700
Subject: [PATCH 11/36] added stuff to testPeriodic in order to find lift's
 "holding" voltage

---
 .../usfirst/frc/team199/Robot2018/Robot.java  | 41 ++-----------------
 .../team199/Robot2018/subsystems/Lift.java    |  2 +-
 2 files changed, 4 insertions(+), 39 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 9a1bda3..bfdf61b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -288,43 +288,8 @@ public void testInit() {
 	 */
 	@Override
 	public void testPeriodic() {
-		// if(firstTime) {
-		// Robot.dt.enableVelocityPIDs();
-		// firstTime = false;
-		//// }
-		// dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
-		// getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
-		// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
-		// 1 / Robot.getConst("Max Low Speed", 84)));
-		// dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
-		// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
-		// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
-		// 1 / Robot.getConst("Max Low Speed", 84)));
-		// dt.resetAllVelocityPIDConsts();
-
-		// dt.setVPIDs(getConst("VPID Test Set", 0.5));
-
-		Scheduler.getInstance().run();
-
-		// System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
-		// System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
-		// System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
-		// System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
-		// System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
-		// System.out.println("Right Enc Rate: " + dt.getRightEncRate());
-		//
-		// System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
-		// System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
-
-		// System.out.println("Left Enc Dist " + dt.getLeftDist());
-		// System.out.println("Right Enc Dist " + dt.getRightDist());
-		// System.out.println("Avg Enc Dist" + dt.getEncAvgDist());
-
-		// dt.dtLeft.set(0.1);
-		// dt.dtRight.set(-oi.rightJoy.getY());
-		// dt.dtLeft.set(-oi.leftJoy.getY());
-		// dt.dtRight.set(-oi.rightJoy.getY());
-
-		// dt.putVelocityControllersToDashboard();
+		// Scheduler.getInstance().run();
+		lift.disable();
+		lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index c850880..5ec85d9 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -117,7 +117,7 @@ public double getLiftSpeed() {
 	 * Runs lift motors at specified speed
 	 * 
 	 * @param speed
-	 *            - desired speed to run at
+	 *            - desired speed to run at [-1, 1]
 	 */
 	@Override
 	public void runMotor(double output) {

From 434360b93eba123553ecdd9da5841ff9c9590806 Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 19:45:58 -0700
Subject: [PATCH 12/36] quick lift fixes

set default lift max height
reset lift enc in autoInit
---
 Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java   | 1 +
 .../org/usfirst/frc/team199/Robot2018/subsystems/Lift.java   | 5 +----
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index bfdf61b..959eee3 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -206,6 +206,7 @@ public void disabledPeriodic() {
 	@Override
 	public void autonomousInit() {
 		dt.resetAHRS();
+		lift.resetEnc();
 		AutoUtils.state = new State(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
 		Scheduler.getInstance().add(new CloseIntake());
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 5ec85d9..b625fe4 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -27,10 +27,7 @@ public Lift() {
 		super("Lift", Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0), Robot.getConst("LiftkD", 0),
 				Robot.getConst("LiftkF", 0.1));
 
-		/**
-		 * @TODO figure out Lift Max Height
-		 */
-		setInputRange(0, Robot.getConst("Lift Max Height", 0));
+		setInputRange(0, Robot.getConst("Lift Max Height", 24));
 		setOutputRange(-1, 1);
 
 		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);

From d83e7c738c3dae0899b4bf9128349685e6bac41c Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Tue, 13 Mar 2018 20:08:09 -0700
Subject: [PATCH 13/36] PID by math setup

implemented getLiftTimeConstant and methods in it for Lift
still need to implement default constant values (using more math, see
PIDMove constructor)
commented out stuff in testPeriodic bc don't need them
---
 .../usfirst/frc/team199/Robot2018/Robot.java  |  4 +-
 .../frc/team199/Robot2018/RobotMap.java       | 21 +++++++--
 .../team199/Robot2018/subsystems/Lift.java    | 46 +++++++++++++++++++
 3 files changed, 64 insertions(+), 7 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 959eee3..fdd6c48 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -290,7 +290,7 @@ public void testInit() {
 	@Override
 	public void testPeriodic() {
 		// Scheduler.getInstance().run();
-		lift.disable();
-		lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
+		// lift.disable();
+		// lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
index af730f8..ecc6fdb 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
@@ -43,7 +43,7 @@ public class RobotMap {
 	public static DigitalSource liftEncPort1;
 	public static DigitalSource liftEncPort2;
 
-	//public static WPI_TalonSRX climberMotor;
+	// public static WPI_TalonSRX climberMotor;
 
 	public static VictorSP leftIntakeMotor;
 	public static VictorSP rightIntakeMotor;
@@ -123,7 +123,7 @@ private void configSPX(WPI_VictorSPX mc) {
 
 	public RobotMap() {
 		pdp = new PowerDistributionPanel();
-		
+
 		liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
 		configSPX(liftMotorA);
 		liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
@@ -252,7 +252,10 @@ public double getRadius() {
 		return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
 	}
 
-	public double getOmegaMax() {
+	/**
+	 * @return the RPM max of a CIM
+	 */
+	public static double getOmegaMax() {
 		return Robot.getConst("Omega Max", 5330);
 	}
 
@@ -264,7 +267,10 @@ public double getCycleTime() {
 		return Robot.getConst("Code cycle time", 0.05);
 	}
 
-	public double getStallTorque() {
+	/**
+	 * @return the stall torque of a CIM
+	 */
+	public static double getStallTorque() {
 		return Robot.getConst("Stall Torque", 2.41);
 	}
 
@@ -273,7 +279,12 @@ private double convertLbsTokG(double lbs) {
 		return lbs * 0.45359237;
 	}
 
-	private double convertNtokG(double newtons) {
+	/**
+	 * @param the
+	 *            weight (in Newtons)
+	 * @return the equivalent mass (in kg)
+	 */
+	public static double convertNtokG(double newtons) {
 		// weight / accel due to grav = kg
 		return newtons / 9.81;
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index b625fe4..cc7d21b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -155,6 +155,14 @@ public double getWiggleRoom() {
 		return WIGGLE_ROOM;
 	}
 
+	/**
+	 * @return the max speed of the lift
+	 */
+	public double getLiftMaxSpeed() {
+		/** @todo find lift max speed and set default below */
+		return Robot.getConst("Lift Max Speed", 0);
+	}
+
 	/**
 	 * Uses AMT103 Encoder to detect the current lift height with respect to the
 	 * lift's min height (inches)
@@ -176,4 +184,42 @@ protected void usePIDOutput(double output) {
 		out += Robot.getConst("Lift: Necessary Voltage", 0);
 		runMotor(out);
 	}
+
+	/**
+	 * Gets the time constant of the drivetrain, which is used to calculate PID
+	 * constants.
+	 * 
+	 * @return time constant
+	 */
+	public double getLiftTimeConstant() {
+		double gearReduction = getLiftGearRatio();
+		double radius = getLiftRadius();
+		double timeConstant = RobotMap.getOmegaMax() / gearReduction / 60 * 2 * Math.PI
+				* RobotMap.convertNtokG(getLiftedWeight()) * radius * radius
+				/ (RobotMap.getStallTorque() * gearReduction);
+		return timeConstant;
+	}
+
+	/**
+	 * @return the gear ratio of the lift gearbox
+	 */
+	public double getLiftGearRatio() {
+		return Robot.getConst("Lift Gear Reduction", 10);
+	}
+
+	/**
+	 * @return the lift sprocket's radius
+	 */
+	public double getLiftRadius() {
+		// 15 tooth, #35 sprocket
+		return Robot.getConst("Lift Sprocket Pitch Diam", 1.79) / 2;
+	}
+
+	/**
+	 * @return the weight (in Newtons) of everything that is being lifted: intake,
+	 *         lift components, NOT cube
+	 */
+	public double getLiftedWeight() {
+		return Robot.getConst("Weight of Lifted Stuff", 342);
+	}
 }

From cd525b2eb8326dabe42d0e18659a963701baf1b6 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Tue, 13 Mar 2018 21:20:32 -0700
Subject: [PATCH 14/36] Compute hopefully reasonable defaults for lift PID
 constants.

---
 .../frc/team199/Robot2018/RobotMap.java       |  9 ++++++-
 .../team199/Robot2018/subsystems/Lift.java    | 19 ++++++++++++---
 shuffleboard.json                             | 24 ++++++++++++-------
 3 files changed, 40 insertions(+), 12 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
index ecc6fdb..ea2164c 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
@@ -130,11 +131,17 @@ public RobotMap() {
 		configSRX(liftMotorB);
 		liftMotorC = new WPI_TalonSRX(getPort("2LiftTalonSRX", 7));
 		configSRX(liftMotorC);
-		liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
+		// liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
+		liftMotors = new SpeedControllerGroup(liftMotorC);
+		liftMotors.setName("Lift", "CIM Motor");
+		LiveWindow.add(liftMotors);
+
 		liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));
 		liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5));
 		liftEnc = new Encoder(liftEncPort1, liftEncPort2);
 		liftEnc.setPIDSourceType(PIDSourceType.kDisplacement);
+		liftEnc.setName("Lift", "Encoder");
+		LiveWindow.add(liftEnc);
 
 		leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 8));
 		rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 9));
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index cc7d21b..307ad7b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -5,6 +5,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.UpdateLiftPosition;
 
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
@@ -24,8 +25,20 @@ public class Lift extends PIDSubsystem implements LiftInterface {
 	private final double BAR_DIST;
 
 	public Lift() {
-		super("Lift", Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0), Robot.getConst("LiftkD", 0),
-				Robot.getConst("LiftkF", 0.1));
+		super("Lift", 0, 0, 0, 0);
+
+		double maxSpeed = getLiftMaxSpeed();
+
+		double r = Robot.getConst("LiftPidR", 3.0);
+		double kP = r / getLiftTimeConstant() / maxSpeed;
+		double kI = 0;
+		double kD = r / maxSpeed;
+		double kF = 1 / (maxSpeed * Robot.getConst("Default PID Update Time", 0.05));
+
+		PIDController liftController = getPIDController();
+
+		liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI", kI), Robot.getConst("LiftkD", kD),
+				Robot.getConst("LiftkF", kF));
 
 		setInputRange(0, Robot.getConst("Lift Max Height", 24));
 		setOutputRange(-1, 1);
@@ -220,6 +233,6 @@ public double getLiftRadius() {
 	 *         lift components, NOT cube
 	 */
 	public double getLiftedWeight() {
-		return Robot.getConst("Weight of Lifted Stuff", 342);
+		return Robot.getConst("Weight of Lifted Stuff", 53.11);
 	}
 }
diff --git a/shuffleboard.json b/shuffleboard.json
index 558854a..82f7a02 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -88,8 +88,7 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 30.0,
-              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
+              "Visible time": 30.0
             }
           },
           "4,0": {
@@ -101,8 +100,7 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 30.0,
-              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
+              "Visible time": 30.0
             }
           },
           "4,3": {
@@ -386,8 +384,7 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 10.0,
-              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
+              "Visible time": 10.0
             }
           },
           "5,2": {
@@ -399,8 +396,7 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 10.0,
-              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
+              "Visible time": 10.0
             }
           },
           "5,1": {
@@ -941,6 +937,18 @@
           }
         }
       }
+    },
+    {
+      "title": "Tab 8",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "tiles": {}
+      }
     }
   ]
 }
\ No newline at end of file

From a9fdf4dab4a37712101261c29a7227cfb7be4d68 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Wed, 14 Mar 2018 20:40:31 -0700
Subject: [PATCH 15/36] lift and intake work as expected :)

---
 .../usfirst/frc/team199/Robot2018/Robot.java  |  2 +
 .../frc/team199/Robot2018/RobotMap.java       |  1 +
 .../Robot2018/commands/MoveLiftWithPID.java   |  6 +-
 .../commands/UpdateLiftPosition.java          | 39 +++++++++---
 .../team199/Robot2018/subsystems/Lift.java    | 61 ++++++++++++++-----
 .../Robot2018/subsystems/LiftInterface.java   |  5 +-
 shuffleboard.json                             | 50 +++++++++++++--
 7 files changed, 133 insertions(+), 31 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index ff4dec1..413eafa 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -191,6 +191,7 @@ public void robotInit() {
 	@Override
 	public void disabledInit() {
 		dt.disableVelocityPIDs();
+		lift.setSetpoint(0);
 	}
 
 	@Override
@@ -283,6 +284,7 @@ public void testInit() {
 		System.out.println("In testInit()");
 		dt.resetAHRS();
 		AutoUtils.state = new State(0, 0, 0);
+		lift.disable();
 	}
 
 	/**
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
index ea2164c..bb50606 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
@@ -134,6 +134,7 @@ public RobotMap() {
 		// liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
 		liftMotors = new SpeedControllerGroup(liftMotorC);
 		liftMotors.setName("Lift", "CIM Motor");
+		liftMotors.setInverted(true);
 		LiveWindow.add(liftMotors);
 
 		liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
index 5f31529..888d4a8 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java
@@ -26,14 +26,14 @@ public MoveLiftWithPID(Lift lift, boolean up) {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		setpoint = 0;
-		lift.setSetpoint(setpoint);
+		// setpoint = lift.getPIDController().getSetpoint();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		setpoint += dir * Robot.getConst("Lift Move Increment", 1); // inches
+		setpoint = lift.getPIDController().getSetpoint() + dir * Robot.getConst("Lift Move Increment", 0.25); // inches/0.05
+																												// secs
 		lift.setSetpoint(setpoint);
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index 6b24eb1..40834d2 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -15,7 +15,8 @@ public class UpdateLiftPosition extends Command {
 	private double desiredDist = 0;
 	private LiftHeight desiredPos;
 
-	private boolean manipulatorPluggedIn;
+	private boolean manipulatorPluggedIn = true;
+	private boolean goToGround = false;
 
 	public UpdateLiftPosition(Lift lift) {
 		requires(Robot.lift);
@@ -39,18 +40,40 @@ protected void execute() {
 		if (manipulatorPluggedIn) {
 			int angle = Robot.oi.manipulator.getPOV();
 
+			System.out.println("POV Reading: " + angle);
+
 			// 180 degrees is down and -1 is "nothing," so basically default to GROUND,
 			// anything else is SWITCH
-			if (angle != 180 && angle != -1) {
+			// if (angle != 180 && angle != -1) {
+			// desiredPos = LiftHeight.SWITCH;
+			// // NOTE: if full lift functionality does become a thing, need to add a couple
+			// // more if-elses here to account for those enum values
+			// } else {
+			// desiredPos = LiftHeight.GROUND;
+			// }
+
+			if (angle == 180) {
+				desiredPos = LiftHeight.HOLD_CUBE;
+				goToGround = true;
+			} else if (angle == 270) {
+				desiredPos = LiftHeight.HOLD_CUBE;
+				goToGround = false;
+			} else if (angle != -1) {
 				desiredPos = LiftHeight.SWITCH;
-				// NOTE: if full lift functionality does become a thing, need to add a couple
-				// more if-elses here to account for those enum values
-			} else {
-				desiredPos = LiftHeight.GROUND;
+				goToGround = false;
 			}
 
-			desiredDist = lift.getDesiredDistFromPos(desiredPos);
-			lift.setSetpoint(desiredDist);
+			if (angle != -1) {
+				desiredDist = lift.getDesiredDistFromPos(desiredPos);
+				lift.setSetpoint(desiredDist);
+			}
+
+			if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
+				desiredPos = LiftHeight.GROUND;
+				desiredDist = lift.getDesiredDistFromPos(desiredPos);
+				lift.setSetpoint(desiredDist);
+				goToGround = false;
+			}
 		}
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 307ad7b..d0c767e 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -29,7 +29,9 @@ public Lift() {
 
 		double maxSpeed = getLiftMaxSpeed();
 
-		double r = Robot.getConst("LiftPidR", 3.0);
+		System.out.println("Lift Time Const: " + getLiftTimeConstant());
+
+		double r = Robot.getConst("LiftPidR", 0.3);
 		double kP = r / getLiftTimeConstant() / maxSpeed;
 		double kI = 0;
 		double kD = r / maxSpeed;
@@ -37,11 +39,18 @@ public Lift() {
 
 		PIDController liftController = getPIDController();
 
-		liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI", kI), Robot.getConst("LiftkD", kD),
-				Robot.getConst("LiftkF", kF));
+		// liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI",
+		// kI), Robot.getConst("LiftkD", kD),
+		// Robot.getConst("LiftkF", kF));
+		liftController.setPID(kP, kI, kD, kF);
 
 		setInputRange(0, Robot.getConst("Lift Max Height", 24));
 		setOutputRange(-1, 1);
+		setAbsoluteTolerance(Robot.getConst("Lift Tolerance", 0.8));
+
+		// dpp = (pitch circumference of sprocket) / (pulses per rev of output shaft)
+		double dpp = 2 * Math.PI * getLiftRadius() / 2048;
+		liftEnc.setDistancePerPulse(dpp);
 
 		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
@@ -71,6 +80,9 @@ public double getDesiredDistFromPos(LiftHeight pos) {
 		case GROUND:
 			desiredDist = 0;
 			break;
+		case HOLD_CUBE:
+			desiredDist = 4;
+			break;
 		case SWITCH:
 			desiredDist = SWITCH_DIST;
 			break;
@@ -88,6 +100,13 @@ public double getDesiredDistFromPos(LiftHeight pos) {
 		return desiredDist;
 	}
 
+	/**
+	 * @return the rate of the lift encoder (in/s)
+	 */
+	public double getSpeed() {
+		return liftEnc.getRate();
+	}
+
 	/**
 	 * Sets the current position in the lift subsystem
 	 * 
@@ -124,14 +143,19 @@ public double getLiftSpeed() {
 	}
 
 	/**
-	 * Runs lift motors at specified speed
+	 * Sends specific voltage to lift motor, clamped at max voltage
 	 * 
-	 * @param speed
-	 *            - desired speed to run at [-1, 1]
+	 * @param output
+	 *            - desired voltage to set motor to [-1, 1]
 	 */
 	@Override
 	public void runMotor(double output) {
-		liftMotors.set(output);
+		double absMax = Robot.getConst("Lift Max Voltage", 0.5);
+		double out = output;
+		if (Math.abs(out) > absMax) {
+			out = Math.signum(out) * absMax;
+		}
+		liftMotors.set(out);
 	}
 
 	/**
@@ -169,11 +193,11 @@ public double getWiggleRoom() {
 	}
 
 	/**
-	 * @return the max speed of the lift
+	 * @return the max speed of the lift (in/s)
 	 */
 	public double getLiftMaxSpeed() {
-		/** @todo find lift max speed and set default below */
-		return Robot.getConst("Lift Max Speed", 0);
+		double maxSpd = RobotMap.getOmegaMax() / getLiftGearRatio() / 60 * 2 * Math.PI * getLiftRadius();
+		return Robot.getConst("Lift Max Speed", maxSpd);
 	}
 
 	/**
@@ -194,7 +218,7 @@ protected double returnPIDInput() {
 	protected void usePIDOutput(double output) {
 		double out = output;
 		double spd = liftEnc.getRate();
-		out += Robot.getConst("Lift: Necessary Voltage", 0);
+		out += Robot.getConst("Lift: Necessary Voltage", 0.125);
 		runMotor(out);
 	}
 
@@ -206,13 +230,22 @@ protected void usePIDOutput(double output) {
 	 */
 	public double getLiftTimeConstant() {
 		double gearReduction = getLiftGearRatio();
-		double radius = getLiftRadius();
+		double radius = convertInToM(getLiftRadius());
 		double timeConstant = RobotMap.getOmegaMax() / gearReduction / 60 * 2 * Math.PI
 				* RobotMap.convertNtokG(getLiftedWeight()) * radius * radius
 				/ (RobotMap.getStallTorque() * gearReduction);
 		return timeConstant;
 	}
 
+	/**
+	 * @param inches
+	 *            - an amount in inches
+	 * @return the equivalent amount in meters
+	 */
+	public double convertInToM(double inches) {
+		return inches * 2.54 / 100;
+	}
+
 	/**
 	 * @return the gear ratio of the lift gearbox
 	 */
@@ -221,7 +254,7 @@ public double getLiftGearRatio() {
 	}
 
 	/**
-	 * @return the lift sprocket's radius
+	 * @return the lift sprocket's radius (inches)
 	 */
 	public double getLiftRadius() {
 		// 15 tooth, #35 sprocket
@@ -233,6 +266,6 @@ public double getLiftRadius() {
 	 *         lift components, NOT cube
 	 */
 	public double getLiftedWeight() {
-		return Robot.getConst("Weight of Lifted Stuff", 53.11);
+		return Robot.getConst("Weight of Lifted Stuff", 62.91);
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
index c67d210..917b820 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java
@@ -8,7 +8,7 @@ public interface LiftInterface {
 	public void initDefaultCommand();
 
 	public enum LiftHeight {
-		GROUND, SWITCH, SCALE, BAR;
+		GROUND, HOLD_CUBE, SWITCH, SCALE, BAR;
 
 		public static LiftHeight toLH(String str) {
 			LiftHeight lh = null;
@@ -16,6 +16,9 @@ public static LiftHeight toLH(String str) {
 			case "GROUND":
 				lh = GROUND;
 				break;
+			case "HOLD_CUBE":
+				lh = HOLD_CUBE;
+				break;
 			case "SWITCH":
 				lh = SWITCH;
 				break;
diff --git a/shuffleboard.json b/shuffleboard.json
index 82f7a02..f721183 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -88,7 +88,8 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 30.0
+              "Visible time": 30.0,
+              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
             }
           },
           "4,0": {
@@ -100,7 +101,8 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 30.0
+              "Visible time": 30.0,
+              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
             }
           },
           "4,3": {
@@ -384,7 +386,8 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 10.0
+              "Visible time": 10.0,
+              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
             }
           },
           "5,2": {
@@ -396,7 +399,8 @@
               "_type": "Graph",
               "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
               "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 10.0
+              "Visible time": 10.0,
+              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
             }
           },
           "5,1": {
@@ -947,7 +951,43 @@
         "showGrid": true,
         "hgap": 16.0,
         "vgap": 16.0,
-        "tiles": {}
+        "tiles": {
+          "0,1": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "Encoder",
+              "_source0": "network_table:///LiveWindow/Lift/Encoder",
+              "_title": "LiveWindow/Lift/Encoder"
+            }
+          },
+          "0,0": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "Speed Controller",
+              "_source0": "network_table:///LiveWindow/Lift/CIM Motor",
+              "_title": "LiveWindow/Lift/CIM Motor",
+              "controllable": false,
+              "orientation": "HORIZONTAL"
+            }
+          },
+          "0,2": {
+            "size": [
+              6,
+              2
+            ],
+            "content": {
+              "_type": "Network Table Tree",
+              "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
+              "_title": "LiveWindow/Ungrouped/Scheduler"
+            }
+          }
+        }
       }
     }
   ]

From 618015dedefa0bb24c78f7eb6065cfae63bcab59 Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Fri, 16 Mar 2018 09:36:27 -0700
Subject: [PATCH 16/36] implemented HOLD_CUBE as first targ when told to go to
 the GROUND

for UpdateLiftPosition and LiftToPosition
---
 .../usfirst/frc/team199/Robot2018/Robot.java  |  3 +-
 .../Robot2018/commands/LiftToPosition.java    |  7 +-
 .../commands/UpdateLiftPosition.java          | 29 +++----
 .../team199/Robot2018/subsystems/Lift.java    | 77 +++++++++++++++++--
 4 files changed, 85 insertions(+), 31 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 413eafa..f194977 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -181,6 +181,8 @@ public void robotInit() {
 		listen = new Listener();
 		// CameraServer.getInstance().startAutomaticCapture(0);
 		// CameraServer.getInstance().startAutomaticCapture(1);
+
+		lift.resetEnc();
 	}
 
 	/**
@@ -207,7 +209,6 @@ public void disabledPeriodic() {
 	@Override
 	public void autonomousInit() {
 		dt.resetAHRS();
-		lift.resetEnc();
 		AutoUtils.state = new State(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
 		Scheduler.getInstance().add(new CloseIntake());
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
index 5e4bafb..6aab891 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
@@ -13,6 +13,7 @@ public class LiftToPosition extends Command {
 
 	private Lift lift;
 	private LiftHeight pos;
+	private boolean goToGround;
 
 	public LiftToPosition(Lift lift, String goal) {
 		requires(Robot.lift);
@@ -25,14 +26,14 @@ public LiftToPosition(Lift lift, String goal) {
 	protected void initialize() {
 		lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
 				Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
-		double setpoint = lift.getDesiredDistFromPos(pos);
-		lift.setSetpoint(setpoint);
-		System.out.println("Target Height: " + setpoint);
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
+		double setpoint = lift.getDesiredDistFromPos(pos);
+		lift.setSetpoint(setpoint);
+		System.out.println("Target Height: " + setpoint);
 		System.out.println("Current Height: " + lift.getHeight());
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index 40834d2..8716b1a 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -16,7 +16,7 @@ public class UpdateLiftPosition extends Command {
 	private LiftHeight desiredPos;
 
 	private boolean manipulatorPluggedIn = true;
-	private boolean goToGround = false;
+	private boolean goToGround;
 
 	public UpdateLiftPosition(Lift lift) {
 		requires(Robot.lift);
@@ -32,6 +32,7 @@ protected void initialize() {
 			System.err.println("[ERROR] Manipulator not plugged in.");
 			manipulatorPluggedIn = false;
 		}
+		goToGround = false;
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -42,18 +43,8 @@ protected void execute() {
 
 			System.out.println("POV Reading: " + angle);
 
-			// 180 degrees is down and -1 is "nothing," so basically default to GROUND,
-			// anything else is SWITCH
-			// if (angle != 180 && angle != -1) {
-			// desiredPos = LiftHeight.SWITCH;
-			// // NOTE: if full lift functionality does become a thing, need to add a couple
-			// // more if-elses here to account for those enum values
-			// } else {
-			// desiredPos = LiftHeight.GROUND;
-			// }
-
 			if (angle == 180) {
-				desiredPos = LiftHeight.HOLD_CUBE;
+				desiredPos = LiftHeight.GROUND;
 				goToGround = true;
 			} else if (angle == 270) {
 				desiredPos = LiftHeight.HOLD_CUBE;
@@ -63,17 +54,17 @@ protected void execute() {
 				goToGround = false;
 			}
 
-			if (angle != -1) {
+			if (goToGround || angle != -1) {
 				desiredDist = lift.getDesiredDistFromPos(desiredPos);
 				lift.setSetpoint(desiredDist);
 			}
 
-			if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
-				desiredPos = LiftHeight.GROUND;
-				desiredDist = lift.getDesiredDistFromPos(desiredPos);
-				lift.setSetpoint(desiredDist);
-				goToGround = false;
-			}
+			// if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
+			// desiredPos = LiftHeight.GROUND;
+			// desiredDist = lift.getDesiredDistFromPos(desiredPos);
+			// lift.setSetpoint(desiredDist);
+			// goToGround = false;
+			// }
 		}
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index d0c767e..50db6d2 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -20,10 +20,15 @@ public class Lift extends PIDSubsystem implements LiftInterface {
 
 	private final int NUM_STAGES;
 	private final double WIGGLE_ROOM;
+	private final double GROUND_DIST;
+	private final double HOLD_CUBE_DIST;
 	private final double SWITCH_DIST;
 	private final double SCALE_DIST;
 	private final double BAR_DIST;
 
+	private LiftHeight desiredPos;
+	private boolean goToGround;
+
 	public Lift() {
 		super("Lift", 0, 0, 0, 0);
 
@@ -35,7 +40,7 @@ public Lift() {
 		double kP = r / getLiftTimeConstant() / maxSpeed;
 		double kI = 0;
 		double kD = r / maxSpeed;
-		double kF = 1 / (maxSpeed * Robot.getConst("Default PID Update Time", 0.05));
+		double kF = 1 / maxSpeed * Robot.getConst("Default PID Update Time", 0.05);
 
 		PIDController liftController = getPIDController();
 
@@ -56,6 +61,8 @@ public Lift() {
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
 
 		// calculate constant measurements
+		GROUND_DIST = 0;
+		HOLD_CUBE_DIST = 4;
 		// distance to switch 18.75 inches in starting position
 		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;
 		// distance to scale 5 feet starting 63 / 3 = 21
@@ -74,26 +81,73 @@ public void initDefaultCommand() {
 		setDefaultCommand(new UpdateLiftPosition(this));
 	}
 
+	/**
+	 * @return whether or not the cube
+	 */
+	private boolean atHoldCube() {
+		double tol = Robot.getConst("Lift Tolerance", 0.8);
+		double height = getHeight();
+		// true if (height within tolerance of HOLD_CUBE height) and (lift is not
+		// moving)
+		return height <= (HOLD_CUBE_DIST + tol) && height >= (HOLD_CUBE_DIST - tol) && getSpeed() <= 0.1;
+	}
+
+	/**
+	 * @return true if reached target height and does not still have to go to the
+	 *         ground as a secondary, consecutive target
+	 */
+	@Override
+	public boolean onTarget() {
+		double targ = getSetpoint();
+		double tol = Robot.getConst("Lift Tolerance", 0.8);
+		double height = getHeight();
+		// true if (height within tolerance of targ) and (lift is not moving)
+		boolean wInTol = height <= (targ + tol) && height >= (targ - tol) && getSpeed() <= 0.1;
+		return wInTol && !goToGround;
+	}
+
+	/**
+	 * Set the lift's setpoint to this value continuously.
+	 * 
+	 * @param pos
+	 *            - the desired FINAL height
+	 * @return the FIRST height to travel to; in all cases but GROUND, FIRST = FINAL
+	 *         targ
+	 */
 	public double getDesiredDistFromPos(LiftHeight pos) {
 		double desiredDist;
-		switch (pos) {
+		LiftHeight height = pos;
+		switch (height) {
 		case GROUND:
-			desiredDist = 0;
-			break;
+			if (atHoldCube()) {
+				desiredDist = GROUND_DIST;
+				// false bc doesn't need to hold the GROUND as a second consecutive targ anymore
+				goToGround = false;
+				break;
+			} else {
+				// falls through to HOLD_CUBE case
+				height = LiftHeight.HOLD_CUBE;
+				goToGround = true;
+			}
 		case HOLD_CUBE:
-			desiredDist = 4;
+			desiredDist = HOLD_CUBE_DIST;
+			goToGround = false;
 			break;
 		case SWITCH:
 			desiredDist = SWITCH_DIST;
+			goToGround = false;
 			break;
 		case SCALE:
 			desiredDist = SCALE_DIST;
+			goToGround = false;
 			break;
 		case BAR:
 			desiredDist = BAR_DIST;
+			goToGround = false;
 			break;
 		default:
-			desiredDist = 0;
+			desiredDist = GROUND_DIST;
+			goToGround = false;
 			break;
 		}
 
@@ -101,10 +155,17 @@ public double getDesiredDistFromPos(LiftHeight pos) {
 	}
 
 	/**
-	 * @return the rate of the lift encoder (in/s)
+	 * @return the desired LiftHeight to be at
+	 */
+	public LiftHeight desiredPos() {
+		return desiredPos;
+	}
+
+	/**
+	 * @return the absolute rate of the lift encoder (in/s)
 	 */
 	public double getSpeed() {
-		return liftEnc.getRate();
+		return Math.abs(liftEnc.getRate());
 	}
 
 	/**

From bc420e37eed4037047b91b9bd1ea571312675ce5 Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Fri, 16 Mar 2018 09:45:37 -0700
Subject: [PATCH 17/36] other max spd option (commented out) in MoveLift

---
 .../org/usfirst/frc/team199/Robot2018/commands/MoveLift.java   | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
index 97f4194..84b056a 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
@@ -27,7 +27,10 @@ public MoveLift(Lift lift, boolean up) {
 	@Override
 	protected void initialize() {
 		lift.disable();
+		// below Lift Speed may be for manual testing only, but commented out code 2
+		// lines below can be used if want max speed
 		SPEED = Robot.getConst("Lift Speed", 0.05);
+		// SPEED = lift.getLiftMaxSpeed();
 	}
 
 	// Called repeatedly when this Command is scheduled to run

From 499526a2a35b42c59af5d46c6fe996d87e36b1ae Mon Sep 17 00:00:00 2001
From: lhmcgann <hailey.mcgann@gmail.com>
Date: Fri, 16 Mar 2018 09:53:25 -0700
Subject: [PATCH 18/36] changed voltage offset default (Lift line 282) back to
 0.25

---
 .../src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 50db6d2..38eed12 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -279,7 +279,7 @@ protected double returnPIDInput() {
 	protected void usePIDOutput(double output) {
 		double out = output;
 		double spd = liftEnc.getRate();
-		out += Robot.getConst("Lift: Necessary Voltage", 0.125);
+		out += Robot.getConst("Lift: Necessary Voltage", 0.25);
 		runMotor(out);
 	}
 

From 82b7bdcb6e7eadcac61ff5afc787413b0fddf09f Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 11:37:58 -0700
Subject: [PATCH 19/36] juft a shuffleboard layout saved

---
 shuffleboard.json | 994 ----------------------------------------------
 1 file changed, 994 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index f721183..e69de29 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -1,994 +0,0 @@
-{
-  "dividerPosition": 0.28486646884273,
-  "tabPane": [
-    {
-      "title": "PID Testing",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,0": {
-            "size": [
-              1,
-              2
-            ],
-            "content": {
-              "_type": "PID Controller",
-              "_source0": "network_table:///SmartDashboard/Left PID Controller",
-              "_title": "SmartDashboard/Left PID Controller"
-            }
-          },
-          "1,0": {
-            "size": [
-              1,
-              2
-            ],
-            "content": {
-              "_type": "PID Controller",
-              "_source0": "network_table:///SmartDashboard/Right PID Controller",
-              "_title": "SmartDashboard/Right PID Controller"
-            }
-          },
-          "0,2": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Left Enc Rate",
-              "_title": "/SmartDashboard/Left Enc Rate",
-              "Visible time": 30.0
-            }
-          },
-          "5,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Left VPID Error",
-              "_title": "SmartDashboard/Left VPID Error"
-            }
-          },
-          "2,2": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Right Enc Rate",
-              "_title": "/SmartDashboard/Right Enc Rate",
-              "Visible time": 30.0
-            }
-          },
-          "5,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Right VPID Error",
-              "_title": "SmartDashboard/Right VPID Error"
-            }
-          },
-          "2,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
-              "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 30.0,
-              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
-            }
-          },
-          "4,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
-              "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 30.0,
-              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
-            }
-          },
-          "4,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Const/VelocityRightkI",
-              "_title": "SmartDashboard/Const/VelocityRightkI"
-            }
-          },
-          "4,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Const/VelocityLeftkI",
-              "_title": "SmartDashboard/Const/VelocityLeftkI"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "Drivetrain",
-      "autoPopulate": true,
-      "autoPopulatePrefix": "SmartDashboard/Drivetrain",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
-              "_title": "Left VPID Targ"
-            }
-          },
-          "1,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
-              "_title": "Right VPID Targ"
-            }
-          },
-          "2,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Current Max Speed",
-              "_title": "Current Max Speed"
-            }
-          },
-          "3,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Dist",
-              "_title": "Left Enc Dist"
-            }
-          },
-          "4,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Rate",
-              "_title": "Left Enc Rate"
-            }
-          },
-          "5,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Dist",
-              "_title": "Right Enc Dist"
-            }
-          },
-          "0,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Rate",
-              "_title": "Right Enc Rate"
-            }
-          },
-          "1,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Enc Avg Dist",
-              "_title": "Enc Avg Dist"
-            }
-          },
-          "0,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Boolean Box",
-              "_source0": "network_table:///SmartDashboard/Bool/High Gear",
-              "_title": "SmartDashboard/Bool/High Gear",
-              "colorWhenTrue": "#7CFC00FF",
-              "colorWhenFalse": "#8B0000FF"
-            }
-          },
-          "2,1": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Toggle Switch",
-              "_source0": "network_table:///Preferences/Bool/Square Drive Values",
-              "_title": "Preferences/Bool/Square Drive Values"
-            }
-          },
-          "2,2": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Toggle Switch",
-              "_source0": "network_table:///Preferences/Bool/Square Joystick Values",
-              "_title": "Preferences/Bool/Square Joystick Values"
-            }
-          },
-          "1,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Toggle Switch",
-              "_source0": "network_table:///Preferences/Bool/Teleop velocity PID",
-              "_title": "Preferences/Bool/Teleop velocity PID"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "Move PID Testing",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "3,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Right Enc Dist",
-              "_title": "/SmartDashboard/Right Enc Dist",
-              "Visible time": 10.0
-            }
-          },
-          "1,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Left Enc Dist",
-              "_title": "/SmartDashboard/Left Enc Dist",
-              "Visible time": 10.0
-            }
-          },
-          "5,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Network Table Tree",
-              "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
-              "_title": "LiveWindow/Ungrouped/Scheduler"
-            }
-          },
-          "0,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Move PID Output",
-              "_title": "SmartDashboard/Move PID Output"
-            }
-          },
-          "1,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Move PID Result",
-              "_title": "SmartDashboard/Move PID Result"
-            }
-          },
-          "1,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Move PID Error",
-              "_title": "SmartDashboard/Move PID Error"
-            }
-          },
-          "2,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Right VPID Error",
-              "_title": "SmartDashboard/Right VPID Error"
-            }
-          },
-          "2,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Left VPID Error",
-              "_title": "SmartDashboard/Left VPID Error"
-            }
-          },
-          "3,2": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
-              "_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
-              "Visible time": 10.0,
-              "SmartDashboard/Drivetrain/Left VPID Targ visible": true
-            }
-          },
-          "5,2": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Graph",
-              "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
-              "_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
-              "Visible time": 10.0,
-              "SmartDashboard/Drivetrain/Right VPID Targ visible": true
-            }
-          },
-          "5,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Const/MoveTolerance",
-              "_title": "SmartDashboard/Const/MoveTolerance"
-            }
-          },
-          "0,0": {
-            "size": [
-              1,
-              2
-            ],
-            "content": {
-              "_type": "PID Controller",
-              "_source0": "network_table:///SmartDashboard/Move PID",
-              "_title": "SmartDashboard/Move PID"
-            }
-          },
-          "0,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Move Targ",
-              "_title": "Preferences/Const/Move Targ"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "Turn PID Testing",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "2,0": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Network Table Tree",
-              "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
-              "_title": "LiveWindow/Ungrouped/Scheduler"
-            }
-          },
-          "3,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Turn PID Error",
-              "_title": "SmartDashboard/Turn PID Error"
-            }
-          },
-          "4,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Turn PID Output",
-              "_title": "SmartDashboard/Turn PID Output"
-            }
-          },
-          "2,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Turn PID Result",
-              "_title": "SmartDashboard/Turn PID Result"
-            }
-          },
-          "0,2": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Angle",
-              "_title": "SmartDashboard/Angle"
-            }
-          },
-          "0,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "PID Controller",
-              "_source0": "network_table:///SmartDashboard/Turn PID",
-              "_title": "SmartDashboard/Turn PID"
-            }
-          },
-          "4,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Left VPID Error",
-              "_title": "SmartDashboard/Left VPID Error"
-            }
-          },
-          "4,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Right VPID Error",
-              "_title": "SmartDashboard/Right VPID Error"
-            }
-          },
-          "4,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Const/Turn Targ",
-              "_title": "SmartDashboard/Const/Turn Targ"
-            }
-          },
-          "3,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Const/TurnTolerance",
-              "_title": "SmartDashboard/Const/TurnTolerance"
-            }
-          },
-          "2,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Output",
-              "_title": "Preferences/Const/Output"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "PID Constants",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/TurnkD",
-              "_title": "Preferences/Const/TurnkD"
-            }
-          },
-          "0,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/TurnkI",
-              "_title": "Preferences/Const/TurnkI"
-            }
-          },
-          "0,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/TurnkP",
-              "_title": "Preferences/Const/TurnkP"
-            }
-          },
-          "4,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Output",
-              "_title": "Preferences/Const/Output"
-            }
-          },
-          "2,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/MovekP",
-              "_title": "Preferences/Const/MovekP"
-            }
-          },
-          "2,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/MovekI",
-              "_title": "Preferences/Const/MovekI"
-            }
-          },
-          "2,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/MovekD",
-              "_title": "/Preferences/Const/MovekD"
-            }
-          },
-          "2,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/MoveTolerance",
-              "_title": "Preferences/Const/MoveTolerance"
-            }
-          },
-          "0,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/TurnTolerance",
-              "_title": "Preferences/Const/TurnTolerance"
-            }
-          },
-          "4,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Weight of Robot",
-              "_title": "Preferences/Const/Weight of Robot"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "Autonomous",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "ComboBox Chooser",
-              "_source0": "network_table:///SmartDashboard/Starting Position",
-              "_title": "SmartDashboard/Starting Position"
-            }
-          },
-          "0,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "ComboBox Chooser",
-              "_source0": "network_table:///SmartDashboard/LL",
-              "_title": "SmartDashboard/LL"
-            }
-          },
-          "1,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "ComboBox Chooser",
-              "_source0": "network_table:///SmartDashboard/LR",
-              "_title": "SmartDashboard/LR"
-            }
-          },
-          "0,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "ComboBox Chooser",
-              "_source0": "network_table:///SmartDashboard/RL",
-              "_title": "SmartDashboard/RL"
-            }
-          },
-          "1,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "ComboBox Chooser",
-              "_source0": "network_table:///SmartDashboard/RR",
-              "_title": "SmartDashboard/RR"
-            }
-          },
-          "1,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Auto Delay",
-              "_title": "SmartDashboard/Auto Delay"
-            }
-          },
-          "2,0": {
-            "size": [
-              3,
-              2
-            ],
-            "content": {
-              "_type": "Network Table Tree",
-              "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
-              "_title": "LiveWindow/Ungrouped/Scheduler"
-            }
-          },
-          "2,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/MoveTolerance",
-              "_title": "Preferences/Const/MoveTolerance"
-            }
-          },
-          "0,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Stall Torque",
-              "_title": "Preferences/Const/Stall Torque"
-            }
-          },
-          "1,3": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/PID Move Fudge Factor",
-              "_title": "Preferences/Const/PID Move Fudge Factor"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "VPID Testing 2",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityLeftkD",
-              "_title": "Preferences/Const/VelocityLeftkD"
-            }
-          },
-          "0,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityLeftkI",
-              "_title": "Preferences/Const/VelocityLeftkI"
-            }
-          },
-          "1,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityRightkD",
-              "_title": "Preferences/Const/VelocityRightkD"
-            }
-          },
-          "1,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityRightkI",
-              "_title": "Preferences/Const/VelocityRightkI"
-            }
-          },
-          "2,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VPID Test Set",
-              "_title": "Preferences/Const/VPID Test Set"
-            }
-          },
-          "3,0": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/kTimeoutMs",
-              "_title": "Preferences/Const/kTimeoutMs"
-            }
-          },
-          "3,1": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/Motor Deadband",
-              "_title": "Preferences/Const/Motor Deadband"
-            }
-          },
-          "0,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityLeftkF",
-              "_title": "Preferences/Const/VelocityLeftkF"
-            }
-          },
-          "1,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///Preferences/Const/VelocityRightkF",
-              "_title": "Preferences/Const/VelocityRightkF"
-            }
-          },
-          "3,2": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/Time Const (kD)",
-              "_title": "SmartDashboard/Time Const (kD)"
-            }
-          }
-        }
-      }
-    },
-    {
-      "title": "Tab 8",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "tiles": {
-          "0,1": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Encoder",
-              "_source0": "network_table:///LiveWindow/Lift/Encoder",
-              "_title": "LiveWindow/Lift/Encoder"
-            }
-          },
-          "0,0": {
-            "size": [
-              2,
-              1
-            ],
-            "content": {
-              "_type": "Speed Controller",
-              "_source0": "network_table:///LiveWindow/Lift/CIM Motor",
-              "_title": "LiveWindow/Lift/CIM Motor",
-              "controllable": false,
-              "orientation": "HORIZONTAL"
-            }
-          },
-          "0,2": {
-            "size": [
-              6,
-              2
-            ],
-            "content": {
-              "_type": "Network Table Tree",
-              "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
-              "_title": "LiveWindow/Ungrouped/Scheduler"
-            }
-          }
-        }
-      }
-    }
-  ]
-}
\ No newline at end of file

From 0777385148a29c9eb8fa1ef74c326803986923bc Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 11:39:42 -0700
Subject: [PATCH 20/36] uncommented cams

---
 .../src/org/usfirst/frc/team199/Robot2018/Robot.java   | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index ba9e39b..34db38d 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -23,6 +23,7 @@
 import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
 
+import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.PIDController;
@@ -59,6 +60,7 @@ public class Robot extends IterativeRobot {
 	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
 	public static SmartDashboardInterface sd = new SmartDashboardInterface() {
+		@Override
 		public double getConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			if (!pref.containsKey("Const/" + key)) {
@@ -71,6 +73,7 @@ public double getConst(String key, double def) {
 			return pref.getDouble("Const/" + key, def);
 		}
 
+		@Override
 		public void putConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			pref.putDouble("Const/" + key, def);
@@ -79,14 +82,17 @@ public void putConst(String key, double def) {
 			}
 		}
 
+		@Override
 		public void putData(String string, PIDController controller) {
 			SmartDashboard.putData(string, controller);
 		}
 
+		@Override
 		public void putNumber(String string, double d) {
 			SmartDashboard.putNumber(string, d);
 		}
 
+		@Override
 		public void putBoolean(String string, boolean b) {
 			SmartDashboard.putBoolean(string, b);
 		}
@@ -157,8 +163,8 @@ public void robotInit() {
 		autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
 
 		listen = new Listener();
-		// CameraServer.getInstance().startAutomaticCapture(0);
-		// CameraServer.getInstance().startAutomaticCapture(1);
+		CameraServer.getInstance().startAutomaticCapture(0);
+		CameraServer.getInstance().startAutomaticCapture(1);
 	}
 
 	/**

From e6c1538951b243fe0efb42ebf4c02a52ea46effc Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 11:46:23 -0700
Subject: [PATCH 21/36] stop intake button: manip btn 7

---
 .../org/usfirst/frc/team199/Robot2018/OI.java |  5 ++++
 .../Robot2018/commands/StopIntake.java        | 23 +++++++++++++++++++
 2 files changed, 28 insertions(+)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index e8d42ba..0152990 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -20,6 +20,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
+import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
 import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
 import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
@@ -61,6 +62,7 @@ public class OI {
 	private JoystickButton outakeCubeButton;
 	private JoystickButton toggleLeftIntakeButton;
 	private JoystickButton toggleRightIntakeButton;
+	private JoystickButton stopIntakeButton;
 
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -131,6 +133,9 @@ public OI(Robot robot) {
 			toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
 			toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
 			toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
+
+			stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 7));
+			stopIntakeButton.whenPressed(new StopIntake());
 		}
 
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
new file mode 100644
index 0000000..ec6075c
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class StopIntake extends InstantCommand {
+
+	public StopIntake() {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		super();
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		Robot.intakeEject.stopIntake();
+	}
+}

From 5af25ef0b21f46a0ee323e437a6d6c4e402c03ff Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 11:48:31 -0700
Subject: [PATCH 22/36] fixed kf in PIDMove

---
 .../src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
index ea24ee7..b2897cb 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
@@ -67,7 +67,7 @@ public PIDMove(double target, double[] point, DrivetrainInterface dt, SmartDashb
 		double kP = r / Robot.rmap.getDrivetrainTimeConstant() / maxSpeed;
 		double kI = 0;
 		double kD = r / maxSpeed;
-		double kF = 1 / (maxSpeed * sd.getConst("Default PID Update Time", 0.05)) / maxSpeed;
+		double kF = 1 / maxSpeed * sd.getConst("Default PID Update Time", 0.05);
 
 		moveController = new PIDController(kP, kI, kD, kF, avg, this);
 		sd.putData("Move PID", moveController);

From eb6f6f3953526f252246f85a2e4da7ccf99221dd Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 15:45:14 -0700
Subject: [PATCH 23/36] changes to lift and intake

lift: reverted to old UpdateLiftPos, etc; implemented command group AutoLift for auto functions of going to the ground in 2 stages
intake: IntakeCube and OuttakeCube ran motors in reverse direction than what they should have
---
 .../org/usfirst/frc/team199/Robot2018/OI.java | 12 ++---
 .../team199/Robot2018/commands/AutoLift.java  | 22 +++++++++
 .../Robot2018/commands/EjectToExchange.java   |  8 ++--
 .../Robot2018/commands/EjectToScale.java      | 14 +++---
 .../Robot2018/commands/EjectToSwitch.java     | 16 ++++---
 .../Robot2018/commands/IntakeCube.java        | 11 ++++-
 .../Robot2018/commands/LiftToPosition.java    | 10 ++--
 .../team199/Robot2018/commands/MoveLift.java  |  2 +-
 .../Robot2018/commands/OuttakeCube.java       |  7 ++-
 .../commands/UpdateLiftPosition.java          | 12 ++---
 .../Robot2018/subsystems/IntakeEject.java     |  3 ++
 .../team199/Robot2018/subsystems/Lift.java    | 47 ++++++++++---------
 12 files changed, 103 insertions(+), 61 deletions(-)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index 46c9c0c..3e7d808 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -7,10 +7,10 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
+import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
 import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
 import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
 import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
-import org.usfirst.frc.team199.Robot2018.commands.LiftToPosition;
 import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
 import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
 import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
@@ -106,8 +106,8 @@ public OI(Robot robot) {
 		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
 
 		testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
-		testLiftPID.whenPressed(new LiftToPosition(Robot.lift,
-				SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
+		testLiftPID.whenPressed(
+				new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
 
 		rightJoy = new Joystick(1);
 		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
@@ -125,6 +125,9 @@ public OI(Robot robot) {
 		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
 		moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
 
+		stopIntakeButton = new JoystickButton(rightJoy, getButton("Stop Intake Button", 6));
+		stopIntakeButton.whenPressed(new StopIntake());
+
 		manipulator = new Joystick(2);
 		if (manipulator.getButtonCount() == 0) {
 			System.err.println(
@@ -148,9 +151,6 @@ public OI(Robot robot) {
 			toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
 			toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
 			toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
-
-			stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 7));
-			stopIntakeButton.whenPressed(new StopIntake());
 		}
 
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
new file mode 100644
index 0000000..7cdef5b
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutoLift extends CommandGroup {
+
+	public AutoLift(Lift lift, String height) {
+		if (height.equals("GROUND")) {
+			addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE")));
+			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
+		} else {
+
+			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
+		}
+	}
+}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
index b6a8913..b4a3b38 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java
@@ -9,8 +9,8 @@
  */
 public class EjectToExchange extends CommandGroup {
 
-    public EjectToExchange() {
-        addSequential(new LiftToPosition(Robot.lift, "GROUND"));
-        addSequential(new OuttakeCube());
-    }
+	public EjectToExchange() {
+		addSequential(new AutoLift(Robot.lift, "GROUND"));
+		addSequential(new OuttakeCube());
+	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
index 801e392..43da132 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
@@ -9,11 +9,13 @@
  */
 public class EjectToScale extends CommandGroup {
 
-    public EjectToScale() {
-    		addSequential(new LiftToPosition(Robot.lift, "SCALE"));
-		addSequential(new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+	public EjectToScale() {
+		addSequential(new AutoLift(Robot.lift, "SCALE"));
+		addSequential(
+				new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
-		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
-		addSequential(new LiftToPosition(Robot.lift, "GROUND"));
-    }
+		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd,
+				Robot.dt.getDistEncAvg()));
+		addSequential(new AutoLift(Robot.lift, "GROUND"));
+	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
index 46612f2..79c7653 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
@@ -9,11 +9,13 @@
  */
 public class EjectToSwitch extends CommandGroup {
 
-    public EjectToSwitch() {
-    		addSequential(new LiftToPosition(Robot.lift, "SWITCH"));
-    		addSequential(new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
-    		addSequential(new OuttakeCube());
-    		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
-    		addSequential(new LiftToPosition(Robot.lift, "GROUND"));
-    }
+	public EjectToSwitch() {
+		addSequential(new AutoLift(Robot.lift, "SWITCH"));
+		addSequential(
+				new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+		addSequential(new OuttakeCube());
+		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd,
+				Robot.dt.getDistEncAvg()));
+		addSequential(new AutoLift(Robot.lift, "GROUND"));
+	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
index e95340e..c08f053 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  *
@@ -20,15 +21,18 @@ public IntakeCube() {
 	}
 
 	// Called just before this Command runs the first time
+	@Override
 	protected void initialize() {
 		tim.reset();
-		tim.start();
+		// tim.start();
 		overDraw = false;
 	}
 
 	// Called repeatedly when this Command is scheduled to run
+	@Override
 	protected void execute() {
-		Robot.intakeEject.runIntake(-1);
+		Robot.intakeEject.runIntake(1);
+		SmartDashboard.putBoolean("Has Cube", Robot.intakeEject.hasCube());
 		if (Robot.intakeEject.hasCube()) {
 			if (!overDraw) {
 				overDraw = true;
@@ -42,16 +46,19 @@ protected void execute() {
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
+	@Override
 	protected boolean isFinished() {
 		return tim.get() > Robot.getConst("Has Cube Timeout", 0.5);
 	}
 
 	// Called once after isFinished returns true
+	@Override
 	protected void end() {
 	}
 
 	// Called when another command which requires one or more of the same
 	// subsystems is scheduled to run
+	@Override
 	protected void interrupted() {
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
index 6aab891..528a6d8 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
@@ -15,10 +15,10 @@ public class LiftToPosition extends Command {
 	private LiftHeight pos;
 	private boolean goToGround;
 
-	public LiftToPosition(Lift lift, String goal) {
+	public LiftToPosition(Lift lift, LiftHeight goal) {
 		requires(Robot.lift);
 		this.lift = lift;
-		pos = LiftHeight.toLH(goal);
+		pos = goal;
 	}
 
 	// Called just before this Command runs the first time
@@ -26,14 +26,14 @@ public LiftToPosition(Lift lift, String goal) {
 	protected void initialize() {
 		lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
 				Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
+		double setpoint = lift.getDesiredDistFromPos(pos);
+		lift.setSetpoint(setpoint);
+		System.out.println("Target Height: " + setpoint);
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		double setpoint = lift.getDesiredDistFromPos(pos);
-		lift.setSetpoint(setpoint);
-		System.out.println("Target Height: " + setpoint);
 		System.out.println("Current Height: " + lift.getHeight());
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
index 84b056a..9f2cb86 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java
@@ -29,7 +29,7 @@ protected void initialize() {
 		lift.disable();
 		// below Lift Speed may be for manual testing only, but commented out code 2
 		// lines below can be used if want max speed
-		SPEED = Robot.getConst("Lift Speed", 0.05);
+		SPEED = Robot.getConst("Lift Speed", 0.5);
 		// SPEED = lift.getLiftMaxSpeed();
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
index ad4ec0b..ffbdcc6 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
@@ -18,6 +18,7 @@ public OuttakeCube() {
 	}
 
 	// Called just before this Command runs the first time
+	@Override
 	protected void initialize() {
 		tim = new Timer();
 		tim.reset();
@@ -25,22 +26,26 @@ protected void initialize() {
 	}
 
 	// Called repeatedly when this Command is scheduled to run
+	@Override
 	protected void execute() {
-		Robot.intakeEject.runIntake(1);
+		Robot.intakeEject.runIntake(-1);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
+	@Override
 	protected boolean isFinished() {
 		return tim.get() > Robot.getConst("Outake Time", 0.5);
 	}
 
 	// Called once after isFinished returns true
+	@Override
 	protected void end() {
 		Robot.intakeEject.runIntake(0);
 	}
 
 	// Called when another command which requires one or more of the same
 	// subsystems is scheduled to run
+	@Override
 	protected void interrupted() {
 		end();
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index 8716b1a..c50ce59 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -59,12 +59,12 @@ protected void execute() {
 				lift.setSetpoint(desiredDist);
 			}
 
-			// if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
-			// desiredPos = LiftHeight.GROUND;
-			// desiredDist = lift.getDesiredDistFromPos(desiredPos);
-			// lift.setSetpoint(desiredDist);
-			// goToGround = false;
-			// }
+			if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
+				desiredPos = LiftHeight.GROUND;
+				desiredDist = lift.getDesiredDistFromPos(desiredPos);
+				lift.setSetpoint(desiredDist);
+				goToGround = false;
+			}
 		}
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
index 5043465..147c027 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -64,6 +64,9 @@ public double getRightIntakeSpeed() {
 	 */
 	@Override
 	public boolean hasCube() {
+		SmartDashboard.putNumber("Intake Current Left", Robot.rmap.getPort("PDP Intake Left Channel", 4));
+
+		SmartDashboard.putNumber("Intake Current Right", Robot.rmap.getPort("PDP Intake Left Channel", 11));
 		return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 39)
 				|| pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current",
 						39);
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
index 38eed12..629f133 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -96,15 +96,16 @@ private boolean atHoldCube() {
 	 * @return true if reached target height and does not still have to go to the
 	 *         ground as a secondary, consecutive target
 	 */
-	@Override
-	public boolean onTarget() {
-		double targ = getSetpoint();
-		double tol = Robot.getConst("Lift Tolerance", 0.8);
-		double height = getHeight();
-		// true if (height within tolerance of targ) and (lift is not moving)
-		boolean wInTol = height <= (targ + tol) && height >= (targ - tol) && getSpeed() <= 0.1;
-		return wInTol && !goToGround;
-	}
+	// @Override
+	// public boolean onTarget() {
+	// double targ = getSetpoint();
+	// double tol = Robot.getConst("Lift Tolerance", 0.8);
+	// double height = getHeight();
+	// // true if (height within tolerance of targ) and (lift is not moving)
+	// boolean wInTol = height <= (targ + tol) && height >= (targ - tol) &&
+	// getSpeed() <= 0.1;
+	// return wInTol && !goToGround;
+	// }
 
 	/**
 	 * Set the lift's setpoint to this value continuously.
@@ -119,23 +120,23 @@ public double getDesiredDistFromPos(LiftHeight pos) {
 		LiftHeight height = pos;
 		switch (height) {
 		case GROUND:
-			if (atHoldCube()) {
-				desiredDist = GROUND_DIST;
-				// false bc doesn't need to hold the GROUND as a second consecutive targ anymore
-				goToGround = false;
-				break;
-			} else {
-				// falls through to HOLD_CUBE case
-				height = LiftHeight.HOLD_CUBE;
-				goToGround = true;
-			}
+			// if (atHoldCube() || goToGround) {
+			desiredDist = GROUND_DIST;
+			// false bc doesn't need to hold the GROUND as a second consecutive targ anymore
+			goToGround = true;
+			break;
+		// } else {
+		// // falls through to HOLD_CUBE case
+		// height = LiftHeight.HOLD_CUBE;
+		// goToGround = false;
+		// }
 		case HOLD_CUBE:
 			desiredDist = HOLD_CUBE_DIST;
-			goToGround = false;
+			// goToGround = false;
 			break;
 		case SWITCH:
 			desiredDist = SWITCH_DIST;
-			goToGround = false;
+			// goToGround = false;
 			break;
 		case SCALE:
 			desiredDist = SCALE_DIST;
@@ -143,11 +144,11 @@ public double getDesiredDistFromPos(LiftHeight pos) {
 			break;
 		case BAR:
 			desiredDist = BAR_DIST;
-			goToGround = false;
+			// goToGround = false;
 			break;
 		default:
 			desiredDist = GROUND_DIST;
-			goToGround = false;
+			// goToGround = false;
 			break;
 		}
 

From e7b22d9c473c5f8d7327ac38fa60be87f6c2c40a Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 15:48:47 -0700
Subject: [PATCH 24/36] deleted an empty line and fixed kf in PIDTurn

---
 .../org/usfirst/frc/team199/Robot2018/commands/AutoLift.java  | 1 -
 .../org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java   | 4 ++--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
index 7cdef5b..fb3fdbc 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
@@ -15,7 +15,6 @@ public AutoLift(Lift lift, String height) {
 			addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE")));
 			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
 		} else {
-
 			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
 		}
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
index b4dae99..10b348c 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
@@ -154,11 +154,11 @@ protected void initialize() {
 		double maxTurnSpeed = Robot.getConst("Max Turn Radians Per Second", 4.0) * 180 / Math.PI;
 		double updateTime = sd.getConst("Default PID Update Time", 0.05);
 		double r = Robot.getConst("TurnPidR", 3.0);
-		
+
 		double kP = r / Robot.getConst("TurnTimeConstant", 0.2) / maxTurnSpeed;
 		double kI = 0;
 		double kD = r / (maxTurnSpeed * updateTime);
-		double kF = 1 / (maxTurnSpeed * updateTime);
+		double kF = 1 / maxTurnSpeed * updateTime;
 		turnController.setPID(kP, kI, kD, kF);
 
 		if (!turnToPoint) {

From cf83e64ab58932c72c497a79d3d52885d338eecb Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 16:52:40 -0700
Subject: [PATCH 25/36] stopintake doesn't work

---
 .../Robot2018/commands/IntakeCube.java        |  1 +
 .../Robot2018/commands/StopIntake.java        | 21 +++++++++++++++++--
 .../Robot2018/subsystems/IntakeEject.java     |  1 +
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
index c08f053..c8dd4db 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
@@ -54,6 +54,7 @@ protected boolean isFinished() {
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
+		Robot.intakeEject.stopIntake();
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
index ec6075c..f9be50b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
@@ -2,22 +2,39 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
 
 /**
  *
  */
-public class StopIntake extends InstantCommand {
+public class StopIntake extends Command {
+
+	private Timer tim;
 
 	public StopIntake() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
 		super();
+		// requires(Robot.intakeEject);
 	}
 
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
+		tim = new Timer();
+		tim.reset();
+		tim.start();
+	}
+
+	@Override
+	protected void execute() {
 		Robot.intakeEject.stopIntake();
 	}
+
+	@Override
+	protected boolean isFinished() {
+		// TODO Auto-generated method stub
+		return tim.get() > 3;
+	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
index 147c027..aa04aa0 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -21,6 +21,7 @@ public class IntakeEject extends Subsystem implements IntakeEjectInterface {
 	private final DoubleSolenoid rightSolenoid = RobotMap.rightIntakeSolenoid;
 	private boolean leftOpen = isOpen(leftSolenoid.get());
 	private boolean rightOpen = isOpen(rightSolenoid.get());
+	private boolean hasCube = false;
 
 	/**
 	 * Return whether or not a side of the intake (L/R) is open

From 84b44340e175416f75e4c0f5ac7f251dedca39ad Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 17:03:36 -0700
Subject: [PATCH 26/36] fix StopIntake

The problem was that StopIntake was called and the intake was stopped,
but the IntakeCube command was still running so StopIntake only jerked
it to a stop and then restarted.
---
 Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java     | 2 ++
 .../org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java | 3 ++-
 .../org/usfirst/frc/team199/Robot2018/commands/StopIntake.java | 3 ++-
 3 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 80897aa..bfb4bef 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -54,6 +54,8 @@ public class Robot extends IterativeRobot {
 
 	public static Map<String, ArrayList<String[]>> autoScripts;
 
+	public static boolean stopIntake = false;
+
 	Command autonomousCommand;
 	SendableChooser<Autonomous.Position> posChooser = new SendableChooser<Autonomous.Position>();
 	Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
index c8dd4db..1712d7a 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
@@ -48,13 +48,14 @@ protected void execute() {
 	// Make this return true when this Command no longer needs to run execute()
 	@Override
 	protected boolean isFinished() {
-		return tim.get() > Robot.getConst("Has Cube Timeout", 0.5);
+		return tim.get() > Robot.getConst("Has Cube Timeout", 0.5) || Robot.stopIntake;
 	}
 
 	// Called once after isFinished returns true
 	@Override
 	protected void end() {
 		Robot.intakeEject.stopIntake();
+		Robot.stopIntake = false;
 	}
 
 	// Called when another command which requires one or more of the same
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
index f9be50b..b613243 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java
@@ -22,6 +22,7 @@ public StopIntake() {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
+		Robot.stopIntake = true;
 		tim = new Timer();
 		tim.reset();
 		tim.start();
@@ -29,7 +30,7 @@ protected void initialize() {
 
 	@Override
 	protected void execute() {
-		Robot.intakeEject.stopIntake();
+		// Robot.intakeEject.stopIntake();
 	}
 
 	@Override

From 0957fa6582585f20ba3e7d1614851dfe5770d1d9 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 21:41:40 -0700
Subject: [PATCH 27/36] set intake motor to be correct for intake/outtake
 commands

---
 .../org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java  | 2 +-
 .../org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
index 1712d7a..70bb74b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
@@ -31,7 +31,7 @@ protected void initialize() {
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		Robot.intakeEject.runIntake(1);
+		Robot.intakeEject.runIntake(-1);
 		SmartDashboard.putBoolean("Has Cube", Robot.intakeEject.hasCube());
 		if (Robot.intakeEject.hasCube()) {
 			if (!overDraw) {
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
index ffbdcc6..7f6c08c 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
@@ -28,7 +28,7 @@ protected void initialize() {
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		Robot.intakeEject.runIntake(-1);
+		Robot.intakeEject.runIntake(1);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()

From 69e9b365b5f69b0f4151b120cb01668d3e684ef7 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 21:42:30 -0700
Subject: [PATCH 28/36] fixed UpdateLiftPosition so it goes to HOLD_CUBE before
 going to GROUND

---
 .../frc/team199/Robot2018/commands/UpdateLiftPosition.java   | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index c50ce59..aab8707 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -44,7 +44,7 @@ protected void execute() {
 			System.out.println("POV Reading: " + angle);
 
 			if (angle == 180) {
-				desiredPos = LiftHeight.GROUND;
+				desiredPos = LiftHeight.HOLD_CUBE;
 				goToGround = true;
 			} else if (angle == 270) {
 				desiredPos = LiftHeight.HOLD_CUBE;
@@ -65,6 +65,9 @@ protected void execute() {
 				lift.setSetpoint(desiredDist);
 				goToGround = false;
 			}
+			System.out.println("Desired Pos: " + desiredPos);
+			System.out.println("Desired Dist: " + desiredDist);
+			System.out.println("Current Dist: " + lift.getHeight());
 		}
 	}
 

From 8a1864d7e9fce732f3cd563829db879d6a2d21a5 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 21:42:57 -0700
Subject: [PATCH 29/36] deleted redundant line in PIDTurn

---
 .../src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
index 10b348c..d4aabcd 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
@@ -57,12 +57,12 @@ public class PIDTurn extends Command implements PIDOutput {
 	public PIDTurn(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs,
 			boolean absoluteRotation, boolean turnToPoint) {
 		this.target = target;
+		System.out.println(this.target);
 		this.point = point;
 		this.dt = dt;
 		this.ahrs = ahrs;
 		this.sd = sd;
 		this.turnToPoint = turnToPoint;
-		this.target = target;
 		this.absoluteRotation = absoluteRotation;
 
 		if (Robot.dt != null) {

From b2c7853e130ed387bb1356bd3261f8e4f564a016 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 21:43:47 -0700
Subject: [PATCH 30/36] changed default max intake current for hasCube method
 to be much lower

---
 .../usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
index aa04aa0..812bf3d 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -68,7 +68,7 @@ public boolean hasCube() {
 		SmartDashboard.putNumber("Intake Current Left", Robot.rmap.getPort("PDP Intake Left Channel", 4));
 
 		SmartDashboard.putNumber("Intake Current Right", Robot.rmap.getPort("PDP Intake Left Channel", 11));
-		return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 39)
+		return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 15)
 				|| pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current",
 						39);
 	}

From 0df70bc967c8eb5d7c69ebb0d19372fd8c3a2986 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 21:44:14 -0700
Subject: [PATCH 31/36] a SD layout w/ cams

---
 CompSD.xml | 66 +++++++++++++++++++++++++++++++-----------------------
 1 file changed, 38 insertions(+), 28 deletions(-)

diff --git a/CompSD.xml b/CompSD.xml
index d252b62..30dfbca 100644
--- a/CompSD.xml
+++ b/CompSD.xml
@@ -1,70 +1,80 @@
 <xml version="1.0">
 <dashboard>
+	<widget field="Has Cube" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="132"/>
+	</widget>
+	<widget field="Intake Current Right" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="111"/>
+	</widget>
+	<widget field="Intake Current Left" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="90"/>
+	</widget>
+	<widget field="Left Current draw" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="482" y="581"/>
+	</widget>
+	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.RobotPreferences">
+		<location x="875" y="450"/>
+	</static-widget>
+	<widget field="Intake Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="487" y="556"/>
+	</widget>
+	<widget field="Right Solenoid Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="478" y="614"/>
+	</widget>
+	<widget field="Right Current draw" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="493" y="601"/>
+	</widget>
 	<widget field="High Gear" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.BooleanBox">
-		<location x="14" y="4"/>
+		<location x="79" y="11"/>
 		<width>93</width>
 		<height>78</height>
 	</widget>
 	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
-		<location x="830" y="4"/>
-		<width>529</width>
-		<height>399</height>
+		<location x="783" y="1"/>
+		<width>586</width>
+		<height>430</height>
 		<property name="Camera Choice" value="USB Camera 1"/>
 	</static-widget>
 	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
-		<location x="324" y="17"/>
-		<width>502</width>
-		<height>383</height>
+		<location x="148" y="2"/>
+		<width>630</width>
+		<height>459</height>
 		<property name="Camera Choice" value=""/>
 	</static-widget>
-	<widget field="Turn PID" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
-		<location x="182" y="426"/>
-	</widget>
-	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.RobotPreferences">
-		<location x="6" y="87"/>
-		<width>284</width>
-		<height>205</height>
-	</static-widget>
 	<widget field="LR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
 		<location x="31" y="2"/>
 	</widget>
 	<widget field="LL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
 		<location x="16" y="12"/>
 	</widget>
-	<widget field="Left PID Controller" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
-		<location x="51" y="417"/>
-	</widget>
-	<widget field="PIDController[1]" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
-		<location x="134" y="229"/>
-	</widget>
 	<widget field="RR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
 		<location x="4" y="11"/>
 	</widget>
 	<widget field="Starting Position" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
 		<location x="11" y="16"/>
 	</widget>
-	<widget field="Move PID" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
-		<location x="32" y="467"/>
-	</widget>
-	<widget field="Right PID Controller" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
-		<location x="-26" y="264"/>
-		<width>159</width>
-	</widget>
 	<widget field="RL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
 		<location x="13" y="30"/>
 	</widget>
 </dashboard>
 <live-window>
 </live-window>
+	<hidden field="Right PID Controller"/>
 	<hidden field="VPID Right kI"/>
 	<hidden field="VPID Left kI"/>
 	<hidden field="Turn PID Error"/>
 	<hidden field="Auto Delay"/>
+	<hidden field="PIDController[2]"/>
 	<hidden field="Move PID Error"/>
+	<hidden field="Turn PID"/>
+	<hidden field="Left PID Controller"/>
+	<hidden field="Left Solenoid Open"/>
 	<hidden field="Angle"/>
 	<hidden field="Turn PID Result"/>
+	<hidden field="Move PID"/>
 	<hidden field="VPID kF"/>
 	<hidden field="Move PID Output"/>
+	<hidden field="PIDController[1]"/>
 	<hidden field="VPID Right kP"/>
 	<hidden field="VPID Left kP"/>
 	<hidden field="Turn PID Output"/>

From 0a669313e570166e55594c896dad3d9493bcdece Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Fri, 16 Mar 2018 22:00:25 -0700
Subject: [PATCH 32/36] just moving some commands/button around to try and make
 room on the manipulator

---
 .../org/usfirst/frc/team199/Robot2018/OI.java | 26 ++++++++-----------
 .../Robot2018/commands/DefaultIntake.java     |  5 ++++
 .../Robot2018/subsystems/IntakeEject.java     |  4 +--
 3 files changed, 18 insertions(+), 17 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index 3e7d808..ef9cac7 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -23,8 +23,6 @@
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
 import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
-import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
-import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
 import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
@@ -125,9 +123,6 @@ public OI(Robot robot) {
 		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
 		moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
 
-		stopIntakeButton = new JoystickButton(rightJoy, getButton("Stop Intake Button", 6));
-		stopIntakeButton.whenPressed(new StopIntake());
-
 		manipulator = new Joystick(2);
 		if (manipulator.getButtonCount() == 0) {
 			System.err.println(
@@ -137,20 +132,21 @@ public OI(Robot robot) {
 			closeIntakeButton.whenPressed(new CloseIntake());
 			openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
 			openIntakeButton.whenPressed(new OpenIntake());
-			// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
-			// Button", 3));
-			// raiseIntake.whenPressed(new RaiseIntake());
-			// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
-			// Button", 4));
-			// lowerIntake.whenPressed(new LowerIntake());
+
 			intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
 			intakeCubeButton.whenPressed(new IntakeCube());
 			outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
 			outakeCubeButton.whenPressed(new OuttakeCube());
-			toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
-			toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
-			toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
-			toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
+
+			stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
+			stopIntakeButton.whenPressed(new StopIntake());
+
+			// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+			// Left Intake Button", 3));
+			// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
+			// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+			// Right Intake Button", 4));
+			// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
 		}
 
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
index c7e7372..8066d3a 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
@@ -18,6 +18,7 @@ public DefaultIntake() {
 	}
 
 	// Called just before this Command runs the first time
+	@Override
 	protected void initialize() {
 		try {
 			Robot.oi.manipulator.getRawAxis(1);
@@ -28,6 +29,7 @@ protected void initialize() {
 	}
 
 	// Called repeatedly when this Command is scheduled to run
+	@Override
 	protected void execute() {
 		// 1 and 5 represent the axes' index in driver station
 		if (manipulatorPluggedIn) {
@@ -37,17 +39,20 @@ protected void execute() {
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
+	@Override
 	protected boolean isFinished() {
 		return false;
 	}
 
 	// Called once after isFinished returns true
+	@Override
 	protected void end() {
 		Robot.intakeEject.runIntake(0);
 	}
 
 	// Called when another command which requires one or more of the same
 	// subsystems is scheduled to run
+	@Override
 	protected void interrupted() {
 		end();
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
index 812bf3d..edf1b26 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -2,7 +2,6 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
-import org.usfirst.frc.team199.Robot2018.commands.DefaultIntake;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
@@ -39,7 +38,8 @@ public boolean isOpen(DoubleSolenoid.Value val) {
 	 */
 	@Override
 	public void initDefaultCommand() {
-		setDefaultCommand(new DefaultIntake());
+		// I don't want this on the manipulator joysticks during a match
+		// setDefaultCommand(new DefaultIntake());
 	}
 
 	/**

From 71efb41e8bcc8d14529ac8b933741d9285ae74e1 Mon Sep 17 00:00:00 2001
From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com>
Date: Sat, 17 Mar 2018 12:43:25 -0700
Subject: [PATCH 33/36] raise lift earlier

---
 AAAScripts/scripts.txt                        | 20 ++++++++-----------
 .../Robot2018/commands/EjectToScale.java      |  4 ++--
 .../Robot2018/commands/EjectToSwitch.java     |  4 ++--
 3 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt
index da2cacf..a613e59 100644
--- a/AAAScripts/scripts.txt
+++ b/AAAScripts/scripts.txt
@@ -9,7 +9,6 @@ moveto (0,120) #Drive past auto line
 RRxx:
 moveto (0,155) 
 turn -90
-move 10
 switch
 
 RLxx:
@@ -17,7 +16,7 @@ move 56
 turn -90
 move 170
 turn 90
-move 35
+move 25
 switch
 
 
@@ -35,7 +34,6 @@ exchange
 RRxE:
 moveto (0,155) 
 turn -90
-move 12
 switch
 turn -90
 move 105
@@ -55,7 +53,7 @@ move 56
 turn -90
 move 170
 turn 90
-move 35
+move 25
 switch
 move -40
 turn 90
@@ -80,7 +78,6 @@ moveto (0, 120) #Drive past auto line
 LLxx:
 move 150
 turn 90
-move 12
 switch
 
 LRxx:
@@ -88,7 +85,7 @@ move 56
 turn 90
 move 170
 turn -90
-move 35
+move 25
 switch	
 
 
@@ -106,7 +103,6 @@ exchange
 LLxE:
 move 150
 turn 90
-move 12
 switch
 turn 90
 move 95
@@ -126,7 +122,7 @@ move 56
 turn 90
 move 170
 turn -90
-move 35
+move 25
 switch
 move -40
 turn -90
@@ -148,17 +144,17 @@ moveto (0,50) (48,50) (48,92)  #cross baseline
 
 # Switch
 CRxx:
-moveto (0,50) (48,50) (48,95) 
+moveto (0,50) (48,50) (48,85) 
 switch #deploy switch 
 
 CLxx:
-moveto (0,50) (-56,50) (-56,95)
+moveto (0,50) (-56,50) (-56,85)
 switch #deploy switch 
 
 
 # Switch and Exchange
 CRxE:
-moveto (0,50) (48,50) (48,95) 
+moveto (0,50) (48,50) (48,85) 
 switch #deploy switch 
 move -48 # 3 feet back
 turn -90
@@ -173,7 +169,7 @@ exchange
 
 
 CLxE:
-moveto (0,50) (-56,50) (-56,95) 
+moveto (0,50) (-56,50) (-56,85) 
 switch #deploy switch 
 move -48 # 3 feet back
 turn 90
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
index 43da132..bc23e32 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
@@ -12,9 +12,9 @@ public class EjectToScale extends CommandGroup {
 	public EjectToScale() {
 		addSequential(new AutoLift(Robot.lift, "SCALE"));
 		addSequential(
-				new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+				new PIDMove(Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
-		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd,
+		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd,
 				Robot.dt.getDistEncAvg()));
 		addSequential(new AutoLift(Robot.lift, "GROUND"));
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
index 79c7653..11bf36a 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
@@ -12,9 +12,9 @@ public class EjectToSwitch extends CommandGroup {
 	public EjectToSwitch() {
 		addSequential(new AutoLift(Robot.lift, "SWITCH"));
 		addSequential(
-				new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+				new PIDMove(Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
-		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd,
+		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd,
 				Robot.dt.getDistEncAvg()));
 		addSequential(new AutoLift(Robot.lift, "GROUND"));
 	}

From cb621a043237016207366be14c8d001e609ef447 Mon Sep 17 00:00:00 2001
From: doawelul <dfang314@gmail.com>
Date: Sat, 17 Mar 2018 14:55:21 -0700
Subject: [PATCH 34/36] added quick clamp to movespeed when in auto and lift is
 up

---
 .../src/org/usfirst/frc/team199/Robot2018/Robot.java     | 4 +++-
 .../usfirst/frc/team199/Robot2018/commands/PIDMove.java  | 9 ++++++++-
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
index 80897aa..4e0272b 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -53,7 +53,7 @@ public class Robot extends IterativeRobot {
 	public static OI oi;
 
 	public static Map<String, ArrayList<String[]>> autoScripts;
-
+	public static boolean auto;
 	Command autonomousCommand;
 	SendableChooser<Autonomous.Position> posChooser = new SendableChooser<Autonomous.Position>();
 	Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
@@ -208,6 +208,7 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
+		auto = true;
 		dt.resetAHRS();
 		AutoUtils.state = new State(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
@@ -237,6 +238,7 @@ public void autonomousPeriodic() {
 
 	@Override
 	public void teleopInit() {
+		auto = false;
 		System.out.println("In teleopInit()");
 		dt.resetAHRS();
 		AutoUtils.state = new State(0, 0, 0);
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
index b2897cb..5dab6eb 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
@@ -4,6 +4,7 @@
 import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
@@ -131,7 +132,13 @@ public void initialize() {
 		dt.resetDistEncs();
 		moveController.disable();
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
-		moveController.setOutputRange(-1.0, 1.0);
+		LiftHeight currPos = Robot.lift.getCurrPos();
+		if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) {
+			moveController.setOutputRange(Robot.getConst("Auto Min Speed", -0.5),
+					Robot.getConst("Auto Max Speed", 0.5));
+		} else {
+			moveController.setOutputRange(-1.0, 1.0);
+		}
 		moveController.setContinuous(false);
 		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.5));
 		System.out.println("move target = " + target);

From 0904738cc38096e1dae307d8c7e0de5c9443b624 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <driverstation@carlmontrobotics.org>
Date: Sat, 17 Mar 2018 15:43:01 -0700
Subject: [PATCH 35/36] sfr working code (not breaking rn :))

---
 .../org/usfirst/frc/team199/Robot2018/OI.java | 68 ++++++++++---------
 .../autonomous/scriptupload/scriptupload.py   |  2 +-
 .../Robot2018/commands/DefaultIntake.java     | 23 ++++---
 .../Robot2018/commands/IntakeCube.java        |  2 +
 .../Robot2018/commands/OuttakeCube.java       |  1 +
 .../team199/Robot2018/commands/PIDMove.java   |  7 +-
 .../team199/Robot2018/commands/PIDTurn.java   |  9 ++-
 .../Robot2018/commands/ToggleIntake.java      | 23 +++++++
 .../commands/UpdateLiftPosition.java          | 63 +++++++++--------
 .../Robot2018/subsystems/Drivetrain.java      | 13 ++++
 .../Robot2018/subsystems/IntakeEject.java     | 12 +++-
 11 files changed, 145 insertions(+), 78 deletions(-)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
index ef9cac7..c4d2111 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -8,12 +8,10 @@
 package org.usfirst.frc.team199.Robot2018;
 
 import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
-import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
 import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
 import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
 import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
-import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
 import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
@@ -23,6 +21,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
 import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
+import org.usfirst.frc.team199.Robot2018.commands.ToggleIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
 import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
@@ -66,6 +65,7 @@ public class OI {
 	private JoystickButton outakeCubeButton;
 	private JoystickButton toggleLeftIntakeButton;
 	private JoystickButton toggleRightIntakeButton;
+	private JoystickButton toggleIntakeButton;
 	private JoystickButton stopIntakeButton;
 
 	public int getButton(String key, int def) {
@@ -98,11 +98,6 @@ public OI(Robot robot) {
 		findTurnTimeConstantButton
 				.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
 
-		moveLiftPIDUpButton = new JoystickButton(leftJoy, getButton("Run Lift PID Up", 3));
-		moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
-		moveLiftPIDDownButton = new JoystickButton(leftJoy, getButton("Run Lift PID Down", 4));
-		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
-
 		testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
 		testLiftPID.whenPressed(
 				new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
@@ -124,30 +119,41 @@ public OI(Robot robot) {
 		moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
 
 		manipulator = new Joystick(2);
-		if (manipulator.getButtonCount() == 0) {
-			System.err.println(
-					"ERROR: manipulator does not appear to be plugged in. Disabling intake code. Restart code with manipulator plugged in to enable intake code");
-		} else {
-			closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
-			closeIntakeButton.whenPressed(new CloseIntake());
-			openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
-			openIntakeButton.whenPressed(new OpenIntake());
-
-			intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
-			intakeCubeButton.whenPressed(new IntakeCube());
-			outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
-			outakeCubeButton.whenPressed(new OuttakeCube());
-
-			stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
-			stopIntakeButton.whenPressed(new StopIntake());
-
-			// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
-			// Left Intake Button", 3));
-			// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
-			// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
-			// Right Intake Button", 4));
-			// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
-		}
+		// if (manipulator.getButtonCount() == 0) {
+		// System.err.println(
+		// "ERROR: manipulator does not appear to be plugged in. Disabling intake code.
+		// Restart code with manipulator plugged in to enable intake code");
+		// } else {
+
+		// closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake
+		// Button", 1));
+		// closeIntakeButton.whenPressed(new CloseIntake());
+		// openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake
+		// Button", 2));
+		// openIntakeButton.whenPressed(new OpenIntake());
+
+		moveLiftPIDUpButton = new JoystickButton(manipulator, getButton("Run Lift PID Up", 4));
+		moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
+		moveLiftPIDDownButton = new JoystickButton(manipulator, getButton("Run Lift PID Down", 1));
+		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
+
+		toggleIntakeButton = new JoystickButton(manipulator, getButton("Toggle Intake Button", 2));
+		toggleIntakeButton.whenPressed(new ToggleIntake());
+
+		stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
+		stopIntakeButton.whenPressed(new StopIntake());
+
+		intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
+		intakeCubeButton.whenPressed(new IntakeCube());
+		outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
+		outakeCubeButton.whenPressed(new OuttakeCube());
+		// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+		// Left Intake Button", 3));
+		// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
+		// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+		// Right Intake Button", 4));
+		// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
+		// }
 
 	}
 
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py
index 948a80f..54c95b1 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py
@@ -2,7 +2,7 @@
 from networktables import NetworkTables
 import __future__
 import time
-NetworkTables.initialize(server='10.1.99.2')
+NetworkTables.initialize(server='172.22.11.2')
 prefs = NetworkTables.getTable("Preferences")
 
 go = False #true when file has been successfully read
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
index 8066d3a..7ef26e7 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
@@ -20,22 +20,25 @@ public DefaultIntake() {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		try {
-			Robot.oi.manipulator.getRawAxis(1);
-		} catch (NullPointerException e) {
-			System.err.println("[ERROR] Manipulator not plugged in.");
-			manipulatorPluggedIn = false;
-		}
+		// try {
+		// Robot.oi.manipulator.getRawAxis(1);
+		// } catch (NullPointerException e) {
+		// System.err.println("[ERROR] Manipulator not plugged in.");
+		// manipulatorPluggedIn = false;
+		// }
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
 		// 1 and 5 represent the axes' index in driver station
-		if (manipulatorPluggedIn) {
-			Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
-			Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
-		}
+		// if (manipulatorPluggedIn) {
+		// Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
+		// Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
+		// }
+		Robot.intakeEject.runLeftIntake(Robot.getConst("Intake Holding Speed", 0.15));
+		Robot.intakeEject.runRightIntake(Robot.getConst("Intake Holding Speed", 0.15));
+
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
index 70bb74b..041f4a2 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java
@@ -17,6 +17,7 @@ public class IntakeCube extends Command {
 	public IntakeCube() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
+		// requires(Robot.intakeEject);
 		tim = new Timer();
 	}
 
@@ -36,6 +37,7 @@ protected void execute() {
 		if (Robot.intakeEject.hasCube()) {
 			if (!overDraw) {
 				overDraw = true;
+				// tim.reset();
 				tim.start();
 			}
 		} else {
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
index 7f6c08c..579c83d 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java
@@ -15,6 +15,7 @@ public class OuttakeCube extends Command {
 	public OuttakeCube() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
+		requires(Robot.intakeEject);
 	}
 
 	// Called just before this Command runs the first time
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
index b2897cb..5bf87a0 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
@@ -23,6 +23,7 @@ public class PIDMove extends Command implements PIDOutput {
 	private SmartDashboardInterface sd;
 	private double[] point;
 	private boolean absolute;
+	private boolean hasInitialized = false;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -120,6 +121,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	public void initialize() {
+		hasInitialized = true;
 		if (absolute) {
 			double dx = point[0] - AutoUtils.state.getX();
 			double dy = point[1] - AutoUtils.state.getY();
@@ -174,7 +176,7 @@ protected boolean isFinished() {
 	@Override
 	protected void end() {
 		moveController.disable();
-		System.out.println("End");
+		System.out.println("PIDMove End");
 		// moveController.free();
 
 		double angle = Math.toRadians(AutoUtils.state.getRot());
@@ -212,6 +214,9 @@ protected void interrupted() {
 	 */
 	@Override
 	public void pidWrite(double output) {
+		if (!hasInitialized) {
+			return;
+		}
 		dt.arcadeDrive(output, 0);
 		sd.putNumber("Move PID Output", output);
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
index d4aabcd..a5c0e7d 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java
@@ -28,6 +28,7 @@ public class PIDTurn extends Command implements PIDOutput {
 	private double[] point;
 	private boolean turnToPoint;
 	private boolean absoluteRotation;
+	private boolean hasInitialized = false;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -148,6 +149,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	protected void initialize() {
+		hasInitialized = true;
 		// calculate pid constants
 
 		// max turn speed from FindTurnTimeConstant, converted to degrees
@@ -231,7 +233,7 @@ protected void execute() {
 	 */
 	@Override
 	protected boolean isFinished() {
-		System.out.println("isFinished");
+		// System.out.println("isFinished");
 		return (turnController.onTarget() && Math.abs(dt.getGyroRate()) < 1);
 		// return turnController.onTarget()
 		// && Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When
@@ -248,7 +250,7 @@ protected boolean isFinished() {
 	@Override
 	protected void end() {
 		turnController.disable();
-		System.out.println("end");
+		System.out.println("PIDTurn end");
 		sd.putNumber("Turn PID Result", turnController.get());
 		sd.putNumber("Turn PID Error", turnController.getError());
 		// turnController.free();
@@ -279,6 +281,9 @@ protected void interrupted() {
 	 */
 	@Override
 	public void pidWrite(double output) {
+		if (!hasInitialized) {
+			return;
+		}
 		dt.arcadeDrive(0, output);
 		SmartDashboard.putNumber("Turn PID Output", output);
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java
new file mode 100644
index 0000000..f266c32
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class ToggleIntake extends InstantCommand {
+
+	public ToggleIntake() {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		super();
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		Robot.intakeEject.toggleIntake();
+	}
+}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
index aab8707..67eb5cf 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -26,49 +26,48 @@ public UpdateLiftPosition(Lift lift) {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		try {
-			Robot.oi.manipulator.getRawAxis(1);
-		} catch (NullPointerException e) {
-			System.err.println("[ERROR] Manipulator not plugged in.");
-			manipulatorPluggedIn = false;
-		}
+		// try {
+		// Robot.oi.manipulator.getRawAxis(1);
+		// } catch (NullPointerException e) {
+		// System.err.println("[ERROR] Manipulator not plugged in.");
+		// manipulatorPluggedIn = false;
+		// }
 		goToGround = false;
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		if (manipulatorPluggedIn) {
-			int angle = Robot.oi.manipulator.getPOV();
+		// if (manipulatorPluggedIn) {
+		int angle = Robot.oi.manipulator.getPOV();
 
-			System.out.println("POV Reading: " + angle);
+		System.out.println("POV Reading: " + angle);
 
-			if (angle == 180) {
-				desiredPos = LiftHeight.HOLD_CUBE;
-				goToGround = true;
-			} else if (angle == 270) {
-				desiredPos = LiftHeight.HOLD_CUBE;
-				goToGround = false;
-			} else if (angle != -1) {
-				desiredPos = LiftHeight.SWITCH;
-				goToGround = false;
-			}
+		if (angle == 180) {
+			desiredPos = LiftHeight.HOLD_CUBE;
+			goToGround = true;
+		} else if (angle == 270) {
+			desiredPos = LiftHeight.HOLD_CUBE;
+			goToGround = false;
+		} else if (angle != -1) {
+			desiredPos = LiftHeight.SWITCH;
+			goToGround = false;
+		}
 
-			if (goToGround || angle != -1) {
-				desiredDist = lift.getDesiredDistFromPos(desiredPos);
-				lift.setSetpoint(desiredDist);
-			}
+		if (goToGround || angle != -1) {
+			desiredDist = lift.getDesiredDistFromPos(desiredPos);
+			lift.setSetpoint(desiredDist);
+		}
 
-			if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
-				desiredPos = LiftHeight.GROUND;
-				desiredDist = lift.getDesiredDistFromPos(desiredPos);
-				lift.setSetpoint(desiredDist);
-				goToGround = false;
-			}
-			System.out.println("Desired Pos: " + desiredPos);
-			System.out.println("Desired Dist: " + desiredDist);
-			System.out.println("Current Dist: " + lift.getHeight());
+		if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
+			desiredPos = LiftHeight.GROUND;
+			desiredDist = lift.getDesiredDistFromPos(desiredPos);
+			lift.setSetpoint(desiredDist);
+			goToGround = false;
 		}
+		System.out.println("Desired Pos: " + desiredPos);
+		System.out.println("Desired Dist: " + desiredDist);
+		System.out.println("Current Dist: " + lift.getHeight());
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java
index 6b6ba36..77b44d5 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java
@@ -52,11 +52,13 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	private SmartDashboardInterface sd;
 
 	private boolean highGear;
+	private int inverted;
 
 	public Drivetrain(SmartDashboardInterface sd) {
 		this.sd = sd;
 
 		highGear = false;
+		inverted = 1;
 
 		// all 0s for controller construction because they all get set to right values
 		// by resetAllVelocityPIDConsts
@@ -85,11 +87,16 @@ public Drivetrain(SmartDashboardInterface sd) {
 		}
 	}
 
+	public void reverseDT() {
+		inverted *= -1;
+	}
+
 	@Override
 	public void initDefaultCommand() {
 		setDefaultCommand(new TeleopDrive());
 	}
 
+	@Override
 	public boolean isVPIDUsed() {
 		return Robot.getBool("Teleop velocity PID", false);
 	}
@@ -151,6 +158,7 @@ public boolean VPIDsOnTarg() {
 	 * 
 	 * @return the rate of the left encoder
 	 */
+	@Override
 	public double getLeftEncRate() {
 		return leftEncRate.getRate();
 	}
@@ -160,6 +168,7 @@ public double getLeftEncRate() {
 	 * 
 	 * @return the rate of the right encoder
 	 */
+	@Override
 	public double getRightEncRate() {
 		return rightEncRate.getRate();
 	}
@@ -331,6 +340,7 @@ public void resetAHRS() {
 		fancyGyro.reset();
 	}
 
+	@Override
 	public double getGyroRate() {
 		return fancyGyro.getRate();
 	}
@@ -432,6 +442,7 @@ public void shiftGearSolenoidOff() {
 	/**
 	 * @return the gyroscope
 	 */
+	@Override
 	public PIDSource getGyro() {
 		return fancyGyro;
 	}
@@ -530,6 +541,7 @@ public void putVelocityControllersToDashboard() {
 		SmartDashboard.putData("Right PID Controller", rightVelocityController);
 	}
 
+	@Override
 	public double getPIDMoveConstant() {
 		double G = Robot.rmap.getGearRatio();
 		double T = Robot.rmap.getStallTorque();
@@ -541,6 +553,7 @@ public double getPIDMoveConstant() {
 		return Math.sqrt(Robot.rmap.convertMtoIn((8 * T * G) / (R * M)));
 	}
 
+	@Override
 	public double getPIDTurnConstant() {
 		double G = Robot.rmap.getGearRatio();
 		double T = Robot.rmap.getStallTorque();
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
index edf1b26..b955133 100644
--- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -2,6 +2,7 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.commands.DefaultIntake;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
@@ -39,7 +40,7 @@ public boolean isOpen(DoubleSolenoid.Value val) {
 	@Override
 	public void initDefaultCommand() {
 		// I don't want this on the manipulator joysticks during a match
-		// setDefaultCommand(new DefaultIntake());
+		setDefaultCommand(new DefaultIntake());
 	}
 
 	/**
@@ -119,6 +120,15 @@ public void runIntake(double speed) {
 		runRightIntake(speed);
 	}
 
+	/**
+	 * Toggles the left and right intake between open (kReverse) and closed
+	 * (kForward).
+	 */
+	public void toggleIntake() {
+		toggleLeftIntake();
+		toggleRightIntake();
+	}
+
 	/**
 	 * Toggles the left intake between open (kReverse) and closed (kForward).
 	 */

From dfa7f506be09fb58fa11d3c3e9befc4292c61cfd Mon Sep 17 00:00:00 2001
From: doawelul <dfang314@gmail.com>
Date: Sat, 17 Mar 2018 15:48:27 -0700
Subject: [PATCH 36/36] added file for motion profiling

---
 .../commands/MotionProfilingMove.java         | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)
 create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java

diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java
new file mode 100644
index 0000000..6c25206
--- /dev/null
+++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class MotionProfilingMove extends Command {
+
+    public MotionProfilingMove() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    }
+
+    // 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() {
+    }
+}