diff --git a/PowerUp/.idea/workspace.xml b/PowerUp/.idea/workspace.xml
index f4db4c4..727d046 100644
--- a/PowerUp/.idea/workspace.xml
+++ b/PowerUp/.idea/workspace.xml
@@ -2,6 +2,8 @@
 <project version="4">
   <component name="ChangeListManager">
     <list default="true" id="217ff8ac-6cfc-4d31-a1f3-01f35dc8d5e3" name="Default" comment="">
+      <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" />
+      <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" />
       <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" />
     </list>
     <ignored path="$PROJECT_DIR$/bin/" />
@@ -20,8 +22,8 @@
       <file leaf-file-name="RobotMap.java" pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="510">
-              <caret line="46" column="76" lean-forward="false" selection-start-line="46" selection-start-column="76" selection-end-line="46" selection-end-column="76" />
+            <state relative-caret-position="631">
+              <caret line="50" column="4" lean-forward="false" selection-start-line="50" selection-start-column="4" selection-end-line="50" selection-end-column="4" />
               <folding />
             </state>
           </provider>
@@ -30,8 +32,8 @@
       <file leaf-file-name="Robot.java" pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="410">
-              <caret line="172" column="93" lean-forward="false" selection-start-line="172" selection-start-column="93" selection-end-line="172" selection-end-column="93" />
+            <state relative-caret-position="360">
+              <caret line="126" column="52" lean-forward="false" selection-start-line="126" selection-start-column="52" selection-end-line="126" selection-end-column="52" />
               <folding />
             </state>
           </provider>
@@ -40,8 +42,8 @@
       <file leaf-file-name="OI.java" pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="52">
-              <caret line="31" column="31" lean-forward="false" selection-start-line="31" selection-start-column="31" selection-end-line="31" selection-end-column="31" />
+            <state relative-caret-position="781">
+              <caret line="62" column="1" lean-forward="false" selection-start-line="62" selection-start-column="1" selection-end-line="62" selection-end-column="1" />
               <folding>
                 <element signature="imports" expanded="true" />
               </folding>
@@ -49,14 +51,24 @@
           </provider>
         </entry>
       </file>
-      <file leaf-file-name="Elevator.java" pinned="false" current-in-tab="true">
+      <file leaf-file-name="Elevator.java" pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="485">
-              <caret line="199" column="27" lean-forward="true" selection-start-line="199" selection-start-column="27" selection-end-line="199" selection-end-column="27" />
+            <state relative-caret-position="284">
+              <caret line="183" column="48" lean-forward="false" selection-start-line="183" selection-start-column="48" selection-end-line="183" selection-end-column="48" />
               <folding>
-                <element signature="e#5346#5347#0" expanded="true" />
-                <element signature="e#5411#5412#0" expanded="true" />
+                <element signature="e#5035#5036#0" expanded="true" />
+                <element signature="e#5055#5056#0" expanded="true" />
+                <element signature="e#5086#5087#0" expanded="true" />
+                <element signature="e#5107#5108#0" expanded="true" />
+                <element signature="e#5147#5148#0" expanded="true" />
+                <element signature="e#5179#5180#0" expanded="true" />
+                <element signature="e#5469#5470#0" expanded="true" />
+                <element signature="e#5525#5526#0" expanded="true" />
+                <element signature="e#6251#6252#0" expanded="true" />
+                <element signature="e#6305#6306#0" expanded="true" />
+                <element signature="e#6342#6343#0" expanded="true" />
+                <element signature="e#6396#6397#0" expanded="true" />
               </folding>
             </state>
           </provider>
@@ -72,14 +84,14 @@
           </provider>
         </entry>
       </file>
-      <file leaf-file-name="ElevatorCommand.java" pinned="false" current-in-tab="false">
+      <file leaf-file-name="ElevatorCommand.java" pinned="false" current-in-tab="true">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="639">
-              <caret line="70" column="26" lean-forward="false" selection-start-line="70" selection-start-column="26" selection-end-line="70" selection-end-column="26" />
+            <state relative-caret-position="400">
+              <caret line="37" column="81" lean-forward="false" selection-start-line="37" selection-start-column="81" selection-end-line="37" selection-end-column="81" />
               <folding>
