diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1b37612..58ce2a1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -257,7 +257,7 @@
     dt_closedloop_.SetGoal(*goal);
     dt_openloop_.SetGoal(*goal);
     dt_spline_.SetGoal(*goal);
-    dt_line_follow_.SetGoal(*goal);
+    dt_line_follow_.SetGoal(monotonic_now, *goal);
   }
 
   dt_openloop_.Update(robot_state().voltage_battery);
@@ -272,7 +272,7 @@
 
   dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
 
-  dt_line_follow_.Update(trajectory_state);
+  dt_line_follow_.Update(monotonic_now, trajectory_state);
 
   switch (controller_type) {
     case 0:
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 74be414..7f778f7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -67,6 +67,14 @@
   float x;
   float y;
   float theta;
+  // Current lateral offset from line pointing straight out of the target.
+  float offset;
+  // Current distance from the plane of the target, in meters.
+  float distance_to_target;
+  // Current goal heading.
+  float goal_theta;
+  // Current relative heading.
+  float rel_theta;
 };
 
 queue_group DrivetrainQueue {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b00ab3f..839f1b8 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -744,7 +744,7 @@
   // Should have run off the end of the target, running along the y=x line.
   EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
   EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
-              drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+              drivetrain_motor_plant_.GetPosition().y(), 0.1);
 }
 
 // Tests that the line follower will not run and defer to regular open-loop
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index b5417af..b6e4947 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -11,7 +11,6 @@
 namespace drivetrain {
 
 constexpr double LineFollowDrivetrain::kMaxVoltage;
-constexpr double LineFollowDrivetrain::kPolyN;
 
 namespace {
 ::Eigen::Matrix<double, 3, 3> AContinuous(
@@ -99,71 +98,34 @@
       target_selector_(target_selector),
       U_(0, 0) {}
 
-double LineFollowDrivetrain::GoalTheta(
-    const ::Eigen::Matrix<double, 5, 1> &state) const {
-  // TODO(james): Consider latching the sign of the goal so that the driver
-  // can back up without the robot trying to turn around...
-  // On the other hand, we may just want to force the driver to take over
-  // entirely if they need to backup.
-  int sign = ::aos::sign(goal_velocity_);
-  // Given a Nth degree polynomial with just a single term that
-  // has its minimum (with a slope of zero) at (0, 0) and passing
-  // through (x0, y0), we will have:
-  // y = a * x^N
-  // a = y0 / x0^N
-  // And the slope of the tangent line at (x0, y0) will be
-  // N * a * x0^(N-1)
-  // = N * y0 / x0^N * x0^(N-1)
-  // = N * y0 / x0
-  // Giving a heading of
-  // atan2(-N * y0, -x0)
-  // where we add the negative signs to make things work out properly when we
-  // are trying to drive forwards towards zero. We reverse the sign of both
-  // terms if we want to drive backwards.
-  return ::std::atan2(-sign * kPolyN * state.y(), -sign * state.x());
-}
-
-double LineFollowDrivetrain::GoalThetaDot(
-    const ::Eigen::Matrix<double, 5, 1> &state) const {
-  // theta = atan2(-N * y0, -x0)
-  // Note: d(atan2(p(t), q(t)))/dt
-  //       = (p(t) * q'(t) - q(t) * p'(t)) / (p(t)^2 + q(t)^2)
-  // Note: x0' = cos(theta) * v, y0' = sin(theta) * v
-  // Note that for the sin/cos calculations we discard the
-  //   negatives in the arctangent to make the signs work out (the
-  //   dtheta/dt needs to be correct as we travel along the path). This
-  //   also corresponds with the fact that thetadot is agnostic towards
-  //   whether the robot is driving forwards or backwards, so long as it is
-  //   trying to drive towards the target.
-  // Note: sin(theta) = sin(atan2(N * y0, x0))
-  //       = N * y0 / sqrt(N^2 * y0^2 + x0^2)
-  // Note: cos(theta) = cos(atan2(N * y0, x0))
-  //       = x0 / sqrt(N^2 * y0^2 + x0^2)
-  // dtheta/dt
-  // = (-x0 * (-N * y0') - -N * y0 * (-x0')) / (N^2 * y0^2 + x0^2)
-  // = N * (x0 * v * sin(theta) - y0 * v * cos(theta)) / (N^2 * y0^2 + x0^2)
-  // = N * v * (x0 * (N * y0) - y0 * (x0)) / (N^2 * y0^2 + x0^2)^1.5
-  // = N * v * (N - 1) * x0 * y0 / (N^2 * y0^2 + x0^2)^1.5
-  const double linear_vel = (state(3, 0) + state(4, 0)) / 2.0;
-  const double x2 = ::std::pow(state.x(), 2);
-  const double y2 = ::std::pow(state.y(), 2);
-  const double norm = y2 * kPolyN * kPolyN + x2;
-  // When we get too near the goal, avoid singularity in a sane manner.
-  if (norm < 1e-3) {
-    return 0.0;
-  }
-  return kPolyN * (kPolyN - 1) * linear_vel * state.x() * state.y() /
-         (norm * ::std::sqrt(norm));
-}
-
 void LineFollowDrivetrain::SetGoal(
+    ::aos::monotonic_clock::time_point now,
     const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
   // TODO(james): More properly calculate goal velocity from throttle.
-  goal_velocity_ = goal.throttle;
+  goal_velocity_ = 4.0 * goal.throttle;
+  // The amount of time to freeze the target choice after the driver releases
+  // the button. Depending on the current state, we vary how long this timeout
+  // is so that we can avoid button glitches causing issues.
+  ::std::chrono::nanoseconds freeze_delay = ::std::chrono::seconds(0);
   // Freeze the target once the driver presses the button; if we haven't yet
   // confirmed a target when the driver presses the button, we will not do
   // anything and report not ready until we have a target.
-  freeze_target_ = goal.controller_type == 3;
+  if (goal.controller_type == 3) {
+    last_enable_ = now;
+    // If we already acquired a target, we want to keep track if it.
+    if (have_target_) {
+      freeze_delay = ::std::chrono::milliseconds(500);
+      // If we've had the target acquired for a while, the driver is probably
+      // happy with the current target selection, so we really want to keep it.
+      if (now - start_of_target_acquire_ > ::std::chrono::milliseconds(1000)) {
+        freeze_delay = ::std::chrono::milliseconds(2000);
+      }
+    }
+  }
+  freeze_target_ = now <= freeze_delay + last_enable_;
+  // Set an adjustment that lets the driver tweak the offset for where we place
+  // the target left/right.
+  side_adjust_ = -goal.wheel * 0.1;
 }
 
 bool LineFollowDrivetrain::SetOutput(
@@ -177,7 +139,48 @@
   return have_target_;
 }
 
+double LineFollowDrivetrain::GoalTheta(
+    const ::Eigen::Matrix<double, 5, 1> &abs_state, double relative_y_offset,
+    double velocity_sign) {
+  // Calculates the goal angle for the drivetrain given our position.
+  // The calculated goal will be such that a point disc_rad to one side of the
+  // drivetrain (the side depends on where we approach from) will end up hitting
+  // the plane of the target exactly disc_rad from the center of the target.
+  // This allows us to better approach targets in the 2019 game from an
+  // angle--radii of zero imply driving straight in.
+  const double disc_rad = target_selector_->TargetRadius();
+  // Depending on whether we are to the right or left of the target, we work off
+  // of a different side of the robot.
+  const double edge_sign = relative_y_offset > 0 ? 1.0 : -1.0;
+  // Note side_adjust which is the input from the driver's wheel to allow
+  // shifting the goal target left/right.
+  const double edge_offset = edge_sign * disc_rad - side_adjust_;
+  // The point that we are trying to get the disc to hit.
+  const Pose corner = Pose(&target_pose_, {0.0, edge_offset, 0.0}, 0.0);
+  // A pose for the current robot position that is square to the target.
+  Pose square_robot =
+      Pose({abs_state.x(), abs_state.y(), 0.0}, 0.0).Rebase(&corner);
+  // To prevent numerical issues, we limit x so that when the localizer isn't
+  // working properly and ends up driving past the target, we still get sane
+  // results.
+  square_robot.mutable_pos()->x() =
+      ::std::min(::std::min(square_robot.mutable_pos()->x(), -disc_rad), -0.01);
+  // Distance from the edge of the disc on the robot to the velcro we want to
+  // hit on the target.
+  const double dist_to_corner = square_robot.xy_norm();
+  // The following actually handles calculating the heading we need the robot to
+  // take (relative to the plane of the target).
+  const double alpha = ::std::acos(disc_rad / dist_to_corner);
+  const double heading_to_robot = edge_sign * square_robot.heading();
+  double theta = -edge_sign * (M_PI - alpha - (heading_to_robot - M_PI_2));
+  if (velocity_sign < 0) {
+    theta = ::aos::math::NormalizeAngle(theta + M_PI);
+  }
+  return theta;
+}
+
 void LineFollowDrivetrain::Update(
+    ::aos::monotonic_clock::time_point now,
     const ::Eigen::Matrix<double, 5, 1> &abs_state) {
   // Because we assume the target selector may have some internal state (e.g.,
   // not confirming a target until some time as passed), we should call
@@ -189,6 +192,8 @@
     // target before.
     if (!have_target_ && new_target) {
       have_target_ = true;
+      start_of_target_acquire_ = now;
+      velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
       target_pose_ = target_selector_->TargetPose();
     }
   } else {
@@ -197,12 +202,15 @@
     have_target_ = new_target;
     if (have_target_) {
       target_pose_ = target_selector_->TargetPose();
+      velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
     }
   }
-
   // Get the robot pose in the target coordinate frame.
-  const Pose relative_robot_pose = Pose({abs_state.x(), abs_state.y(), 0.0},
-                                        abs_state(2, 0)).Rebase(&target_pose_);
+  relative_pose_ = Pose({abs_state.x(), abs_state.y(), 0.0}, abs_state(2, 0))
+                       .Rebase(&target_pose_);
+  double goal_theta =
+      GoalTheta(abs_state, relative_pose_.rel_pos().y(), velocity_sign_);
+
   // Always force a slight negative X, so that the driver can continue to drive
   // past zero if they want.
   // The "slight negative" needs to be large enough that we won't force
@@ -210,28 +218,38 @@
   // line--because, in practice, the driver should never be trying to drive to a
   // target where they are significantly off laterally at <0.1m from the target,
   // this should not be a problem.
-  const double relative_x = ::std::min(relative_robot_pose.rel_pos().x(), -0.1);
+  const double relative_x = ::std::min(relative_pose_.rel_pos().x(), -0.1);
   const ::Eigen::Matrix<double, 5, 1> rel_state =
       (::Eigen::Matrix<double, 5, 1>() << relative_x,
-       relative_robot_pose.rel_pos().y(), relative_robot_pose.rel_theta(),
-       abs_state(3, 0), abs_state(4, 0))
-          .finished();
-  ::Eigen::Matrix<double, 3, 1> controls_goal(
-      GoalTheta(rel_state), goal_velocity_, GoalThetaDot(rel_state));
+       relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
+       abs_state(3, 0), abs_state(4, 0)).finished();
+  if (velocity_sign_ * goal_velocity_ < 0)  {
+    goal_theta = rel_state(2, 0);
+  }
+  controls_goal_ << goal_theta, goal_velocity_, 0.0;
   ::Eigen::Matrix<double, 3, 1> controls_state;
   controls_state(0, 0) = rel_state(2, 0);
   controls_state.block<2, 1>(1, 0) =
       dt_config_.Tlr_to_la() * rel_state.block<2, 1>(3, 0);
-  ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal - controls_state;
+  ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal_ - controls_state;
   // Because we are taking the difference of an angle, normaliez to [-pi, pi].
   controls_err(0, 0) = ::aos::math::NormalizeAngle(controls_err(0, 0));
   // TODO(james): Calculate the next goal so that we are properly doing
   // feed-forwards.
   ::Eigen::Matrix<double, 2, 1> U_ff =
-      Kff_ * (controls_goal - A_d_ * controls_goal);
+      Kff_ * (controls_goal_ - A_d_ * controls_goal_);
   U_ = K_ * controls_err + U_ff;
-  const double maxU = U_.lpNorm<::Eigen::Infinity>();
-  U_ *= (maxU > kMaxVoltage) ? kMaxVoltage / maxU : 1.0;
+  const double maxU = ::std::max(U_(0, 0), U_(1, 0));
+  const double minU = ::std::min(U_(0, 0), U_(1, 0));
+  const double diffU = ::std::abs(U_(1, 0) - U_(0, 0));
+  const double maxAbsU = ::std::max(maxU, -minU);
+  if (diffU > 24.0) {
+    U_ *= (maxAbsU > kMaxVoltage) ? kMaxVoltage / maxAbsU : 1.0;
+  } else if (maxU > 12.0) {
+    U_ = U_ + U_.Ones() * (12.0 - maxU);
+  } else if (minU < -12.0) {
+    U_ = U_ - U_.Ones() * (minU + 12.0);
+  }
 }
 
 void LineFollowDrivetrain::PopulateStatus(
@@ -241,6 +259,11 @@
   status->line_follow_logging.x = target_pose_.abs_pos().x();
   status->line_follow_logging.y = target_pose_.abs_pos().y();
   status->line_follow_logging.theta = target_pose_.abs_theta();
+  status->line_follow_logging.offset = relative_pose_.rel_pos().y();
+  status->line_follow_logging.distance_to_target =
+      -relative_pose_.rel_pos().x();
+  status->line_follow_logging.goal_theta = controls_goal_(0, 0);
+  status->line_follow_logging.rel_theta = relative_pose_.rel_theta();
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index de9ff5a..09b2080 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -17,25 +17,22 @@
 
 // A drivetrain that permits a velocity input from the driver while controlling
 // lateral motion.
-// TODO(james): Do we want to allow some driver input on lateral control? It may
-// just be too confusing to make work effectively, but it also wouldn't be too
-// hard to just allow the driver to specify an offset to the angle/angular
-// velocity.
 class LineFollowDrivetrain {
  public:
   typedef TypedPose<double> Pose;
 
-  LineFollowDrivetrain(
-      const DrivetrainConfig<double> &dt_config,
-      TargetSelectorInterface *target_selector);
+  LineFollowDrivetrain(const DrivetrainConfig<double> &dt_config,
+                       TargetSelectorInterface *target_selector);
   // Sets the current goal; the drivetrain queue contains throttle_velocity
   // which is used to command overall robot velocity. The goal_pose is a Pose
-  // representing where we are tring to go. This would typically be the Pose of
+  // representing where we are trying to go. This would typically be the Pose of
   // a Target; the positive X-axis in the Pose's frame represents the direction
   // we want to go (we approach the pose from the left-half plane).
-  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+  void SetGoal(::aos::monotonic_clock::time_point now,
+               const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
   // State: [x, y, theta, left_vel, right_vel]
-  void Update(const ::Eigen::Matrix<double, 5, 1> &state);
+  void Update(::aos::monotonic_clock::time_point now,
+              const ::Eigen::Matrix<double, 5, 1> &state);
   // Returns whether we set the output. If false, that implies that we do not
   // yet have a target to track into and so some other drivetrain should take
   // over.
@@ -48,24 +45,18 @@
   // Nominal max voltage.
   // TODO(james): Is there a config for this or anything?
   static constexpr double kMaxVoltage = 12.0;
-  // Degree of the polynomial to follow in. Should be strictly greater than 1.
-  // A value of 1 would imply that we drive straight to the target (but not hit
-  // it straight on, unless we happened to start right in front of it). A value
-  // of zero would imply driving straight until we hit the plane of the target.
-  // Values between 0 and 1 would imply hitting the target from the side.
-  static constexpr double kPolyN = 3.0;
-  static_assert(kPolyN > 1.0,
-                "We want to hit targets straight on (see above comments).");
 
-  double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &state) const;
-  double GoalThetaDot(const ::Eigen::Matrix<double, 5, 1> &state) const;
+  double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &abs_state,
+                   double relative_y_offset, double velocity_sign);
 
   const DrivetrainConfig<double> dt_config_;
   // TODO(james): These should probably be factored out somewhere.
+  // TODO(james): This controller is not actually asymptotically stable, due to
+  // the varying goal theta.
   const ::Eigen::DiagonalMatrix<double, 3> Q_ =
       (::Eigen::DiagonalMatrix<double, 3>().diagonal()
-           << 1.0 / ::std::pow(0.01, 2),
-       1.0 / ::std::pow(0.1, 2), 1.0 / ::std::pow(10.0, 2))
+           << 1.0 / ::std::pow(0.1, 2),
+       1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
           .finished()
           .asDiagonal();
   const ::Eigen::DiagonalMatrix<double, 2> R_ =
@@ -86,6 +77,20 @@
   bool have_target_ = false;
   Pose target_pose_;
   double goal_velocity_ = 0.0;
+  // The amount to shift the center of the goal target side-to-side, to give the
+  // driver an input that lets them account for panels that are offset on the
+  // grabber.
+  double side_adjust_ = 0.0;  // meters
+  double velocity_sign_ = 1.0;
+
+  // The last time at which we saw an enable button.
+  ::aos::monotonic_clock::time_point last_enable_;
+  // The time at which we first acquired the current target.
+  ::aos::monotonic_clock::time_point start_of_target_acquire_;
+  // Most recent relative pose to target, used for debugging.
+  Pose relative_pose_;
+  // Current goal state, used for debugging.
+  ::Eigen::Matrix<double, 3, 1> controls_goal_;
 
   // Voltage output to apply
   ::Eigen::Matrix<double, 2, 1> U_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 65cf9bf..414ec81 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -53,8 +53,8 @@
     ::frc971::control_loops::DrivetrainQueue::Goal goal;
     goal.throttle = driver_model_(state_);
     goal.controller_type = freeze_target_ ? 3 : 0;
-    drivetrain_.SetGoal(goal);
-    drivetrain_.Update(state_);
+    drivetrain_.SetGoal(t_, goal);
+    drivetrain_.Update(t_, state_);
 
     ::frc971::control_loops::DrivetrainQueue::Output output;
     EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
@@ -85,51 +85,26 @@
     simulation_ur_.push_back(U(1, 0));
     simulation_x_.push_back(state_.x());
     simulation_y_.push_back(state_.y());
+    simulation_theta_.push_back(state_(3, 0));
   }
 
   // Check that left/right velocities are near zero and the absolute position is
   // near that of the goal Pose.
   void VerifyNearGoal() const {
-    EXPECT_NEAR(goal_pose().abs_pos().x(), state_(0, 0), 1e-3);
-    EXPECT_NEAR(goal_pose().abs_pos().y(), state_(1, 0), 1e-3);
-    // state should be at 0 or PI (we should've come in striaght on or straight
-    // backwards).
-    const double angle_err =
-        ::aos::math::DiffAngle(goal_pose().abs_theta(), state_(2, 0));
-    const double angle_pi_err = ::aos::math::DiffAngle(angle_err, M_PI);
-    EXPECT_LT(::std::min(::std::abs(angle_err), ::std::abs(angle_pi_err)),
-              1e-3);
+    EXPECT_NEAR(goal_pose().abs_pos().x(), state_(0, 0), 1e-2);
+    EXPECT_NEAR(goal_pose().abs_pos().y(), state_(1, 0), 1e-2);
     // Left/right velocities are zero:
-    EXPECT_NEAR(0.0, state_(3, 0), 1e-3);
-    EXPECT_NEAR(0.0, state_(4, 0), 1e-3);
+    EXPECT_NEAR(0.0, state_(3, 0), 1e-2);
+    EXPECT_NEAR(0.0, state_(4, 0), 1e-2);
   }
 
-  void CheckGoalTheta(double x, double y, double v, double expected_theta,
-                          double expected_thetadot) {
+  double GoalTheta(double x, double y, double v, double throttle) {
+    ::frc971::control_loops::DrivetrainQueue::Goal goal;
+    goal.throttle = throttle;
+    drivetrain_.SetGoal(t_, goal);
     ::Eigen::Matrix<double, 5, 1> state;
     state << x, y, 0.0, v, v;
-    const double theta = drivetrain_.GoalTheta(state);
-    const double thetadot = drivetrain_.GoalThetaDot(state);
-    EXPECT_EQ(expected_theta, theta) << "x " << x << " y " << y << " v " << v;
-    EXPECT_EQ(expected_thetadot, thetadot)
-        << "x " << x << " y " << y << " v " << v;
-  }
-
-  void CheckGoalThetaDotAtState(double x, double y, double v) {
-    ::Eigen::Matrix<double, 5, 1> state;
-    state << x, y, 0.0, v, v;
-    const double theta = drivetrain_.GoalTheta(state);
-    const double thetadot = drivetrain_.GoalThetaDot(state);
-    const double dx = v * ::std::cos(theta);
-    const double dy = v * ::std::sin(theta);
-    constexpr double kEps = 1e-5;
-    state(0, 0) += dx * kEps;
-    state(1, 0) += dy * kEps;
-    const double next_theta = drivetrain_.GoalTheta(state);
-    EXPECT_NEAR(thetadot, ::aos::math::DiffAngle(next_theta, theta) / kEps,
-                1e-4)
-        << "theta: " << theta << " nexttheta: " << next_theta << " x " << x
-        << " y " << y << " v " << v;
+    return drivetrain_.GoalTheta(state, y, throttle > 0.0 ? 1.0 : -1.0);
   }
 
   void TearDown() override {
@@ -137,6 +112,7 @@
       matplotlibcpp::figure();
       matplotlibcpp::plot(time_, simulation_ul_, {{"label", "ul"}});
       matplotlibcpp::plot(time_, simulation_ur_, {{"label", "ur"}});
+      matplotlibcpp::plot(time_, simulation_theta_, {{"label", "theta"}});
       matplotlibcpp::legend();
 
       TypedPose<double> target = goal_pose();
@@ -194,18 +170,33 @@
   ::std::vector<double> simulation_ur_;
   ::std::vector<double> simulation_x_;
   ::std::vector<double> simulation_y_;
+  ::std::vector<double> simulation_theta_;
 };
 
-TEST_F(LineFollowDrivetrainTest, ValidGoalThetaDot) {
+TEST_F(LineFollowDrivetrainTest, BasicGoalThetaCheck) {
   for (double x : {1.0, 0.0, -1.0, -2.0, -3.0}) {
     for (double y : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
       for (double v : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
-        if (x == 0.0 && y == 0.0) {
-          // When x/y are zero, we can encounter singularities. The code should
-          // just provide zeros in this case.
-          CheckGoalTheta(x, y, v, 0.0, 0.0);
-        } else {
-          CheckGoalThetaDotAtState(x, y, v);
+        for (double throttle : {-1.0, 1.0}) {
+          target_selector_.set_target_radius(0.0);
+          const double zero_rad_theta = GoalTheta(x, y, v, throttle);
+          EXPECT_NEAR(
+              0.0,
+              ::aos::math::DiffAngle((throttle > 0.0 ? M_PI : 0.0) +
+                                         ::std::atan2(y, ::std::min(-0.01, x)),
+                                     zero_rad_theta),
+              1e-14);
+          target_selector_.set_target_radius(0.05);
+          const double small_rad_theta = GoalTheta(x, y, v, throttle);
+          if (y > 0) {
+            EXPECT_LT(small_rad_theta, zero_rad_theta);
+          } else if (y == 0) {
+            EXPECT_NEAR(0.0,
+                        ::aos::math::DiffAngle(small_rad_theta, zero_rad_theta),
+                        1e-14);
+          } else {
+            EXPECT_GT(small_rad_theta, zero_rad_theta);
+          }
         }
       }
     }
@@ -215,8 +206,12 @@
 // If the driver commands the robot to keep going, we should just run straight
 // off the end and keep going along the line:
 TEST_F(LineFollowDrivetrainTest, RunOffEnd) {
-  state_ << -1.0, 0.1, 0.0, 0.0, 0.0;
-  driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 1.0; };
+  freeze_target_ = true;
+  state_ << -1.0, 0.0, 0.0, 0.0, 0.0;
+  // TODO(james): This test currently relies on the scalar on the throttle to
+  // velocity conversion being 4.0. This should probably be moved out into a
+  // config.
+  driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 0.25; };
   RunForTime(chrono::seconds(10));
   EXPECT_LT(6.0, state_.x());
   EXPECT_NEAR(0.0, state_.y(), 1e-4);
@@ -250,7 +245,8 @@
   set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
 
   RunForTime(::std::chrono::seconds(5));
-  EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25);
+  EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25)
+      << "Expected state of zero, got: " << state_.transpose();
 }
 
 // Tests that when we freeze the controller without having acquired a target, we
@@ -268,9 +264,9 @@
 
   // Now, provide a target:
   target_selector_.set_has_target(true);
-  set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
+  set_goal_pose({{1.0, 2.0, 0.0}, M_PI_2});
   driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
-    return (state.topRows<2>() - goal_pose().abs_pos().topRows<2>()).norm();
+    return -(state.y() - goal_pose().abs_pos().y());
   };
   expect_output_ = true;
 
@@ -289,10 +285,13 @@
     : public LineFollowDrivetrainTest,
       public ::testing::WithParamInterface<Pose> {};
 TEST_P(LineFollowDrivetrainTargetParamTest, NonZeroTargetTest) {
+  freeze_target_ = true;
+  target_selector_.set_has_target(true);
   // Start the state at zero and then put the target in a
   state_.setZero();
   driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
-    return (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
+    return 0.2 *
+           (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
   };
   set_goal_pose(GetParam());
   RunForTime(chrono::seconds(10));
@@ -353,13 +352,13 @@
                 .finished()),
         ::testing::Values(
             [](const ::Eigen::Matrix<double, 5, 1> &state) {
-              return -5.0 * state.x();
+              return -1.0 * state.x();
             },
             [](const ::Eigen::Matrix<double, 5, 1> &state) {
-              return 5.0 * state.x();
+              return 1.0 * state.x();
             },
             [](const ::Eigen::Matrix<double, 5, 1> &state) {
-              return -1.0 * ::std::abs(state.x()) - 0.5 * state.x() * state.x();
+              return -0.25 * ::std::abs(state.x()) - 0.125 * state.x() * state.x();
             })));
 
 }  // namespace testing