Convert kError states to voltage
When we are in force mode, the state can't be use directly as a voltage.
Instead, scale it by the controller gains which do just that already.
Change-Id: I551bc6a3839138bef7db7058097b7feb03735ee1
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index edde3c7..7b7fbca 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -300,8 +300,12 @@
// TODO(james): The regular Kalman Filter's voltage error terms are
// currently unusable--either don't use voltage error at all for the spline
// following code, or use the EKF's voltage error estimates.
+ static_assert(kLeftError + 1 == kRightError);
+ Eigen::Matrix<double, 2, 2> error_K;
+ error_K << kf_.controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
+ kf_.controller().K(kRightVoltage, kRightError);
const Eigen::Matrix<double, 2, 1> voltage_error =
- 0 * kf_.X_hat().block<2, 1>(kLeftError, 0);
+ 0 * error_K * kf_.X_hat().block<2, 1>(kLeftError, 0);
dt_spline_.Update(
output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
trajectory_state, voltage_error);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 3eacef1..3e58d09 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -72,6 +72,10 @@
kf_->controller().K(kRightVoltage, kLeftVelocity),
kf_->controller().K(kRightVoltage, kRightVelocity);
+ Eigen::Matrix<double, 2, 2> error_K;
+ error_K << kf_->controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
+ kf_->controller().K(kRightVoltage, kRightError);
+
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(kLeftPosition), error(kRightPosition);
// drive_error = [total_distance_error, left_error - right_error]
@@ -79,8 +83,9 @@
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(kLeftVelocity), error(kRightVelocity);
- Eigen::Matrix<double, 2, 1> U_integral;
- U_integral << kf_->X_hat(kLeftError), kf_->X_hat(kRightError);
+ Eigen::Matrix<double, 2, 1> U_integral =
+ error_K * Eigen::Matrix<double, 2, 1>(kf_->X_hat(kLeftError),
+ kf_->X_hat(kRightError));
const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
U_poly_.static_H() * position_K * T_,