-                <element signature="e#2272#2273#0" expanded="true" />
-                <element signature="e#2291#2292#0" expanded="true" />
+                <element signature="e#2542#2543#0" expanded="true" />
+                <element signature="e#2561#2562#0" expanded="true" />
               </folding>
             </state>
           </provider>
@@ -98,11 +110,11 @@
       <file leaf-file-name="Gamepad.java" pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="175">
-              <caret line="7" column="13" lean-forward="false" selection-start-line="7" selection-start-column="13" selection-end-line="7" selection-end-column="13" />
+            <state relative-caret-position="277">
+              <caret line="21" column="21" lean-forward="false" selection-start-line="21" selection-start-column="21" selection-end-line="21" selection-end-column="21" />
               <folding>
-                <marker date="1519167965201" expanded="true" signature="1946:2296" ph="{...}" />
-                <marker date="1519167965201" expanded="true" signature="2367:2838" ph="{...}" />
+                <element signature="e#471#472#0" expanded="true" />
+                <element signature="e#507#508#0" expanded="true" />
               </folding>
             </state>
           </provider>
@@ -123,6 +135,9 @@
       <find>...</find>
       <find>LEVER</find>
       <find>Point</find>
+      <find>enable</find>
+      <find>setPo</find>
+      <find>system.out.prin</find>
     </findStrings>
     <replaceStrings>
       <replace>null</replace>
@@ -181,11 +196,11 @@
         <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java" />
         <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java" />
         <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java" />
         <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Config.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" />
+        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java" />
         <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" />
+        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" />
+        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" />
       </list>
     </option>
   </component>
@@ -198,9 +213,9 @@
   <component name="MavenRunner">
     <option name="runMavenInBackground" value="false" />
   </component>
-  <component name="ProjectFrameBounds" extendedState="7">
-    <option name="x" value="291" />
-    <option name="y" value="-11" />
+  <component name="ProjectFrameBounds" extendedState="6">
+    <option name="x" value="260" />
+    <option name="y" value="40" />
     <option name="width" value="1400" />
     <option name="height" value="1000" />
   </component>
@@ -591,7 +606,7 @@
     <servers />
   </component>
   <component name="TimeTrackingManager">
-    <option name="totallyTimeSpent" value="13347000" />
+    <option name="totallyTimeSpent" value="16347000" />
   </component>
   <component name="ToolWindowManager">
     <frame x="-11" y="-11" width="1942" height="1102" extended-state="6" />
@@ -611,6 +626,7 @@
       <window_info id="Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="4" side_tool="false" content_ui="tabs" />
       <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.2308108" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" />
       <window_info id="Database" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="false" content_ui="tabs" />
+      <window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.3293348" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
       <window_info id="Structure" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
       <window_info id="Ant Build" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
       <window_info id="UI Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="5" side_tool="false" content_ui="tabs" />
@@ -644,7 +660,6 @@
       <window_info id="Terminal" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="9" side_tool="false" content_ui="tabs" />
       <window_info id="Run with VisualVM" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="10" side_tool="false" content_ui="tabs" />
       <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.16770834" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" />
-      <window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.3293348" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
       <window_info id="Theme Preview" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
       <window_info id="Favorites" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="true" content_ui="tabs" />
       <window_info id="Debug with VisualVM" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="11" side_tool="false" content_ui="tabs" />
@@ -799,6 +814,11 @@
       <provider selected="true" editor-type-id="text-editor">
         <state relative-caret-position="378">
           <caret line="18" column="13" lean-forward="false" selection-start-line="18" selection-start-column="13" selection-end-line="18" selection-end-column="13" />
+          <folding>
+            <element signature="imports" expanded="false" />
+            <element signature="e#455#456#0" expanded="false" />
+            <element signature="e#503#504#0" expanded="false" />
+          </folding>
         </state>
       </provider>
     </entry>
