diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f81d..d097ec8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,7 +337,8 @@
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
-  dt_spline_.Update(output != NULL && controller_type == 2);
+  dt_spline_.Update(output != NULL && controller_type == 2,
+                    xytheta_state_.block<5, 1>(0, 0));
 
   switch (controller_type) {
     case 0:
@@ -389,6 +390,7 @@
 
     dt_openloop_.PopulateStatus(status);
     dt_closedloop_.PopulateStatus(status);
+    dt_spline_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 88547d5..0f57bf9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -111,6 +111,15 @@
     return angular;
   }
 
+  Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
+    return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
+            -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+  }
+
+  Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
+    return Tlr_to_la().inverse();
+  }
+
   // Converts the linear and angular position, velocity to the top 4 states of
   // the robot state.
   Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 17da3ab..6c16667 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,6 +13,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/queues/gyro.q.h"
 #include "y2016/constants.h"
@@ -108,7 +109,15 @@
                              ".frc971.control_loops.drivetrain_queue.position",
                              ".frc971.control_loops.drivetrain_queue.output",
                              ".frc971.control_loops.drivetrain_queue.status"),
-        gyro_reading_(::frc971::sensors::gyro_reading.name()) {
+        gyro_reading_(::frc971::sensors::gyro_reading.name()),
+        velocity_drivetrain_(::std::unique_ptr<StateFeedbackLoop<
+            2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+            HybridKalman<2, 2, 2>>>(
+            new StateFeedbackLoop<2, 2, 2, double,
+                                  StateFeedbackHybridPlant<2, 2, 2>,
+                                  HybridKalman<2, 2, 2>>(
+                GetDrivetrainConfig()
+                    .make_hybrid_drivetrain_velocity_loop()))) {
     Reinitialize();
     last_U_.setZero();
   }
@@ -188,6 +197,16 @@
     U(0, 0) += drivetrain_plant_->left_voltage_offset();
     U(1, 0) += drivetrain_plant_->right_voltage_offset();
     drivetrain_plant_->Update(U);
+    double dt_float =
+        ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+            GetDrivetrainConfig().dt).count();
+    state = RungeKuttaU(
+        [this](const ::Eigen::Matrix<double, 5, 1> &X,
+               const ::Eigen::Matrix<double, 2, 1> &U) {
+          return ContinuousDynamics(velocity_drivetrain_->plant(),
+                                    GetDrivetrainConfig().Tlr_to_la(), X, U);
+        },
+        state, U, dt_float);
   }
 
   void set_left_voltage_offset(double left_voltage_offset) {
@@ -199,10 +218,18 @@
 
   ::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
 
+  ::Eigen::Matrix<double, 2, 1> GetPosition() const {
+    return state.block<2,1>(0,0);
+  }
+
  private:
   ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
   ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
 
+  ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+  ::std::unique_ptr<
+      StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                        HybridKalman<2, 2, 2>>> velocity_drivetrain_;
   double last_left_position_;
   double last_right_position_;
 
@@ -259,6 +286,12 @@
                 drivetrain_motor_plant_.GetRightPosition(), 1e-3);
   }
 
+  void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+    auto actual = drivetrain_motor_plant_.GetPosition();
+    EXPECT_NEAR(actual(0), expected(0), 1e-2);
+    EXPECT_NEAR(actual(1), expected(1), 1e-2);
+  }
+
   virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
 };
 
@@ -526,6 +559,135 @@
   VerifyNearGoal();
 }
 
+// Tests that simple spline converges on a goal.
+TEST_F(DrivetrainTest, SplineSimple) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it doesn't start where it thinks.
+TEST_F(DrivetrainTest, SplineOffset) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.5, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it starts to the side of where it
+// thinks.
+TEST_F(DrivetrainTest, SplineSideOffset) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that a multispline converges on a goal.
+TEST_F(DrivetrainTest, MultiSpline) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 2;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(4000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
+// Tests that several splines converges on a goal.
+TEST_F(DrivetrainTest, SequentialSplines) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  second_spline_goal->controller_type = 2;
+  second_spline_goal->spline.spline_idx = 2;
+  second_spline_goal->spline.spline_count = 1;
+  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  second_spline_goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  second_start_goal->controller_type = 2;
+  second_start_goal->spline_handle = 2;
+  second_start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
 ::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
                                              double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0d6b6ae..b45f218 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,5 +1,7 @@
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
+#include <iostream>
+
 #include "Eigen/Dense"
 
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -12,7 +14,17 @@
 namespace drivetrain {
 
 SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
-    : dt_config_(dt_config) {}
+    : dt_config_(dt_config),
+      current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+
+void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+  bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
+                       ::std::abs((*U)(1, 0)) > 12.0;
+
+  if (output_was_capped) {
+    *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
+  }
+}
 
 // TODO(alex): put in another thread to avoid malloc in RT.
 void SplineDrivetrain::SetGoal(
@@ -65,17 +77,36 @@
     current_trajectory_->Plan();
     current_xva_ = current_trajectory_->FFAcceleration(0);
     current_xva_(1) = 0.0;
+    current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
   }
 }
 
