Moved AngularLinearToLeftRight to DrivetrainConfig
This makes it so the battlebot doesn't need SSDrivetrain. The
conversion functions also fit better with the drivetrain config
object.
Change-Id: Ife4e0ac0c0ebf2f15f3ce1012385579d9ca8eaa8
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 651976e..7bbba74 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -205,14 +205,14 @@
status->robot_speed = (kf_.X_hat(1, 0) + kf_.X_hat(3, 0)) / 2.0;
Eigen::Matrix<double, 2, 1> linear =
- dt_closedloop_.LeftRightToLinear(kf_.X_hat());
+ dt_config_.LeftRightToLinear(kf_.X_hat());
Eigen::Matrix<double, 2, 1> angular =
- dt_closedloop_.LeftRightToAngular(kf_.X_hat());
+ dt_config_.LeftRightToAngular(kf_.X_hat());
angular(0, 0) = integrated_kf_heading_;
Eigen::Matrix<double, 4, 1> gyro_left_right =
- dt_closedloop_.AngularLinearToLeftRight(linear, angular);
+ dt_config_.AngularLinearToLeftRight(linear, angular);
status->estimated_left_position = gyro_left_right(0, 0);
status->estimated_right_position = gyro_left_right(2, 0);