Fixed bug where kalman filter was being coppied rather than passed by pointer.
Change-Id: Ib2d0fa0ec2e5a0b19592ca419fec339252a3f698
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