commit 95771d9ac4447b099a1fa06bc0540db6cf03fa14
author Austin Schuh <austin.linux@gmail.com> Sat Jan 23 14:42:25 2021 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Jan 23 14:53:38 2021 -0800
tree 7a3471404bdeccd32fa8085e5d4fe018100a2fdc
parent 47a0ab34302242df7956ef576fcac95c8c5f46fc

Use enums for drivetrain KF states

This makes things a lot easier to follow.

Change-Id: Ib3d2b60fd8f77851cf842340447d92905b69cb75

frc971/control_loops/drivetrain/polydrivetrain.h

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index d6cd2b5..17a32b8 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -20,6 +20,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain_status_float_generated.h"
 #endif  // __linux__
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_states.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
 namespace frc971 {
@@ -294,8 +295,8 @@
 template <typename Scalar>
 void PolyDrivetrain<Scalar>::Update(Scalar voltage_battery) {
   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()(kLeftVelocity);
+    loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(kRightVelocity);
   }
 
   // TODO(austin): Observer for the current velocity instead of difference