Added more internal state logging to arm.
Change-Id: Ic4c07c74f740af4e5af32d2d140a76ba78f00ee5
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 3bc41cb..b8c674f 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -57,6 +57,9 @@
// Returns the current position.
double angle() const { return Y_(0, 0); }
+ // Returns the controller error.
+ const StateFeedbackLoop<3, 1, 1> &controller() const { return *loop_; }
+
// Returns the filtered goal.
const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }
@@ -159,6 +162,9 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+ // Returns the controller error.
+ const StateFeedbackLoop<6, 2, 2> &controller() const { return *loop_; }
+
// Returns the unprofiled goal.
const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
return unprofiled_goal_;