diff --git a/config/Subsystems.yaml b/config/Subsystems.yaml
index 1f70a0a0..724e9785 100644
--- a/config/Subsystems.yaml
+++ b/config/Subsystems.yaml
@@ -111,35 +111,26 @@ lift:
   homingPower: -0.2
 pivot:
   pivotMotor: { type: VICTOR_SPX, config: { id: 5, inverted: false } }
-  pivotEncoder: {
-    channel: 0, fullRange: 360.0, minValue: -180, continuous: true,
-    averageBits: 4, oversampleBits: 4,
-    offset: 104
-  }
+  pivotEncoder: { aChannel: 6, bChannel: 7, reverse: false,
+    distancePerPulse: 0.087890625 } # 360 degrees / 4096 CPR
   pivotLockSolenoid: { channel: 1 }
   pivotLockSensor: { channel: 16, type: PNP }
 
   pivotController:
-#    kpP: 0.04 # 0.04
-#    kiP: 0.0 # 0.04
-#    kpV: 0.001
-#    kfV: 0.002
-#    kMinOutput: 0.0537
-#    kfA: 0.0005
-#    kHoldPower: 0.0
-#    minPosition: -90.0
-#    maxPosition: 90.0
-#    maxOutput: 0.4
-#    minOutput: -0.4
-#    maxAcceleration: 180.0
-#    travelVelocity: 90.0
-#    endVelocity: 0.0
-    target: { within: 3.0 }
-    minSetpoint: -90.0
-    maxSetpoint: 90.0
-    maxAcceleration: 1.2
-    movePower: 0.4
-    endRamp: 20.0
+    kpP: 0.04 # 0.04
+    kiP: 0.0 # 0.04
+    kpV: 0.001
+    kfV: 0.002
+    kMinOutput: 0.0537
+    kfA: 0.0005
+    kHoldPower: 0.0
+    minPosition: -90.0
+    maxPosition: 90.0
+    maxOutput: 0.4
+    minOutput: -0.4
+    maxAcceleration: 180.0
+    travelVelocity: 90.0
+    endVelocity: 0.0
 #    logData: true
   pivotHoldPower: 0.15
   anglePresets:
diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java
index ed39bd55..185e1bab 100644
--- a/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java
+++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java
@@ -1,5 +1,7 @@
 package org.teamtators.levitator.subsystems;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.Solenoid;