-void SplineDrivetrain::Update(bool enable) {
-  if (enable && current_trajectory_ &&
-      !current_trajectory_->is_at_end(current_state_)) {
+// TODO(alex): Hold position when done following the spline.
+// TODO(Austin): Compensate for voltage error.
+void SplineDrivetrain::Update(bool enable,
+                              const ::Eigen::Matrix<double, 5, 1> &state) {
+  enable_ = enable;
+  if (enable && current_trajectory_) {
+    ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+    if (!current_trajectory_->is_at_end(current_state_)) {
+      // TODO(alex): It takes about a cycle for the outputs to propagate to the
+      // motors. Consider delaying the output by a cycle.
+      U_ff = current_trajectory_->FFVoltage(current_xva_(0));
+    }
+    ::Eigen::Matrix<double, 2, 5> K =
+        current_trajectory_->KForState(state, dt_config_.dt, Q, R);
+    ::Eigen::Matrix<double, 5, 1> goal_state =
+        current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+    ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+    next_U_ = U_ff + U_fb;
+    uncapped_U_ = next_U_;
+    ScaleCapU(&next_U_);
+
     next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &current_state_);
   }
 }
 
-// TODO(alex): Handle drift.
 void SplineDrivetrain::SetOutput(
     ::frc971::control_loops::DrivetrainQueue::Output *output) {
   if (!output) {
@@ -86,16 +117,22 @@
   }
   if (current_spline_handle_ == current_spline_idx_) {
     if (!current_trajectory_->is_at_end(current_state_)) {
-      double current_distance = current_xva_(0);
-      ::Eigen::Matrix<double, 2, 1> FFVoltage =
-          current_trajectory_->FFVoltage(current_distance);
-      output->left_voltage = FFVoltage(0);
-      output->right_voltage = FFVoltage(1);
+      output->left_voltage = next_U_(0);
+      output->right_voltage = next_U_(1);
       current_xva_ = next_xva_;
     }
   }
 }
 
+void SplineDrivetrain::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+  if (status && enable_) {
+    status->uncapped_left_voltage = uncapped_U_(0);
+    status->uncapped_right_voltage = uncapped_U_(1);
+    status->robot_speed = current_xva_(1);
+  }
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 4bb7ac9..a065167 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -19,7 +19,7 @@
 
   void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
-  void Update(bool enabled);
+  void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
 
   void SetOutput(
       ::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -27,6 +27,8 @@
   void PopulateStatus(
       ::frc971::control_loops::DrivetrainQueue::Status *status) const;
  private:
+  void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
+
   const DrivetrainConfig<double> dt_config_;
 
   uint32_t current_spline_handle_; // Current spline told to excecute.
@@ -35,6 +37,25 @@
   ::std::unique_ptr<Trajectory> current_trajectory_;
   ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
   ::Eigen::Matrix<double, 2, 1> current_state_;
+  ::Eigen::Matrix<double, 2, 1> next_U_;
+  ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+
+  bool enable_;
+
+  // TODO(alex): pull this out of dt_config.
+  const ::Eigen::DiagonalMatrix<double, 5> Q =
+      (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+           << 1.0 / ::std::pow(0.05, 2),
+       1.0 / ::std::pow(0.05, 2), 1.0 / ::std::pow(0.2, 2),
+       1.0 / ::std::pow(0.5, 2), 1.0 / ::std::pow(0.5, 2))
+          .finished()
+          .asDiagonal();
+  const ::Eigen::DiagonalMatrix<double, 2> R =
+      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+           << 1.0 / ::std::pow(12.0, 2),
+       1.0 / ::std::pow(12.0, 2))
+          .finished()
+          .asDiagonal();
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 26b2cb2..e59286f 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -26,9 +26,8 @@
     return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
 
 
-def mToPx(i):
-    return (i * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
-
+def mToPx(m):
+    return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
 
 def px(cr):
     return OverrideMatrix(cr, identity)
@@ -229,7 +228,138 @@
                                     (points[index - 1][1] - point[1])**2)
         return distance
 
-    # Handle the expose-event by updating the Window and drawing
+    def draw_HAB(self, cr):
+        print("WENT IN")
+        # BASE Constants
+        X_BASE = -450+mToPx(2.41568)
+        Y_BASE  = -150+mToPx(4.129151)
+
+        BACKWALL_X = -450
+
+        # HAB Levels 2 and 3 called in variables backhab
+
+        WIDTH_BACKHAB = mToPx(1.2192)
+
+        Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+        BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+        BACKHAB_LV3_LENGTH = mToPx(1.2192)
+        Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+        Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
+
+        # HAB LEVEL 1
+        X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+        WIDTH_LV1_BOX = mToPx(0.90805)
+        LENGTH_LV1_BOX = mToPx(1.6256)
+
+        Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+        # Ramp off Level 1
+        X_RAMP = X_LV1_BOX
+
+        Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+        WIDTH_TOP_RAMP = mToPx(1.20015)
+        LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+        X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+        Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+        LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
+        WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+        Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+        # Side Bars to Hold in balls
+        X_BARS = BACKWALL_X
+        WIDTH_BARS = WIDTH_BACKHAB
+        LENGTH_BARS = mToPx(0.574675)
+
+        Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+        Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+        set_color(cr, palette["BLACK"])
+        cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+                BACKHAB_LV2_LENGTH)
+        cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
+                BACKHAB_LV3_LENGTH)
+        cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+                BACKHAB_LV2_LENGTH)
+        cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+        cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
+                LENGTH_LV1_BOX)
+        cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+        cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+                LENGTH_MIDDLE_RAMP)
+        cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+        cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+        cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+        cr.stroke()
+        #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
+        #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
+        cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+        cr.stroke()
+
+    def draw_rockets(self, cr):
+        # BASE Constants
+        X_BASE = -450+mToPx(2.41568)
+        Y_BASE  = -150+mToPx(4.129151)
+
+        # Top Rocket
+
+        # Leftmost Line
+        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+        cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+        # Top Line
+        cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+        cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+        #Rightmost Line
+        cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+        #Back Line
+        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+        # Bottom Rocket
+
+        # Leftmost Line
+        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+        cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+        # Top Line
+        cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+        cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+        #Rightmost Line
+        cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+        #Back Line
+        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+        cr.stroke()
+
+    def draw_cargo_ship(self, cr):
+        # BASE Constants
+        X_BASE = -450+mToPx(2.41568)
+        Y_BASE  = -150+mToPx(4.129151)
+
+        cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+                mToPx(2.43205), mToPx(1.41605))
+
+        cr.stroke()
+
+    def draw_field_elements(self, cr):
+        self.draw_HAB(cr)
+        self.draw_rockets(cr)
+        self.draw_cargo_ship(cr)
+
     def handle_draw(self, cr):
         # print(self.new_point)
         # print("SELF.POINT_SELECTED: " + str(self.point_selected))
