diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 50c5df7..aabd89d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -29,7 +29,9 @@
     : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
           my_drivetrain),
       dt_config_(dt_config),
-      kf_(dt_config_.make_kf_drivetrain_loop()) {
+      kf_(dt_config_.make_kf_drivetrain_loop()),
+      dt_openloop_(dt_config_, &kf_),
+      dt_closedloop_(dt_config_) {
   ::aos::controls::HPolytope<0>::Init();
 }
 
@@ -116,6 +118,8 @@
     status->output_was_capped = dt_closedloop_.OutputWasCapped();
     status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
     status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
+
+    dt_openloop_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 22a6c32..a2f2015 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -41,9 +41,9 @@
 
   const DrivetrainConfig dt_config_;
 
-  PolyDrivetrain dt_openloop_{dt_config_};
-  DrivetrainMotorsSS dt_closedloop_{dt_config_};
   StateFeedbackLoop<7, 2, 3> kf_;
+  PolyDrivetrain dt_openloop_;
+  DrivetrainMotorsSS dt_closedloop_;
 
   double last_left_voltage_ = 0;
   double last_right_voltage_ = 0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 7fa91b9..24b1d38 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -112,6 +112,10 @@
     double uncapped_left_voltage;
     double uncapped_right_voltage;
 
+    // The goal velocities for the polydrive controller.
+    double left_velocity_goal;
+    double right_velocity_goal;
+
     // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 6e08b84..7287df1 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -20,8 +20,9 @@
 using ::frc971::control_loops::CIMLogging;
 using ::frc971::control_loops::CoerceGoal;
 
-PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config)
-    : kf_(dt_config.make_kf_drivetrain_loop()),
+PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
+                               StateFeedbackLoop<7, 2, 3> *kf)
+    : kf_(kf),
       U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
                /*[*/ -1, 0 /*]*/,
                /*[*/ 0, 1 /*]*/,
@@ -212,7 +213,7 @@
   }
 }
 
-double PolyDrivetrain::FilterVelocity(double throttle) {
+double PolyDrivetrain::FilterVelocity(double throttle) const {
   const Eigen::Matrix<double, 2, 2> FF =
       loop_->B().inverse() *
       (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
@@ -261,8 +262,8 @@
 
 void PolyDrivetrain::Update() {
   if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
-    loop_->mutable_X_hat()(0, 0) = kf_.X_hat()(1, 0);
-    loop_->mutable_X_hat()(1, 0) = kf_.X_hat()(3, 0);
+    loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
+    loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
   }
 
   // TODO(austin): Observer for the current velocity instead of difference
@@ -292,6 +293,8 @@
     }
     const double left_velocity = fvel - steering_velocity;
     const double right_velocity = fvel + steering_velocity;
+    goal_left_velocity_ = left_velocity;
+    goal_right_velocity_ = right_velocity;
 
     // Integrate velocity to get the position.
     // This position is used to get integral control.
@@ -354,6 +357,8 @@
 
       LOG_STRUCT(DEBUG, "currently", logging);
     }
+    goal_left_velocity_ = current_left_velocity;
+    goal_right_velocity_ = current_right_velocity;
 
     // Any motor is not in gear. Speed match.
     ::Eigen::Matrix<double, 1, 1> R_left;
@@ -384,6 +389,12 @@
   }
 }
 
+void PolyDrivetrain::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) {
+  status->left_velocity_goal = goal_left_velocity_;
+  status->right_velocity_goal = goal_right_velocity_;
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c482f77..689e76f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -15,7 +15,8 @@
  public:
   enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
 
-  PolyDrivetrain(const DrivetrainConfig &dt_config);
+  PolyDrivetrain(const DrivetrainConfig &dt_config,
+                 StateFeedbackLoop<7, 2, 3> *kf);
 
   int controller_index() const { return loop_->controller_index(); }
 
@@ -39,7 +40,7 @@
   void SetPosition(
       const ::frc971::control_loops::DrivetrainQueue::Position *position);
 
-  double FilterVelocity(double throttle);
+  double FilterVelocity(double throttle) const;
 
   double MaxVelocity();
 
@@ -47,8 +48,10 @@
 
   void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
 
+  void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+
  private:
-  StateFeedbackLoop<7, 2, 3> kf_;
+  StateFeedbackLoop<7, 2, 3> *kf_;
 
   const ::aos::controls::HPolytope<2> U_Poly_;
 
@@ -66,6 +69,9 @@
   ::frc971::control_loops::DrivetrainQueue::Position position_;
   int counter_;
   DrivetrainConfig dt_config_;
+
+  double goal_left_velocity_ = 0.0;
+  double goal_right_velocity_ = 0.0;
 };
 
 }  // namespace drivetrain