@@ -28,9 +30,9 @@
 
 public class Climber extends Subsystem implements Configurable<Climber.Config> {
     private final TatorRobot robot;
-    private SpeedControllerGroup climberMotor;
+    private SpeedControllerGroup climberMotors;
     private WPI_TalonSRX masterMotor;
-//    private MotorPowerUpdater climberMotorUpdater;
+    //    private MotorPowerUpdater climberMotorUpdater;
     private DigitalSensor topLimit;
     private DigitalSensor bottomLimit;
     private Solenoid releaser;
@@ -42,14 +44,14 @@ public Climber(TatorRobot robot) {
         this.robot = robot;
     }
 
-    public void setHomed(boolean homed) {
-        this.homed = homed;
-    }
-
     public boolean isHomed() {
         return homed;
     }
 
+    public void setHomed(boolean homed) {
+        this.homed = homed;
+    }
+
     public void setPower(double power, boolean force) {
         double pow = power;
         if (!force) {
@@ -62,7 +64,15 @@ public void setPower(double power, boolean force) {
                 pow = 0;
             }
         }
-        climberMotor.set(pow);
+        climberMotors.set(pow);
+    }
+
+    public void setNeutralMode(NeutralMode neutralMode) {
+        for (SpeedController climberMotor : climberMotors.getSpeedControllers()) {
+            if (climberMotor instanceof BaseMotorController) {
+                ((BaseMotorController) climberMotor).setNeutralMode(neutralMode);
+            }
+        }
     }
 
     public double getPosition() {
@@ -101,22 +111,32 @@ public void onEnterRobotState(RobotState state) {
                 robot.getScheduler().startCommand(homeCommand);
             }
         }
+        switch (state) {
+            case DISABLED:
+                setNeutralMode(NeutralMode.Coast);
+                break;
+            default:
+                setNeutralMode(NeutralMode.Brake);
+                break;
+        }
     }
 
     @Override
     public void configure(Config config) {
         this.config = config;
-        climberMotor = config.climberMotor.create();
-        masterMotor = (WPI_TalonSRX) climberMotor.getSpeedControllers()[0];
+        climberMotors = config.climberMotor.create();
+        masterMotor = (WPI_TalonSRX) climberMotors.getSpeedControllers()[0];
         topLimit = config.topLimit.create();
         bottomLimit = config.bottomLimit.create();
         releaser = config.releaser.create();
-//        climberMotorUpdater = new MotorPowerUpdater(climberMotor);
+//        climberMotorUpdater = new MotorPowerUpdater(climberMotors);
+
+        setNeutralMode(NeutralMode.Coast);
 
-        climberMotor.setName("Climber", "climberMotor");
-        for (int i = 0; i < climberMotor.getSpeedControllers().length; i++) {
-            SpeedController speedController = climberMotor.getSpeedControllers()[i];
-            ((Sendable) speedController).setName("Climber", ("climberMotor(" + i + ")"));
+        climberMotors.setName("Climber", "climberMotors");
+        for (int i = 0; i < climberMotors.getSpeedControllers().length; i++) {
+            SpeedController speedController = climberMotors.getSpeedControllers()[i];
+            ((Sendable) speedController).setName("Climber", ("climberMotors(" + i + ")"));
         }
         topLimit.setName("Climber", "topLimit");
         bottomLimit.setName("Climber", "bottomLimit");
@@ -127,7 +147,7 @@ public void configure(Config config) {
 
     @Override
     public void deconfigure() {
-        SpeedControllerConfig.free(climberMotor);
+        SpeedControllerConfig.free(climberMotors);
         topLimit.free();
         bottomLimit.free();
         releaser.free();
@@ -137,10 +157,10 @@ public void deconfigure() {
     public ManualTestGroup createManualTests() {
         ManualTestGroup group = super.createManualTests();
         group.addTest(new ClimberEncoderTest());
-        group.addTest(new SpeedControllerTest("climberMotor", climberMotor));
-        for (int i = 0; i < climberMotor.getSpeedControllers().length; i++) {
-            SpeedController speedController = climberMotor.getSpeedControllers()[i];
-            group.addTest(new SpeedControllerTest("climberMotor(" + i + ")", speedController));
+        group.addTest(new SpeedControllerTest("climberMotors", climberMotors));
+        for (int i = 0; i < climberMotors.getSpeedControllers().length; i++) {
+            SpeedController speedController = climberMotors.getSpeedControllers()[i];
+            group.addTest(new SpeedControllerTest("climberMotors(" + i + ")", speedController));
         }
         group.addTest(new DigitalSensorTest("topLimit", topLimit));
         group.addTest(new DigitalSensorTest("bottomLimit", bottomLimit));
diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java
index 9b75d9a6..25cf174a 100644
--- a/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java
+++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java
@@ -1,5 +1,7 @@
 package org.teamtators.levitator.subsystems;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
 import edu.wpi.first.wpilibj.*;
 import org.teamtators.common.config.Configurable;
 import org.teamtators.common.config.helpers.EncoderConfig;
@@ -204,6 +206,16 @@ public void setLeftPower(double power) {
         leftMotor.set(power);
     }
 
+    public void setNeutralMode(NeutralMode neutralMode) {
+        for (SpeedControllerGroup group : Arrays.asList(leftMotor, rightMotor)) {
+            for (SpeedController motor : group.getSpeedControllers()) {
+                if (motor instanceof BaseMotorController) {
+                    ((BaseMotorController) motor).setNeutralMode(neutralMode);
+                }
+            }
+        }
+    }
+
     public double getLeftRate() {
         return leftEncoder.getRate();
     }
@@ -353,6 +365,7 @@ public void configure(Config config) {
         rightEncoder.setName("Drive", "rightEncoder");
         gyro.setName("Drive", "gyro");
 
+        setNeutralMode(NeutralMode.Coast);
         poseEstimator.start();
     }
 
diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java
index 0e0a1afb..6910add7 100644
--- a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java
+++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java
@@ -1,6 +1,9 @@
 package org.teamtators.levitator.subsystems;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.teamtators.common.config.Configurable;
 import org.teamtators.common.config.helpers.*;
@@ -20,6 +23,7 @@
 import java.util.Map;
 
 public class Lift extends Subsystem implements Configurable<Lift.Config> {
+    private static final boolean ENABLE_HOMING = false;
     private Pivot pivot;
 
     private SpeedControllerGroup liftMotor;
@@ -69,6 +73,14 @@ public void linkTo(Pivot pivot) {
         this.setPivot(pivot);
     }
 
+    public void setNeutralMode(NeutralMode neutralMode) {
+        for (SpeedController liftMotor : liftMotor.getSpeedControllers()) {
+            if (liftMotor instanceof BaseMotorController) {
+                ((BaseMotorController) liftMotor).setNeutralMode(neutralMode);
+            }
+        }
+    }
+
     /**
      * @return height in inches
      */
@@ -347,6 +359,7 @@ public void configure(Config config) {
 
         homed = false;
         homeTimer.stop();
+        setNeutralMode(NeutralMode.Coast);
     }
 
     @Override
@@ -409,10 +422,11 @@ private void updateHeight() {
                 return;
             }
             setLiftPower(config.homingPower);
-            if (!isAtBottomLimit() && false && !homeTimeout) {
+            if (!isAtBottomLimit() && ENABLE_HOMING && !homeTimeout) {
                 return;
             }
             logger.info("Lift homed");
+            setNeutralMode(NeutralMode.Brake);
             liftEncoder.reset();
             enableLiftController();
             homed = true;
diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java
index ab193e69..7b875736 100644
--- a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java
+++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java
@@ -1,5 +1,6 @@
 package org.teamtators.levitator.subsystems;
 
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.SpeedController;
@@ -26,14 +27,14 @@ public class Pivot extends Subsystem implements Configurable<Pivot.Config> {
     private SpeedController pivotMotor;
     private MotorPowerUpdater pivotMotorUpdater;
     private AbstractUpdatable pivotUpdatable;
-    private AnalogPotentiometer pivotEncoder;
+    private Encoder pivotEncoder;
     private Solenoid pivotLockSolenoid;
     private DigitalSensor pivotLockSensor;
 
-    private /*TrapezoidalProfileFollower*/ StupidController pivotController;
-    private InputDerivative pivotVelocity;
+    private TrapezoidalProfileFollower pivotController;
     private BooleanSampler locked = new BooleanSampler(this::isPivotLockedRaw);
 
+    private boolean homed = false;
     private double desiredPivotAngle;
     private double targetAngle;
     private double lastAttemptedAngle;
@@ -47,13 +48,11 @@ public class Pivot extends Subsystem implements Configurable<Pivot.Config> {
     public Pivot() {
         super("Pivot");
 
-        pivotVelocity = new InputDerivative("pivotAngleDerivative", this::getCurrentPivotAngle);
-        pivotController = new /*TrapezoidalProfileFollower*/StupidController("pivotController");
-//        pivotController.setPositionProvider(this::getCurrentPivotAngle);
-//        pivotController.setVelocityProvider(pivotVelocity);
-        pivotController.setInputProvider(this::getCurrentPivotAngle);
+        pivotController = new TrapezoidalProfileFollower("pivotController");
+        pivotController.setPositionProvider(this::getCurrentPivotAngle);
+        pivotController.setVelocityProvider(this::getCurrentPivotVelocity);
         pivotController.setOutputConsumer(this::setPivotPower);
-//        pivotController.setOnTargetPredicate(ControllerPredicates.alwaysFalse());
+        pivotController.setOnTargetPredicate(ControllerPredicates.alwaysFalse());
 
         pivotUpdatable = new PivotUpdatable();
     }
@@ -66,8 +65,20 @@ public void setLift(Lift lift) {
         this.lift = lift;
     }
 
+    public boolean isHomed() {
+        return homed;
+    }
+
     public double getCurrentPivotAngle() {
-        return pivotEncoder.get();
+        return pivotEncoder.getDistance();
+    }
+
+    private void resetPivotAngle() {
+        pivotEncoder.reset();
+    }
+
+    public double getCurrentPivotVelocity() {
+        return pivotEncoder.getRate();
     }
 
     public double getDesiredPivotAngle() {
@@ -75,7 +86,6 @@ public double getDesiredPivotAngle() {
     }
 
     public void setDesiredPivotAngle(double desiredAngle, boolean force) {
-//        if (getSafePivotAngle(desiredAngle) == desiredAngle) {
         if (rotationForced && !force) {
             return;
         }
@@ -86,9 +96,6 @@ public void setDesiredPivotAngle(double desiredAngle, boolean force) {
             this.desiredPivotAngle = desiredAngle;
             this.rotationForced = force;
         }
-//        } else {
-//            logger.warn("Rotation to desired angle {} is not allowed at the current height {}!!", desiredAngle, getCurrentHeight());
-//        }
     }
 
     public void setDesiredAnglePreset(AnglePreset desiredPivotAngle) {
@@ -122,7 +129,7 @@ public void setTargetAngle(double angle) {
         logger.debug(String.format("Setting lift target angle to %.3f (degrees to move: %.3f)",
                 angle, distance));
         targetAngle = angle;
-        pivotController./*moveToPosition*/setSetpoint(angle);
+        pivotController.moveToPosition(angle);
         pivotController.setHoldPower(Math.signum(angle) * config.pivotHoldPower);
     }
 
@@ -165,7 +172,7 @@ public Updatable getPivotController() {
     }
 
     public List<Updatable> getUpdatables() {
-        return Arrays.asList(pivotVelocity, pivotUpdatable, pivotController);
+        return Arrays.asList(pivotUpdatable, pivotController);
     }
 
     public boolean isPivotLockedRaw() {
@@ -244,11 +251,11 @@ public void onEnterRobotState(RobotState state) {
     public ManualTestGroup createManualTests() {
         ManualTestGroup tests = super.createManualTests();
         tests.addTest(new SpeedControllerTest("pivotMotor", pivotMotor));
-        tests.addTest(new AnalogPotentiometerTest("pivotEncoder", pivotEncoder));
+        tests.addTest(new EncoderTest("pivotEncoder", pivotEncoder));
         tests.addTest(new SolenoidTest("pivotLockSolenoid", pivotLockSolenoid));
         tests.addTest(new DigitalSensorTest("pivotLockSensor", pivotLockSensor));
 
-        tests.addTest(new /*MotionCalibrationTest*/ControllerTest(pivotController, 90.0));
+        tests.addTest(new MotionCalibrationTest(pivotController));
 
         tests.addTest(new PivotTest());
         return tests;
@@ -274,6 +281,8 @@ public void configure(Config config) {
         pivotLockSensor.setName("Pivot", "pivotLockSensor");
 
         pivotMotorUpdater = new MotorPowerUpdater(pivotMotor);
+
+        homed = false;
     }
 
     @Override
@@ -289,11 +298,11 @@ public void deconfigure() {
     @SuppressWarnings("WeakerAccess")
     public static class Config {
         public SpeedControllerConfig pivotMotor;
-        public AnalogPotentiometerConfig pivotEncoder;
+        public EncoderConfig pivotEncoder;
         public SolenoidConfig pivotLockSolenoid;
         public DigitalSensorConfig pivotLockSensor;
 
-        public /*TrapezoidalProfileFollower*/ StupidController.Config pivotController;
+        public TrapezoidalProfileFollower.Config pivotController;
         public double pivotHoldPower;
 
         public Map<AnglePreset, Double> anglePresets;
@@ -336,10 +345,10 @@ public void start() {
         public void onButtonDown(LogitechF310.Button button) {
             switch (button) {
                 case B:
-                    double angle = (config.pivotController./*maxPosition*/maxSetpoint - config.pivotController./*minPosition*/minSetpoint)
-                            * ((axisValue + 1) / 2) + config.pivotController./*minPosition*/minSetpoint;
+                    double angle = (config.pivotController.maxPosition - config.pivotController.minPosition)
+                            * ((axisValue + 1) / 2) + config.pivotController.minPosition;
                     logger.info("Moving pivot to angle {}", angle);
-                    pivotController./*moveToPosition*/setSetpoint(angle);
+                    pivotController.moveToPosition(angle);
                     break;
                 case Y:
                     enable();
@@ -381,6 +390,19 @@ public synchronized void start() {
         @Override
         public void doUpdate(double delta) {
             Pivot pivot = Pivot.this;
+            if (!pivot.isHomed()) {
+                pivot.disablePivotController();
+                pivot.setPivotPower(0.0);
+                pivot.setPivotLockSolenoid(true);
+                if (pivot.isPivotLocked()) {
+                    pivot.resetPivotAngle();
+                    logger.info("Pivot homed");
+                    pivot.setDesiredAnglePreset(AnglePreset.CENTER);
+                    pivot.homed = true;
+                } else {
+                    return;
+                }
+            }
             double centerAngle = pivot.getAnglePreset(AnglePreset.CENTER);
             double currentAngle = pivot.getCurrentPivotAngle();
             double safePivotAngle = pivot.getSafePivotAngle(desiredPivotAngle);