@@ -900,29 +920,18 @@
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="175">
-          <caret line="7" column="13" lean-forward="false" selection-start-line="7" selection-start-column="13" selection-end-line="7" selection-end-column="13" />
-          <folding>
-            <marker date="1519167965201" expanded="true" signature="1946:2296" ph="{...}" />
-            <marker date="1519167965201" expanded="true" signature="2367:2838" ph="{...}" />
-          </folding>
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java">
+    <entry file="file://$PROJECT_DIR$/build.xml">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="510">
-          <caret line="46" column="76" lean-forward="false" selection-start-line="46" selection-start-column="76" selection-end-line="46" selection-end-column="76" />
+        <state relative-caret-position="300">
+          <caret line="12" column="28" lean-forward="false" selection-start-line="12" selection-start-column="28" selection-end-line="12" selection-end-column="28" />
           <folding />
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/build.xml">
+    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="300">
-          <caret line="12" column="28" lean-forward="false" selection-start-line="12" selection-start-column="28" selection-end-line="12" selection-end-column="28" />
+        <state relative-caret-position="631">
+          <caret line="50" column="4" lean-forward="false" selection-start-line="50" selection-start-column="4" selection-end-line="50" selection-end-column="4" />
           <folding />
         </state>
       </provider>
@@ -937,40 +946,61 @@
     </entry>
     <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="52">
-          <caret line="31" column="31" lean-forward="false" selection-start-line="31" selection-start-column="31" selection-end-line="31" selection-end-column="31" />
+        <state relative-caret-position="781">
+          <caret line="62" column="1" lean-forward="false" selection-start-line="62" selection-start-column="1" selection-end-line="62" selection-end-column="1" />
           <folding>
             <element signature="imports" expanded="true" />
           </folding>
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java">
+    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="410">
-          <caret line="172" column="93" lean-forward="false" selection-start-line="172" selection-start-column="93" selection-end-line="172" selection-end-column="93" />
-          <folding />
+        <state relative-caret-position="277">
+          <caret line="21" column="21" lean-forward="false" selection-start-line="21" selection-start-column="21" selection-end-line="21" selection-end-column="21" />
+          <folding>
+            <element signature="e#471#472#0" expanded="true" />
+            <element signature="e#507#508#0" expanded="true" />
+          </folding>
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java">
+    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="639">
-          <caret line="70" column="26" lean-forward="false" selection-start-line="70" selection-start-column="26" selection-end-line="70" selection-end-column="26" />
+        <state relative-caret-position="284">
+          <caret line="183" column="48" lean-forward="false" selection-start-line="183" selection-start-column="48" selection-end-line="183" selection-end-column="48" />
           <folding>
-            <element signature="e#2272#2273#0" expanded="true" />
-            <element signature="e#2291#2292#0" expanded="true" />
+            <element signature="e#5035#5036#0" expanded="true" />
+            <element signature="e#5055#5056#0" expanded="true" />
+            <element signature="e#5086#5087#0" expanded="true" />
+            <element signature="e#5107#5108#0" expanded="true" />
+            <element signature="e#5147#5148#0" expanded="true" />
+            <element signature="e#5179#5180#0" expanded="true" />
+            <element signature="e#5469#5470#0" expanded="true" />
+            <element signature="e#5525#5526#0" expanded="true" />
+            <element signature="e#6251#6252#0" expanded="true" />
+            <element signature="e#6305#6306#0" expanded="true" />
+            <element signature="e#6342#6343#0" expanded="true" />
+            <element signature="e#6396#6397#0" expanded="true" />
           </folding>
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java">
+    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java">
+      <provider selected="true" editor-type-id="text-editor">
+        <state relative-caret-position="360">
+          <caret line="126" column="52" lean-forward="false" selection-start-line="126" selection-start-column="52" selection-end-line="126" selection-end-column="52" />
+          <folding />
+        </state>
+      </provider>
+    </entry>
+    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="485">
-          <caret line="199" column="27" lean-forward="true" selection-start-line="199" selection-start-column="27" selection-end-line="199" selection-end-column="27" />
+        <state relative-caret-position="400">
+          <caret line="37" column="81" lean-forward="false" selection-start-line="37" selection-start-column="81" selection-end-line="37" selection-end-column="81" />
           <folding>