@@ -248,7 +378,7 @@
                      self.extents_y_max - self.extents_y_min)
         cr.fill()
 
-        #Drawing the switch and scale in the field
+        #Drawing the field
         cr.move_to(0, 50)
         cr.show_text('Press "e" to export')
         cr.show_text('Press "i" to import')
@@ -261,6 +391,8 @@
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
 
+        self.draw_field_elements(cr)
+
         y = 0
 
         # update all the things
diff --git a/tools/bazel b/tools/bazel
index dd5d9a5..a691066 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -26,7 +26,7 @@
 
 readonly VERSION="0.19.0rc4-201810201638+ac88041"
 
-readonly DOWNLOAD_DIR="$(dirname "${BASH_SOURCE[0]}")/../bazel-downloads"
+readonly DOWNLOAD_DIR="${HOME}/.cache/bazel"
 # Directory to unpack bazel into.  This must change whenever bazel changes.
 readonly VERSION_DIR="${DOWNLOAD_DIR}/${VERSION}-v1"
 readonly VERSION_BAZEL="${VERSION_DIR}/usr/bin/bazel"
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 93e5d09..4688378 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -6,7 +6,8 @@
 
 # Include --config=eigen to enable Eigen assertions so that we catch potential
 # bugs with Eigen.
-bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
-bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
-bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --curses=no --color=no //motors/... //y2019/jevois/... --cpu=cortex-m4f
+bazel test -c opt --stamp=no --config=eigen --curses=no --color=no ${TARGETS}
+bazel build -c opt --stamp=no --curses=no --color=no ${TARGETS} --cpu=roborio
+bazel build --stamp=no --curses=no --color=no ${TARGETS} --cpu=armhf-debian
+bazel build -c opt --stamp=no --curses=no --color=no \
+    //motors/... //y2019/jevois/... --cpu=cortex-m4f
diff --git a/y2019/BUILD b/y2019/BUILD
index ab4a842..14d0d6f 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -24,7 +24,9 @@
         "//aos/mutex",
         "//aos/network:team_number",
         "//frc971:constants",
+        "//frc971/control_loops:pose",
         "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+        "//y2019/control_loops/drivetrain:camera",
         "//y2019/control_loops/drivetrain:polydrivetrain_plants",
         "//y2019/control_loops/superstructure/elevator:elevator_plants",
         "//y2019/control_loops/superstructure/intake:intake_plants",
@@ -74,6 +76,30 @@
     ],
 )
 
