Made KF data available to polydrivetrain.
Change-Id: I067d0d05b9fc32d6ad50fbd35a6e9ad07b63f20d
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index c538894..5717f36 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -33,7 +33,8 @@
::y2014::control_loops::DrivetrainQueue *my_drivetrain)
: aos::controls::ControlLoop<::y2014::control_loops::DrivetrainQueue>(
my_drivetrain),
- kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()) {
+ kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()),
+ dt_openloop_(&kf_) {
::aos::controls::HPolytope<0>::Init();
}
@@ -49,6 +50,16 @@
}
no_position_.Print();
+ kf_.set_controller_index(dt_openloop_.controller_index());
+
+ {
+ Eigen::Matrix<double, 3, 1> Y;
+ Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
+ kf_.Correct(Y);
+ integrated_kf_heading_ +=
+ kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
+ }
+
bool control_loop_driving = false;
if (goal) {
double wheel = goal->steering;
@@ -125,14 +136,6 @@
left_voltage *= scalar;
right_voltage *= scalar;
- kf_.set_controller_index(dt_openloop_.controller_index());
-
- Eigen::Matrix<double, 3, 1> Y;
- Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
- kf_.Correct(Y);
- integrated_kf_heading_ +=
- kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
-
// To validate, look at the following:
// Observed - dx/dt velocity for left, right.
diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
index 7ecc09f..489b4ca 100644
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ b/y2014/control_loops/drivetrain/drivetrain.h
@@ -38,9 +38,9 @@
double last_gyro_heading_ = 0.0;
double last_gyro_rate_ = 0.0;
+ StateFeedbackLoop<7, 2, 3> kf_;
PolyDrivetrain dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
- StateFeedbackLoop<7, 2, 3> kf_;
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.cc b/y2014/control_loops/drivetrain/polydrivetrain.cc
index 16ca10a..02d0658 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.cc
+++ b/y2014/control_loops/drivetrain/polydrivetrain.cc
@@ -23,8 +23,9 @@
using ::y2014::control_loops::CIMLogging;
using ::frc971::control_loops::CoerceGoal;
-PolyDrivetrain::PolyDrivetrain()
- : U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+PolyDrivetrain::PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf)
+ : kf_(kf),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
/*[*/ 0, -1 /*]]*/).finished(),
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.h b/y2014/control_loops/drivetrain/polydrivetrain.h
index d57ff4c..5cd7fff 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.h
+++ b/y2014/control_loops/drivetrain/polydrivetrain.h
@@ -32,7 +32,7 @@
// Torque constant
static constexpr double Kt = drivetrain::kT;
- PolyDrivetrain();
+ PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf);
int controller_index() const { return loop_->controller_index(); }
@@ -65,6 +65,7 @@
void SendMotors(::y2014::control_loops::DrivetrainQueue::Output *output);
private:
+ StateFeedbackLoop<7, 2, 3> *kf_;
const ::aos::controls::HPolytope<2> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;