-            <element signature="e#5346#5347#0" expanded="true" />
-            <element signature="e#5411#5412#0" expanded="true" />
+            <element signature="e#2542#2543#0" expanded="true" />
+            <element signature="e#2561#2562#0" expanded="true" />
           </folding>
         </state>
       </provider>
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
index 1baf05b..f44a058 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
@@ -129,6 +129,7 @@ public void teleopInit() {
         if (autonCommands != null)
             autonCommands.cancel();
 
+        elevator.enableControl();
         drivetrain.shiftUp(); // start in high gear
         drivetrain.reset();
     }
@@ -171,6 +172,7 @@ private void updateSmartDashboard() {
         SmartDashboard.putBoolean("Elevator isZeroing", elevator.isZeroing());
         SmartDashboard.putBoolean("Elevator isZeroed", elevator.isZeroed());
         SmartDashboard.putString("Elevator power mode", elevatorMotor.getControlMode().name());
+        SmartDashboard.putBoolean("Elevator elevator mode", elevator.isMotionControlled());
     }
 
     /**
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java
index bd765be..cd1530e 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java
@@ -31,8 +31,11 @@ protected void initialize() {
 
 	@Override
 	protected void execute() {
+//		System.out.println(elevator.isMotionControlled());
 		if (!elevator.isMotionControlled()) {
-			if (Math.abs(gamepad.getLeftY()) > TOLERANCE) {
+//			System.out.println("Joystick input receiving");
+//			System.out.println("Abs get Y " + Math.abs(gamepad.getLeftY()));
+			if (Math.abs(gamepad.getLeftY()) /*TODO look at why getLeftY return 0*/ > TOLERANCE) {
 				elevator.setPower(-gamepad.getLeftY());
 			} else if (Math.abs(gamepad.getRightY()) > TOLERANCE) {
 				elevator.setPower(-gamepad.getRightY());
@@ -41,6 +44,8 @@ protected void execute() {
 			}
 
 		} else {
+//			System.out.println("Getting button inputs");
+
 			if (elevatorNudgeUp.get() && !elevator.atTopPosition()) {
 				elevator.nudge(NUDGE_DISTANCE);
 			}
@@ -59,7 +64,7 @@ protected void execute() {
 		}
 
 		if (elevatorToggleControl.get()) {
-			System.out.println("screeeee toggle pressed!");
+//			System.out.println("screeeee toggle pressed!");
 			if (elevator.isMotionControlled()) {
 				elevator.disableControl();
 			} else {
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java
index cd1074d..b6ccf25 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java
@@ -1,5 +1,18 @@
 package org.usfirst.frc.team2974.robot.subsystems;
 
+import static org.usfirst.frc.team2974.robot.Config.Elevator.ACCELERATION;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.CRUISE_SPEED;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.INCHES_TO_NU;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.KD;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.KF;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.KI;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.KP;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.MAXIMUM_HEIGHT;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.MINUMUM_HEIGHT;
+import static org.usfirst.frc.team2974.robot.Config.Elevator.TIMEOUT;
+import static org.usfirst.frc.team2974.robot.RobotMap.elevatorLimitLower;
+import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor;
+
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
@@ -10,197 +23,204 @@
 import org.usfirst.frc.team2974.robot.command.teleop.ElevatorCommand;
 import org.usfirst.frc.team2974.robot.util.ElevatorLogger;
 
-import static org.usfirst.frc.team2974.robot.Config.Elevator.*;
-import static org.usfirst.frc.team2974.robot.RobotMap.elevatorLimitLower;
-import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor;
-
 /**
- * The elevator subsystem, which raises and lowers the intake/outtake
- * <p>
- * TODO: finish me
+ * The elevator subsystem, which raises and lowers the intake/outtake <p> TODO: finish me
  */
 public class Elevator extends Subsystem {
 
-    private ElevatorLogger logger;
-    private boolean isMotionControlled;
-
-    private double power;
-
-    private boolean zeroing;
-    private boolean zeroed;
-    private Timer timer = new Timer();
+	private ElevatorLogger logger;
+	private boolean isMotionControlled;
 
-    public Elevator(ElevatorLogger logger) {
-        initConstants();
-        this.logger = logger;
-    }
+	private double power;
 
-    public double getError() {
-        if (isMotionControlled)
-            return elevatorMotor.getClosedLoopError(0);
-        else
-            return 0;
-    }
+	private boolean zeroing;
+	private boolean zeroed;
+	private Timer timer = new Timer();
 
-    @Override
-    protected void initDefaultCommand() {
-        setDefaultCommand(new ElevatorCommand());
-    }
+	public Elevator(ElevatorLogger logger) {
+		zeroing = true;
+		initConstants();
+		this.logger = logger;
+	}
+
+	public double getError() {
+		if (isMotionControlled) {
+			return elevatorMotor.getClosedLoopError(0);
+		} else {
+			return 0;
+		}
+	}
+
+	@Override
+	protected void initDefaultCommand() {
+		setDefaultCommand(new ElevatorCommand());
+	}
+
+	@Override
+	public void periodic() {
+		logger.addMotionData(
+			new ElevatorLogger.ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(),
+				getCurrentPositionNU(), power));
 
-    @Override
-    public void periodic() {
-        logger.addMotionData(new ElevatorLogger.ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(), getCurrentPositionNU(), power));
-
-        if (zeroing)
-            setPower(-0.1);
-        if (!elevatorLimitLower.get() || timer.hasPeriodPassed(2.0 /*TODO tune time to be as small as possible*/)) {
-            zeroing = false;
-            timer.stop();
-            zeroEncoder();
-            enableControl();
-        }
-    }
-
-    public void initConstants() {
-        elevatorMotor.setNeutralMode(NeutralMode.Brake);
-        elevatorMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, TIMEOUT);
-        elevatorMotor.setSensorPhase(true);
-        elevatorMotor.setInverted(false);
-
-        elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT);
-        elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, TIMEOUT);
-
-        elevatorMotor.configNominalOutputForward(0, TIMEOUT);
-        elevatorMotor.configNominalOutputReverse(0, TIMEOUT);
-        elevatorMotor.configPeakOutputForward(1, TIMEOUT);
-        elevatorMotor.configPeakOutputReverse(-1, TIMEOUT);
-
-        elevatorMotor.selectProfileSlot(0, 0);
-
-        elevatorMotor.config_kP(0, KP, TIMEOUT);
-        elevatorMotor.config_kI(0, KI, TIMEOUT);
-        elevatorMotor.config_kD(0, KD, TIMEOUT);
-        elevatorMotor.config_kF(0, KF, TIMEOUT);
-
-        elevatorMotor.configMotionCruiseVelocity(CRUISE_SPEED, TIMEOUT);
-        elevatorMotor.configMotionAcceleration(ACCELERATION, TIMEOUT);
-
-        zeroed = false;
-
-        elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10);
-        elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10);
-
-        elevatorMotor.configForwardSoftLimitEnable(true, 10);
-        elevatorMotor.configReverseSoftLimitEnable(true, 10);
-
-        /* pass false to FORCE OFF the feature.  Otherwise the enable flags above are honored */
-        elevatorMotor.overrideLimitSwitchesEnable(true);
-
-
-    }
-
-    public void zeroEncoder() {
-        elevatorMotor.setSelectedSensorPosition(0, 0, TIMEOUT); // TODO: figure out later
-
-        /* +14 rotations forward when using CTRE Mag encoder */
-        elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); // TODO: FIX
-        /* -15 rotations reverse when using CTRE Mag encoder */
-        elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); // TODO: FIX
-
-        elevatorMotor.configForwardSoftLimitEnable(true, 10);
-        elevatorMotor.configReverseSoftLimitEnable(true, 10);
-
-        /* pass false to FORCE OFF the feature.  Otherwise the enable flags above are honored */
-        elevatorMotor.overrideLimitSwitchesEnable(true);
-
-        zeroed = true;
-    }
-
-    public void nudge(double distance) {
-        if (isMotionControlled && zeroed) {
-            setTarget(getCurrentPosition() + distance);
-        }
-    }
-
-    public void enableControl() {
-        if (zeroed) {
-            isMotionControlled = true;
-            elevatorMotor.set(ControlMode.MotionMagic, elevatorMotor.getSelectedSensorPosition(0));
-        }
-    }
-
-    public void disableControl() {
-        isMotionControlled = false;
-        elevatorMotor.set(ControlMode.Disabled, 0);
-    }
-
-    public boolean isZeroed() {
-        return zeroed;
-    }
-
-    public boolean isZeroing() {
-        return zeroing;
-    }
-
-    public boolean isMotionControlled() {
-        return isMotionControlled;
-    }
-
-    /**
-     * Gets the current position of the elevator in inches.
-     *
-     * @return
-     */
-    public double getCurrentPosition() {
-        return elevatorMotor.getSelectedSensorPosition(0) / INCHES_TO_NU;
-    }
-
-    /**
-     * Gets the current position of the elevator in native units
-     *
-     * @return
-     */
-    public double getCurrentPositionNU() {
-        return elevatorMotor.getSelectedSensorPosition(0);
-    }
-
-    /**
-     * Sets the target position from the zero point.
-     *
-     * @param inches the position is in inches, duh
-     */
-    public void setTarget(double inches) {
-        System.out.println("Target: " + inches);
-
-        if (zeroed)
-            elevatorMotor.set(
-                    ControlMode.MotionMagic,
-                    Math.min(MAXIMUM_HEIGHT, Math.max(MINUMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */
-            );
-    }
-
-    public void setPower(double percent) {
-        percent = Math.min(.75, Math.max(-.75, percent)); // throttle power in
-
-        power = percent;
-        SmartDashboard.putNumber("Elevator Power", percent);
-        elevatorMotor.set(ControlMode.PercentOutput, percent);
-    }
-
-    public boolean atTopPosition() {
-        return getCurrentPositionNU() >= MAXIMUM_HEIGHT;
-    }
-
-    public boolean atLowerPosition() {
-        return getCurrentPositionNU() <= MINUMUM_HEIGHT;
-    }
-
-    public void startZero() {
-        if (!zeroed) {
-            zeroing = true;
+		if (zeroing) {
+			setPower(-0.2);
+
+			if (!elevatorLimitLower.get() || timer
+				.hasPeriodPassed(2.0 /*TODO tune time to be as small as possible*/)) {
+				zeroing = false;
+				timer.stop();
+				enableControl();
+				zeroEncoder();
+			}
+		}
+	}
+
+	public void initConstants() {
+		elevatorMotor.setNeutralMode(NeutralMode.Brake);
+		elevatorMotor
+			.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, TIMEOUT);
+		elevatorMotor.setSensorPhase(true);
+		elevatorMotor.setInverted(false);
+
+		elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT);
+		elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, TIMEOUT);
+
+		elevatorMotor.configNominalOutputForward(0, TIMEOUT);
+		elevatorMotor.configNominalOutputReverse(0, TIMEOUT);
+		elevatorMotor.configPeakOutputForward(1, TIMEOUT);
+		elevatorMotor.configPeakOutputReverse(-1, TIMEOUT);
+
+		elevatorMotor.selectProfileSlot(0, 0);
+
+		elevatorMotor.config_kP(0, KP, TIMEOUT);
+		elevatorMotor.config_kI(0, KI, TIMEOUT);
+		elevatorMotor.config_kD(0, KD, TIMEOUT);
+		elevatorMotor.config_kF(0, KF, TIMEOUT);
+
+		elevatorMotor.configMotionCruiseVelocity(CRUISE_SPEED, TIMEOUT);
+		elevatorMotor.configMotionAcceleration(ACCELERATION, TIMEOUT);
+
+		zeroed = false;
+
+		elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10);
+		elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10);
+
+		elevatorMotor.configForwardSoftLimitEnable(true, 10);
+		elevatorMotor.configReverseSoftLimitEnable(false, 10);
+
+		/* pass false to FORCE OFF the feature.  Otherwise the enable flags above are honored */
+		elevatorMotor.overrideLimitSwitchesEnable(true);
+
+
+	}
+
+	public void zeroEncoder() {
+		elevatorMotor.setSelectedSensorPosition(0, 0, TIMEOUT); // TODO: figure out later
+
+		/* +14 rotations forward when using CTRE Mag encoder */
+		elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); // TODO: FIX
+		/* -15 rotations reverse when using CTRE Mag encoder */
+		elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); // TODO: FIX
+
+		elevatorMotor.configForwardSoftLimitEnable(true, 10);
+		elevatorMotor.configReverseSoftLimitEnable(true, 10);
+
+		/* pass false to FORCE OFF the feature.  Otherwise the enable flags above are honored */
+		elevatorMotor.overrideLimitSwitchesEnable(true);
+
+		zeroed = true;
+	}
+
+	public void nudge(double distance) {
+		if (isMotionControlled && zeroed) {
+			setTarget(getCurrentPosition() + distance);
+		}
+	}
+
+	public void enableControl() {
+		if (zeroed) {
+			isMotionControlled = true;
+			elevatorMotor.set(ControlMode.MotionMagic, elevatorMotor.getSelectedSensorPosition(0));
+		}
+	}
+
+	public void disableControl() {
+		isMotionControlled = false;
+		elevatorMotor.set(ControlMode.Disabled, 0);
+	}
+
+	public boolean isZeroed() {
+		return zeroed;
+	}
+
+	public boolean isZeroing() {
+		return zeroing;
+	}
+
+	public boolean isMotionControlled() {
+		return isMotionControlled;
+	}
+
+	/**
+	 * Gets the current position of the elevator in inches.
+	 */
+	public double getCurrentPosition() {
+		return elevatorMotor.getSelectedSensorPosition(0) / INCHES_TO_NU;
+	}
+
+	/**
+	 * Gets the current position of the elevator in native units
+	 */
+	public double getCurrentPositionNU() {
+		return elevatorMotor.getSelectedSensorPosition(0);
+	}
+
+	/**
+	 * Sets the target position from the zero point.
+	 *
+	 * @param inches the position is in inches, duh
+	 */
+	public void setTarget(double inches) {
+//		System.out.println("Target: " + inches);
+
+		if (zeroed) {
+			elevatorMotor.set(
+				ControlMode.MotionMagic,
+				Math.min(MAXIMUM_HEIGHT,
+					Math.max(MINUMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */
+			);
+		}
+	}
+
+	public void setPower(double percent) {
+		percent = Math.min(.75, Math.max(-.75, percent)); // throttle power in
+
+		power = percent;
+		SmartDashboard.putNumber("Elevator Power", percent);
+
+//		System.out.println("Setting Power "  + percent);
+		elevatorMotor.set(ControlMode.PercentOutput, percent);
+	}
+
+	public boolean atTopPosition() {
+		return getCurrentPositionNU() >= MAXIMUM_HEIGHT;
+	}
+
+	public boolean atLowerPosition() {
+		return getCurrentPositionNU() <= MINUMUM_HEIGHT;
+	}
+
+	public void startZero() {
+		if (!zeroed) {
+			zeroing = true;
+			zeroed = false;
+			elevatorMotor.configReverseSoftLimitEnable(false, 10);
 //            zeroEncoder();
-            timer.start();
-        }
-    }
+			disableControl();
+
+//			System.out.println("Heloo ");
+			timer.start();
+		}
+	}
 }