+cc_library(
+    name = "joystick_angle",
+    srcs = [
+        "joystick_angle.cc",
+    ],
+    hdrs = [
+        "joystick_angle.h",
+    ],
+    deps = [
+        "//aos/input:drivetrain_input",
+    ],
+)
+
+cc_test(
+    name = "joystick_angle_test",
+    srcs = [
+        "joystick_angle_test.cc",
+    ],
+    deps = [
+        ":joystick_angle",
+        "//aos/testing:googletest",
+    ],
+)
+
 cc_binary(
     name = "joystick_reader",
     srcs = [
diff --git a/y2019/constants.cc b/y2019/constants.cc
index b53fb99..791a6c4 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -30,6 +30,16 @@
 const uint16_t kPracticeTeamNumber = 9971;
 const uint16_t kCodingRobotTeamNumber = 7971;
 
+constexpr double FeetToMeters(double ft) {
+  return 0.3048 * ft;
+}
+constexpr double InchToMeters(double in) {
+  return 0.0254 * in;
+}
+constexpr double DegToRad(double deg) {
+  return deg * M_PI / 180.0;
+}
+
 const Values *DoGetValuesForTeam(uint16_t team) {
   Values *const r = new Values();
   Values::PotAndAbsConstants *const elevator = &r->elevator;
@@ -99,7 +109,7 @@
   stilts_params->zeroing_voltage = 3.0;
   stilts_params->operating_voltage = 12.0;
   stilts_params->zeroing_profile_params = {0.1, 0.5};
-  stilts_params->default_profile_params = {0.5, 0.5};
+  stilts_params->default_profile_params = {0.1, 0.5};
   stilts_params->range = Values::kStiltsRange();
   stilts_params->make_integral_loop =
       &control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
@@ -210,5 +220,154 @@
   return *values[team_number];
 }
 
+constexpr size_t Field::kNumTargets;
+constexpr size_t Field::kNumObstacles;
+
+Field::Field() {
+  // TODO(james): These values need to re-verified. I got them by skimming the
+  // manual and they all seem to be pretty much correct.
+  //
+  // Note: Per //frc971/control_loops/pose.h, coordinate system is:
+  // -In meters
+  // -Origin at center of our driver's station wall
+  // -Positive X-axis pointing straight out from driver's station
+  // -Positive Y-axis pointing straight left from the driver's perspective
+  // -Positive Z-axis is straight up.
+  // -The angle of the target is such that the angle is the angle you would
+  //  need to be facing to see it straight-on. I.e., if the target angle is
+  //  pi / 2.0, then you would be able to see it face on by facing straight
+  //  left from the driver's point of view (if you were standing in the right
+  //  spot).
+  constexpr double kCenterFieldX = FeetToMeters(27.0) + InchToMeters(1.125);
+
+  constexpr double kFarSideCargoBayX =
+      kCenterFieldX - InchToMeters(20.875);
+  constexpr double kMidSideCargoBayX = kFarSideCargoBayX - InchToMeters(21.75);
+  constexpr double kNearSideCargoBayX = kMidSideCargoBayX - InchToMeters(21.75);
+  constexpr double kSideCargoBayY = InchToMeters(24 + 3 + 0.875);
+  constexpr double kSideCargoBayTheta = -M_PI_2;
+
+  constexpr double kFaceCargoBayX =
+      kCenterFieldX - InchToMeters(7 * 12 + 11.75 + 9);
+  constexpr double kFaceCargoBayY = InchToMeters(10.875);
+  constexpr double kFaceCargoBayTheta = 0.0;
+
+  constexpr double kRocketX = kCenterFieldX - FeetToMeters(8);
+  constexpr double kRocketY = InchToMeters((26 * 12 + 10.5) / 2.0);
+
+  constexpr double kRocketPortX = kRocketX;
+  constexpr double kRocketPortY = kRocketY - 0.70;
+  constexpr double kRocketPortTheta = M_PI_2;
+
+  // Half of portal + guess at width * cos(61.5 deg)
+  const double kRocketHatchXOffset = InchToMeters(14.634);
+  const double kRocketHatchY = kRocketPortY + InchToMeters(9.326);
+  const double kRocketNearX = kRocketX - kRocketHatchXOffset;
+  const double kRocketFarX = kRocketX + kRocketHatchXOffset;
+  constexpr double kRocketNearTheta = DegToRad(28.5);
+  constexpr double kRocketFarTheta = M_PI - kRocketNearTheta;
+
+  constexpr double kHpSlotY = InchToMeters((26 * 12 + 10.5) / 2.0 - 25.9);
+  constexpr double kHpSlotTheta = M_PI;
+
+  constexpr double kNormalZ = 0.80;
+  constexpr double kPortZ = 0.99;
+
+  const Pose far_side_cargo_bay({kFarSideCargoBayX, kSideCargoBayY, kNormalZ},
+                                kSideCargoBayTheta);
+  const Pose mid_side_cargo_bay({kMidSideCargoBayX, kSideCargoBayY, kNormalZ},
+                                kSideCargoBayTheta);
+  const Pose near_side_cargo_bay({kNearSideCargoBayX, kSideCargoBayY, kNormalZ},
+                                 kSideCargoBayTheta);
+
+  const Pose face_cargo_bay({kFaceCargoBayX, kFaceCargoBayY, kNormalZ},
+                            kFaceCargoBayTheta);
+
+  const Pose rocket_port({kRocketPortX, kRocketPortY, kPortZ},
+                         kRocketPortTheta);
+
+  const Pose rocket_near({kRocketNearX, kRocketHatchY, kNormalZ},
+                         kRocketNearTheta);
+  const Pose rocket_far({kRocketFarX, kRocketHatchY, kNormalZ},
+                        kRocketFarTheta);
+
+  const Pose hp_slot({0.0, kHpSlotY, kNormalZ}, kHpSlotTheta);
+
+  const ::std::array<Pose, 8> quarter_field_targets{
+      {far_side_cargo_bay, mid_side_cargo_bay, near_side_cargo_bay,
+       face_cargo_bay, rocket_port, rocket_near, rocket_far, hp_slot}};
+
+  // Mirror across center field mid-line (short field axis):
+  ::std::array<Pose, 16> half_field_targets;
+  ::std::copy(quarter_field_targets.begin(), quarter_field_targets.end(),
+              half_field_targets.begin());
+  for (int ii = 0; ii < 8; ++ii) {
+    const int jj = ii + 8;
+    half_field_targets[jj] = quarter_field_targets[ii];
+    half_field_targets[jj].mutable_pos()->x() =
+        2.0 * kCenterFieldX - half_field_targets[jj].rel_pos().x();
+    half_field_targets[jj].set_theta(
+        aos::math::NormalizeAngle(M_PI - half_field_targets[jj].rel_theta()));
+  }
+
+  ::std::array<Pose, 32> target_poses_;
+
+  // Mirror across x-axis (long field axis):
+  ::std::copy(half_field_targets.begin(), half_field_targets.end(),
+              target_poses_.begin());
+  for (int ii = 0; ii < 16; ++ii) {
+    const int jj = ii + 16;
+    target_poses_[jj] = half_field_targets[ii];
+    target_poses_[jj].mutable_pos()->y() *= -1;
+    target_poses_[jj].set_theta(-target_poses_[jj].rel_theta());
+  }
+  for (int ii = 0; ii < 32; ++ii) {
+    targets_[ii] = {target_poses_[ii]};
+  }
+
+  // Define rocket obstacles as just being a single line that should block any
+  // cameras trying to see through the rocket up and down the field.
+  // This line is parallel to the driver's station wall and extends behind
+  // the portal.
+  Obstacle rocket_obstacle({{kRocketPortX, kRocketY, 0.0}, 0.0},
+                           {{kRocketPortX, kRocketPortY + 0.01, 0.0}, 0.0});
+  // First, we mirror rocket obstacles across x-axis:
+  Obstacle rocket_obstacle2({{kRocketPortX, -kRocketY, 0.0}, 0.0},
+                            {{kRocketPortX, -kRocketPortY - 0.01, 0.0}, 0.0});
+
+  // Define an obstacle for the Hab that extends striaght out a few feet from
+  // the driver's station wall.
+  // TODO(james): Does this actually block our view?
+  const double kHabL3X = FeetToMeters(4.0);
+  Obstacle hab_obstacle({}, {{kHabL3X, 0.0, 0.0}, 0.0});
+  ::std::array<Obstacle, 3> half_obstacles{
+      {rocket_obstacle, rocket_obstacle2, hab_obstacle}};
+  ::std::copy(half_obstacles.begin(), half_obstacles.end(), obstacles_.begin());
+
+  // Next, we mirror across the mid-line (short axis) to duplicate the
+  // rockets and hab to opposite side of the field.
+  for (int ii = 0; ii < 3; ++ii) {
+    const int jj = ii + 3;
+    obstacles_[jj] = half_obstacles[ii];
+    obstacles_[jj].mutable_pose1()->mutable_pos()->x() =
+        2.0 * kCenterFieldX - obstacles_[jj].mutable_pose1()->rel_pos().x();
+    obstacles_[jj].mutable_pose2()->mutable_pos()->x() =
+        2.0 * kCenterFieldX - obstacles_[jj].mutable_pose2()->rel_pos().x();
+  }
+
+  // Finally, define a rectangular cargo ship.
+  const double kCargoCornerX = kFaceCargoBayX + 0.1;
+  const double kCargoCornerY = kSideCargoBayY - 0.1;
+  ::std::array<Pose, 4> cargo_corners{
+      {{{kCargoCornerX, kCargoCornerY, 0.0}, 0.0},
+       {{kCargoCornerX, -kCargoCornerY, 0.0}, 0.0},
+       {{2.0 * kCenterFieldX - kCargoCornerX, -kCargoCornerY, 0.0}, 0.0},
+       {{2.0 * kCenterFieldX - kCargoCornerX, kCargoCornerY, 0.0}, 0.0}}};
+  for (int ii = 6; ii < 10; ++ii) {
+    obstacles_[ii] = Obstacle(cargo_corners[ii % cargo_corners.size()],
+                              cargo_corners[(ii + 1) % cargo_corners.size()]);
+  }
+}
+
 }  // namespace constants
 }  // namespace y2019
diff --git a/y2019/constants.h b/y2019/constants.h
index 46d28f1..24d8b92 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -1,6 +1,7 @@
 #ifndef Y2019_CONSTANTS_H_
 #define Y2019_CONSTANTS_H_
 
+#include <array>
 #include <math.h>
 #include <stdint.h>
 
@@ -11,6 +12,8 @@
 #include "y2019/control_loops/superstructure/intake/intake_plant.h"
 #include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
 #include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+#include "y2019/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/pose.h"
 
 namespace y2019 {
 namespace constants {
@@ -25,6 +28,33 @@
 //
 // All ratios are from the encoder shaft to the output units.
 
+
+class Field {
+ public:
+  typedef ::frc971::control_loops::TypedPose<double> Pose;
+  typedef ::y2019::control_loops::TypedTarget<double> Target;
+  typedef ::frc971::control_loops::TypedLineSegment<double> Obstacle;
+
+  static constexpr size_t kNumTargets = 32;
+  static constexpr size_t kNumObstacles = 10;
+
+  Field();
+
+  ::std::array<Target, kNumTargets> targets() const { return targets_; }
+  ::std::array<Obstacle, kNumObstacles> obstacles() const { return obstacles_; }
+
+ private:
+  // All target locations are defined as being at the center of the target,
+  // except for the height, for which we use the top of the target.
+  ::std::array<Target, kNumTargets> targets_;
+  // Obstacle locations are approximate, as we are just trying to roughly
+  // approximate what will block our view when on the field.
+  // If anything, we should err on the side of making obstacles too small so
+  // that if there is any error in our position, we don't assume that it must
+  // be hidden behind a target when it really is not.
+  ::std::array<Obstacle, kNumObstacles> obstacles_;
+};
+
 struct Values {
   static const int kZeroingSampleSize = 200;
 
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 959e839..38c73f0 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -84,6 +84,7 @@
 cc_library(
     name = "camera",
     srcs = ["camera.h"],
+    visibility = ["//y2019:__pkg__"],
     deps = [
         "//aos/containers:sized_array",
         "//frc971/control_loops:pose",
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 5edfce3..187379b 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,16 +8,33 @@
 namespace control_loops {
 namespace superstructure {
 
-void suction_cups(
-    const SuperstructureQueue::Goal *unsafe_goal,
-    SuperstructureQueue::Output *output) {
-  const double on_voltage = 12.0;
+void Superstructure::HandleSuction(const SuctionGoal *unsafe_goal,
+                                   float suction_pressure,
+                                   SuperstructureQueue::Output *output,
+                                   bool *has_piece) {
+  constexpr double kPumpVoltage = 12.0;
+  constexpr double kPumpHasPieceVoltage = 8.0;
 
-  if(unsafe_goal && output) {
-    if(unsafe_goal->suction.top || unsafe_goal->suction.bottom) {
-      output->pump_voltage = on_voltage;
+  // TODO(austin): Low pass filter on pressure.
+  *has_piece = suction_pressure < 0.70;
+
+  if (unsafe_goal && output) {
+    const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
+    if (evacuate) {
+      vacuum_count_ = 200;
     }
+    // TODO(austin): High speed pump a bit longer after we detect we have the
+    // game piece.
+    // Once the vacuum evacuates, the pump speeds up because there is no
+    // resistance.  So, we want to turn it down to save the pump from
+    // overheating.
+    output->pump_voltage =
+        (vacuum_count_ > 0) ? (*has_piece ? kPumpHasPieceVoltage : kPumpVoltage)
+                            : 0.0;
+    output->intake_suction_top = unsafe_goal->top;
+    output->intake_suction_bottom = unsafe_goal->bottom;
   }
+  vacuum_count_ = ::std::max(0, vacuum_count_ - 1);
 }
 
 Superstructure::Superstructure(::aos::EventLoop *event_loop,
@@ -60,6 +77,9 @@
                   output != nullptr ? &(output->stilts_voltage) : nullptr,
                   &(status->stilts));
 
+  HandleSuction(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
+                position->suction_pressure, output, &(status->has_piece));
+
   status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
                    status->intake.zeroed && status->stilts.zeroed;
 
@@ -67,10 +87,8 @@
                      status->intake.estopped || status->stilts.estopped;
 
   if (output) {
-    if (status->intake.position > kMinIntakeAngleForRollers) {
-      output->intake_roller_voltage =
-          (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
-
+    if (unsafe_goal && status->intake.position > kMinIntakeAngleForRollers) {
+      output->intake_roller_voltage = unsafe_goal->roller_voltage;
     } else {
       output->intake_roller_voltage = 0.0;
     }
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index c3e53f2..9879e17 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -40,12 +40,16 @@
                             SuperstructureQueue::Status *status) override;
 
  private:
+  void HandleSuction(const SuctionGoal *unsafe_goal, float suction_pressure,
+                     SuperstructureQueue::Output *output, bool *has_piece);
+
   PotAndAbsoluteEncoderSubsystem elevator_;
   PotAndAbsoluteEncoderSubsystem wrist_;
   AbsoluteEncoderSubsystem intake_;
   PotAndAbsoluteEncoderSubsystem stilts_;
 
   CollisionAvoidance collision_avoidance_;
+  int vacuum_count_ = 0;
 
   static constexpr double kMinIntakeAngleForRollers = -0.7;
 
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index a71f81a..4176057 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -53,9 +53,9 @@
   };
 
   message Position {
-    // Input from pressure sensor in psi
-    // 0 = current atmospheric pressure, negative = suction.
-    double suction_pressure;
+    // Input from pressure sensor in bar
+    // 1 = 1 atm, 0 = full vacuum
+    float suction_pressure;
 
     // Position of the elevator, 0 at lowest position, positive when up.
     .frc971.PotAndAbsolutePosition elevator;
diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
new file mode 100644
index 0000000..1923c53
--- /dev/null
+++ b/y2019/joystick_angle.cc
@@ -0,0 +1,36 @@
+#include "y2019/joystick_angle.h"
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+                                  const JoystickAxis &y_axis,
+                                  const Data &data) {
+  return GetJoystickPosition(data.GetAxis(x_axis), data.GetAxis(y_axis));
+}
+
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis) {
+  if (x_axis > kJoystickRight) {
+    if (y_axis < kJoystickDown) {
+      return JoystickAngle::kUpperRight;
+    } else if (y_axis > kJoystickUp) {
+      return JoystickAngle::kLowerRight;
+    } else {
+      return JoystickAngle::kMiddleRight;
+    }
+  } else if (x_axis < kJoystickLeft) {
+    if (y_axis < kJoystickDown) {
+      return JoystickAngle::kUpperLeft;
+    } else if (y_axis > kJoystickUp) {
+      return JoystickAngle::kLowerLeft;
+    } else {
+      return JoystickAngle::kMiddleLeft;
+    }
+  }
+  return JoystickAngle::kDefault;
+}
+
+}  // namespace joysticks
+}  // namespace input
+}  // namespace y2019
diff --git a/y2019/joystick_angle.h b/y2019/joystick_angle.h
new file mode 100644
index 0000000..a2c7e36
--- /dev/null
+++ b/y2019/joystick_angle.h
@@ -0,0 +1,37 @@
+#ifndef Y2019_JOYSTICK_ANGLE_H_
+#define Y2019_JOYSTICK_ANGLE_H_
+
+#include "aos/input/driver_station_data.h"
+
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::Data;
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+namespace {
+constexpr double kJoystickLeft = -0.5;
+constexpr double kJoystickRight = 0.5;
+constexpr double kJoystickUp = 0.5;
+constexpr double kJoystickDown = -0.5;
+}
+
+enum class JoystickAngle {
+  kDefault,
+  kUpperRight,
+  kMiddleRight,
+  kLowerRight,
+  kUpperLeft,
+  kMiddleLeft,
+  kLowerLeft
+};
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+                                  const JoystickAxis &y_axis, const Data &data);
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis);
+
+}  // namespace joysticks
+}  // namespace input
+}  // namespace y2019
+
+#endif  // Y2019_JOYSTICK_ANGLE_H_
diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
new file mode 100644
index 0000000..c352359
--- /dev/null
+++ b/y2019/joystick_angle_test.cc
@@ -0,0 +1,18 @@
+#include "y2019/joystick_angle.h"
+#include <iostream>
+#include "aos/input/driver_station_data.h"
+#include "gtest/gtest.h"
+
+using y2019::input::joysticks::JoystickAngle;
+using y2019::input::joysticks::GetJoystickPosition;
+
+TEST(JoystickAngleTest, JoystickAngleTest) {
+  EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, -0.75));
+  EXPECT_EQ(JoystickAngle::kMiddleRight, GetJoystickPosition(0.75, 0));
+  EXPECT_EQ(JoystickAngle::kLowerRight, GetJoystickPosition(0.75, 0.75));
+  EXPECT_EQ(JoystickAngle::kUpperLeft, GetJoystickPosition(-0.75, -0.75));
+  EXPECT_EQ(JoystickAngle::kMiddleLeft, GetJoystickPosition(-0.75, 0));
+  EXPECT_EQ(JoystickAngle::kLowerLeft, GetJoystickPosition(-0.75, 0.75));
+
+  EXPECT_EQ(JoystickAngle::kDefault, GetJoystickPosition(0, 0));
+}
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index f718c3d..17cc45f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -30,14 +30,28 @@
 namespace joysticks {
 
 // TODO(sabina): update button locations when the board is done
-const ButtonLocation kElevatorUp(4, 4);
 const ButtonLocation kIntakeOut(3, 3);
 const ButtonLocation kElevatorDown(0, 0);
-const ButtonLocation kElevator1(4, 1);
-const ButtonLocation kElevator2(4, 11);
-const ButtonLocation kElevator3(4, 9);
-const ButtonLocation kElevator4(4, 7);
-const ButtonLocation kElevator5(4, 5);
+const ButtonLocation kElevatorFront1(4, 1);
+const ButtonLocation kElevatorFront2(4, 11);
+const ButtonLocation kElevatorFront3(4, 9);
+const ButtonLocation kElevatorFront4(4, 7);
+const ButtonLocation kElevatorFront5(4, 5);
+
+const ButtonLocation kElevatorBack1(3, 14);
+const ButtonLocation kElevatorBack2(4, 12);
+const ButtonLocation kElevatorBack3(4, 10);
+const ButtonLocation kElevatorBack4(4, 8);
+const ButtonLocation kElevatorBack5(4, 6);
+
+const ButtonLocation kElevatorIntaking(3, 4);
+const ButtonLocation kElevatorIntakingUp(3, 6);
+const ButtonLocation kRelease(4, 4);
+
+const ButtonLocation kSuctionBall(3, 13);
+const ButtonLocation kSuctionHatch(3, 12);
+const ButtonLocation kDeployStilt(3, 8);
+const ButtonLocation kFallOver(3, 9);
 
 const ButtonLocation kDiskLoad(0, 0);
 const ButtonLocation kDiskRocketMiddle(0, 0);
@@ -54,11 +68,11 @@
 const ButtonLocation kCargoSuction(0, 0);
 const ButtonLocation kDiskSuction(0, 0);
 const ButtonLocation kSuctionOut(0, 0);
-const ButtonLocation kDeployStilt(3, 8);
 const ButtonLocation kRetractStilt(0, 0);
 const ButtonLocation kBackwards(0, 0);
 
-const ButtonLocation kWristDown(3, 1);
+const ButtonLocation kWristBackwards(3, 10);
+const ButtonLocation kWristForwards(3, 7);
 
 class Reader : public ::aos::input::ActionJoystickInput {
  public:
@@ -146,6 +160,7 @@
 
     // TODO(sabina): decide if we should really have disk suction as its own
     // button
+    /*
     if (data.IsPressed(kCargoSuction)) {
       new_superstructure_goal->suction.top = false;
       new_superstructure_goal->suction.bottom = true;
@@ -156,39 +171,104 @@
       new_superstructure_goal->suction.top = true;
       new_superstructure_goal->suction.bottom = true;
     }
+    */
+
+    if (data.IsPressed(kSuctionBall)) {
+      top_ = false;
+      bottom_ = true;
+    } else if (data.IsPressed(kSuctionHatch)) {
+      top_ = true;
+      bottom_ = true;
+    } else if (data.IsPressed(kRelease) ||
+               !superstructure_queue.status->has_piece) {
+      top_ = false;
+      bottom_ = false;
+    }
 
     // TODO(sabina): max height please?
     if (data.IsPressed(kDeployStilt)) {
-      new_superstructure_goal->stilts.unsafe_goal = 0.3;
+      new_superstructure_goal->stilts.unsafe_goal = 0.50;
+    } else if (data.IsPressed(kFallOver)) {
+      new_superstructure_goal->stilts.unsafe_goal = 0.71;
     } else {
       new_superstructure_goal->stilts.unsafe_goal = 0.01;
     }
 
-    if (data.IsPressed(kIntakeOut)) {
-      new_superstructure_goal->intake.unsafe_goal = 0.8;
-      new_superstructure_goal->roller_voltage = 5.0;
-    } else {
-      new_superstructure_goal->intake.unsafe_goal = -1.2;
-      new_superstructure_goal->roller_voltage = 0.0;
-    }
-
-    if (data.IsPressed(kElevator1)) {
+    if (data.IsPressed(kElevatorFront1)) {
       elevator_height_ = 1.5;
-    } else if (data.IsPressed(kElevator2)) {
+    } else if (data.IsPressed(kElevatorFront2)) {
       elevator_height_ = 1.2;
-    } else if (data.IsPressed(kElevator3)) {
+    } else if (data.IsPressed(kElevatorFront3)) {
       elevator_height_ = 0.8;
-    } else if (data.IsPressed(kElevator4)) {
+    } else if (data.IsPressed(kElevatorFront4)) {
       elevator_height_ = 0.3;
-    } else if (data.IsPressed(kElevator5)) {
+    } else if (data.IsPressed(kElevatorFront5)) {
       elevator_height_ = 0.01;
     }
 
+    /*
     if (data.IsPressed(kWristDown)) {
       wrist_angle_ = -M_PI / 3.0;
     } else {
       wrist_angle_ = M_PI / 3.0;
     }
+    */
+    if (data.IsPressed(kWristBackwards)) {
+      // Hatch pannel back
+      elevator_height_ = 0.03;
+      wrist_angle_ = -M_PI / 2.0;
+      Disc();
+    } else if (data.IsPressed(kWristForwards)) {
+      // Hatch pannel front
+      elevator_height_ = 0.03;
+      wrist_angle_ = M_PI / 2.0;
+      Disc();
+    } else if (data.IsPressed(kElevatorFront5)) {
+      // Ball front
+      Ball();
+      elevator_height_ = 0.52;
+      wrist_angle_ = 1.1;
+    } else if (data.IsPressed(kElevatorBack5)) {
+      // Ball back
+      elevator_height_ = 0.52;
+      wrist_angle_ = -1.1;
+    } else if (data.IsPressed(kElevatorFront2)) {
+      elevator_height_ = 1.5;
+      wrist_angle_ = 0.0;
+    } else {
+      wrist_angle_ = 0.0;
+      elevator_height_ = 0.36;
+    }
+    //if (data.IsPressed(kElevatorIntaking)) {
+    //}
+    if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
+      elevator_height_ = 0.29;
+      wrist_angle_ = 2.14;
+      new_superstructure_goal->intake.unsafe_goal = 0.52;
+      if (data.IsPressed(kElevatorIntaking)) {
+        new_superstructure_goal->roller_voltage = 6.0;
+      } else {
+        new_superstructure_goal->roller_voltage = 0.0;
+      }
+      Ball();
+    } else {
+      new_superstructure_goal->intake.unsafe_goal = -1.2;
+      new_superstructure_goal->roller_voltage = 0.0;
+    }
+
+    if (data.IsPressed(kElevatorIntakingUp)) {
+      elevator_height_ = 0.29 + 0.3;
+      wrist_angle_ = 2.14;
+    }
+
+
+    if (data.IsPressed(kRelease)) {
+      top_ = false;
+      bottom_ = false;
+    }
+
+    new_superstructure_goal->suction.top = top_;
+    new_superstructure_goal->suction.bottom = bottom_;
 
     new_superstructure_goal->elevator.unsafe_goal = elevator_height_;
     new_superstructure_goal->wrist.unsafe_goal = wrist_angle_;
@@ -199,10 +279,22 @@
     }
   }
 
+  void Disc() {
+    top_ = true;
+    bottom_ = true;
+  }
+  void Ball() {
+    top_ = false;
+    bottom_ = true;
+  }
+
  private:
   // Current goals here.
   double elevator_height_ = 0.0;
   double wrist_angle_ = 0.0;
+
+  bool top_ = false;
+  bool bottom_ = false;
 };
 
 }  // namespace joysticks
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 339a388..b3969fb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -194,6 +194,11 @@
     stilts_encoder_.set_potentiometer(::std::move(potentiometer));
   }
 
+  // Vacuum pressure sensor
+  void set_vacuum_sensor(int port) {
+    vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
+  }
+
   void RunIteration() override {
     {
       auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -236,6 +241,13 @@
                    Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
                    values.stilts.potentiometer_offset);
 
+      // Suction
+      constexpr float kMinVoltage = 0.5;
+      constexpr float kMaxVoltage = 2.1;
+      superstructure_message->suction_pressure =
+          (vacuum_sensor_->GetVoltage() - kMinVoltage) /
+          (kMaxVoltage - kMinVoltage);
+
       superstructure_message.Send();
     }
   }
@@ -244,6 +256,8 @@
   ::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
       wrist_encoder_, stilts_encoder_;
 
+  ::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+
   ::frc971::wpilib::AbsoluteEncoder intake_encoder_;
   // TODO(sabina): Add wrist and elevator hall effects.
 };
@@ -254,6 +268,10 @@
     elevator_victor_ = ::std::move(t);
   }
 
+  void set_suction_victor(::std::unique_ptr<::frc::VictorSP> t) {
+    suction_victor_ = ::std::move(t);
+  }
+
   void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
     intake_victor_ = ::std::move(t);
   }
@@ -267,12 +285,12 @@
   }
 
  private:
-  virtual void Read() override {
+  void Read() override {
     ::y2019::control_loops::superstructure::superstructure_queue.output
         .FetchAnother();
   }
 
-  virtual void Write() override {
+  void Write() override {
     auto &queue =
         ::y2019::control_loops::superstructure::superstructure_queue.output;
     LOG_STRUCT(DEBUG, "will output", *queue);
@@ -292,19 +310,24 @@
     stilts_victor_->SetSpeed(::aos::Clip(queue->stilts_voltage,
                                          -kMaxBringupPower, kMaxBringupPower) /
                              12.0);
+
+    suction_victor_->SetSpeed(
+        ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
+        12.0);
   }
 
-  virtual void Stop() override {
+  void Stop() override {
     LOG(WARNING, "Superstructure output too old.\n");
 
     elevator_victor_->SetDisabled();
     intake_victor_->SetDisabled();
     wrist_victor_->SetDisabled();
     stilts_victor_->SetDisabled();
+    suction_victor_->SetDisabled();
   }
 
   ::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
-      wrist_victor_, stilts_victor_;
+      wrist_victor_, stilts_victor_, suction_victor_;
 };
 
 class SolenoidWriter {
@@ -314,11 +337,13 @@
             ".y2019.control_loops.superstructure.superstructure_queue.output") {
   }
 
-  void set_big_suction_cup(int index) {
-    big_suction_cup_ = pcm_.MakeSolenoid(index);
+  void set_big_suction_cup(int index0, int index1) {
+    big_suction_cup0_ = pcm_.MakeSolenoid(index0);
+    big_suction_cup1_ = pcm_.MakeSolenoid(index1);
   }
-  void set_small_suction_cup(int index) {
-    small_suction_cup_ = pcm_.MakeSolenoid(index);
+  void set_small_suction_cup(int index0, int index1) {
+    small_suction_cup0_ = pcm_.MakeSolenoid(index0);
+    small_suction_cup1_ = pcm_.MakeSolenoid(index1);
   }
 
   void set_intake_roller_talon(
@@ -348,8 +373,10 @@
         if (superstructure_.get()) {
           LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
 
-          big_suction_cup_->Set(!superstructure_->intake_suction_top);
-          small_suction_cup_->Set(!superstructure_->intake_suction_bottom);
+          big_suction_cup0_->Set(!superstructure_->intake_suction_top);
+          big_suction_cup1_->Set(!superstructure_->intake_suction_top);
+          small_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
+          small_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
 
           intake_rollers_talon_->Set(
               ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
@@ -374,8 +401,8 @@
  private:
   ::frc971::wpilib::BufferedPcm pcm_;
 
-  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup_,
-      small_suction_cup_;
+  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup0_,
+      big_suction_cup1_, small_suction_cup0_, small_suction_cup1_;
 
   ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
       intake_rollers_talon_;
@@ -424,6 +451,7 @@
     reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
 
     reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+    reader.set_vacuum_sensor(7);
 
     ::std::thread reader_thread(::std::ref(reader));
 
@@ -450,8 +478,8 @@
     superstructure_writer.set_elevator_victor(
         ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
     // TODO(austin): Do the vacuum
-    //superstructure_writer.set_vacuum(
-        //::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+    superstructure_writer.set_suction_victor(
+        ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
     superstructure_writer.set_intake_victor(
         ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
     superstructure_writer.set_wrist_victor(
@@ -465,8 +493,8 @@
     SolenoidWriter solenoid_writer;
     solenoid_writer.set_intake_roller_talon(
         make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
-    solenoid_writer.set_big_suction_cup(0);
-    solenoid_writer.set_small_suction_cup(1);
+    solenoid_writer.set_big_suction_cup(0, 1);
+    solenoid_writer.set_small_suction_cup(2, 3);
 
     ::std::thread solenoid_writer_thread(::std::ref(solenoid_writer));