Split out statespace drivetrain code.
Change-Id: I02900583abfab39b2d85473bd245ffd6bf421d58
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 97fe62c..fa29692 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -79,6 +79,28 @@
)
cc_library(
+ name = 'ssdrivetrain',
+ srcs = [
+ 'ssdrivetrain.cc',
+ ],
+ hdrs = [
+ 'ssdrivetrain.h',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ '//y2014:constants',
+ '//aos/common/controls:polytope',
+ '//aos/common:math',
+ '//aos/common/messages:robot_state',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:coerce_goal',
+ '//aos/common/util:log_interval',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_library(
name = 'polydrivetrain',
srcs = [
'polydrivetrain.cc',
@@ -111,11 +133,9 @@
deps = [
':drivetrain_queue',
':polydrivetrain',
+ ':ssdrivetrain',
'//aos/common/controls:control_loop',
'//y2014:constants',
- '//aos/common:math',
- '//frc971/control_loops:state_feedback_loop',
- '//frc971/control_loops:coerce_goal',
'//frc971/queues:gyro',
'//aos/common/util:log_interval',
'//aos/common/logging:queue_logging',
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index 9a3aff1..0fa1de9 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -7,17 +7,14 @@
#include "Eigen/Dense"
#include "aos/common/logging/logging.h"
-#include "aos/common/controls/polytope.h"
-#include "aos/common/commonmath.h"
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
#include "y2014/constants.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/coerce_goal.h"
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2014/control_loops/drivetrain/polydrivetrain.h"
+#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
@@ -30,225 +27,10 @@
namespace frc971 {
namespace control_loops {
-using ::y2014::control_loops::drivetrain::kDt;
-
-class DrivetrainMotorsSS {
- public:
- class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
- public:
- LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
- : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
- U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
- -1, 0,
- 0, 1,
- 0, -1).finished(),
- (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
- 12.0, 12.0).finished()) {
- ::aos::controls::HPolytope<0>::Init();
- T << 1, -1, 1, 1;
- T_inverse = T.inverse();
- }
-
- bool output_was_capped() const {
- return output_was_capped_;
- }
-
- private:
- virtual void CapU() {
- const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
-
- if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
- mutable_U() =
- U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
- LOG_MATRIX(DEBUG, "U is now", U());
- // TODO(Austin): Figure out why the polytope stuff wasn't working and
- // remove this hack.
- output_was_capped_ = true;
- return;
-
- LOG_MATRIX(DEBUG, "U at start", U());
- LOG_MATRIX(DEBUG, "R at start", R());
- LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
-
- Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 2),
- K(1, 0), K(1, 2);
- Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 1), K(0, 3),
- K(1, 1), K(1, 3);
-
- Eigen::Matrix<double, 2, 1> position_error;
- position_error << error(0, 0), error(2, 0);
- const auto drive_error = T_inverse * position_error;
- Eigen::Matrix<double, 2, 1> velocity_error;
- velocity_error << error(1, 0), error(3, 0);
- LOG_MATRIX(DEBUG, "error", error);
-
- const auto &poly = U_Poly_;
- const Eigen::Matrix<double, 4, 2> pos_poly_H =
- poly.H() * position_K * T;
- const Eigen::Matrix<double, 4, 1> pos_poly_k =
- poly.k() - poly.H() * velocity_K * velocity_error;
- const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
-
- Eigen::Matrix<double, 2, 1> adjusted_pos_error;
- {
- const auto &P = drive_error;
-
- Eigen::Matrix<double, 1, 2> L45;
- L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
- const double w45 = 0;
-
- Eigen::Matrix<double, 1, 2> LH;
- if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
- LH << 0, 1;
- } else {
- LH << 1, 0;
- }
- const double wh = LH.dot(P);
-
- Eigen::Matrix<double, 2, 2> standard;
- standard << L45, LH;
- Eigen::Matrix<double, 2, 1> W;
- W << w45, wh;
- const Eigen::Matrix<double, 2, 1> intersection =
- standard.inverse() * W;
-
- bool is_inside_h;
- const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
- const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
- if (pos_poly.IsInside(intersection)) {
- adjusted_pos_error = adjusted_pos_error_h;
- } else {
- if (is_inside_h) {
- if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
- adjusted_pos_error = adjusted_pos_error_h;
- } else {
- adjusted_pos_error = adjusted_pos_error_45;
- }
- } else {
- adjusted_pos_error = adjusted_pos_error_45;
- }
- }
- }
-
- LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
- mutable_U() =
- velocity_K * velocity_error + position_K * T * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U());
- } else {
- output_was_capped_ = false;
- }
- }
-
- const ::aos::controls::HPolytope<2> U_Poly_;
- Eigen::Matrix<double, 2, 2> T, T_inverse;
- bool output_was_capped_ = false;;
- };
-
- DrivetrainMotorsSS()
- : loop_(new LimitedDrivetrainLoop(
- constants::GetValues().make_drivetrain_loop())),
- filtered_offset_(0.0),
- gyro_(0.0),
- left_goal_(0.0),
- right_goal_(0.0),
- raw_left_(0.0),
- raw_right_(0.0) {
- // High gear on both.
- loop_->set_controller_index(3);
- }
-
- void SetGoal(double left, double left_velocity, double right,
- double right_velocity) {
- left_goal_ = left;
- right_goal_ = right;
- loop_->mutable_R() << left, left_velocity, right, right_velocity;
- }
- void SetRawPosition(double left, double right) {
- raw_right_ = right;
- raw_left_ = left;
- Eigen::Matrix<double, 2, 1> Y;
- Y << left + filtered_offset_, right - filtered_offset_;
- loop_->Correct(Y);
- }
- void SetPosition(double left, double right, double gyro) {
- // Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * constants::GetValues().turn_width) / 2.0;
- filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
- gyro_ = gyro;
- SetRawPosition(left, right);
- }
-
- void SetExternalMotors(double left_voltage, double right_voltage) {
- loop_->mutable_U() << left_voltage, right_voltage;
- }
-
- void Update(bool stop_motors, bool enable_control_loop) {
- if (enable_control_loop) {
- loop_->Update(stop_motors);
- } else {
- if (stop_motors) {
- loop_->mutable_U().setZero();
- loop_->mutable_U_uncapped().setZero();
- }
- loop_->UpdateObserver();
- }
- ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
- LOG_MATRIX(DEBUG, "E", E);
- }
-
- double GetEstimatedRobotSpeed() const {
- // lets just call the average of left and right velocities close enough
- return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
- }
-
- double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0, 0);
- }
-
- double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2, 0);
- }
-
- bool OutputWasCapped() const {
- return loop_->output_was_capped();
- }
-
- void SendMotors(DrivetrainQueue::Output *output) const {
- if (output) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
- output->left_high = true;
- output->right_high = true;
- }
- }
-
- const LimitedDrivetrainLoop &loop() const { return *loop_; }
-
- private:
- ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
-
- double filtered_offset_;
- double gyro_;
- double left_goal_;
- double right_goal_;
- double raw_left_;
- double raw_right_;
-};
-
-
void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
const DrivetrainQueue::Position *position,
DrivetrainQueue::Output *output,
- DrivetrainQueue::Status * status) {
- // TODO(aschuh): These should be members of the class.
- static DrivetrainMotorsSS dt_closedloop;
- static PolyDrivetrain dt_openloop;
-
+ DrivetrainQueue::Status *status) {
bool bad_pos = false;
if (position == nullptr) {
LOG_INTERVAL(no_position_);
@@ -269,12 +51,12 @@
double left_goal = goal->left_goal;
double right_goal = goal->right_goal;
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
+ dt_closedloop_.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
#if HAVE_SHIFTERS
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ dt_openloop_.SetGoal(wheel, throttle, quickturn, highgear);
#else
- dt_openloop.SetGoal(wheel, throttle, quickturn, false);
+ dt_openloop_.SetGoal(wheel, throttle, quickturn, false);
#endif
}
@@ -283,25 +65,25 @@
const double right_encoder = position->right_encoder;
if (gyro_reading.FetchLatest()) {
LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
- dt_closedloop.SetPosition(left_encoder, right_encoder,
- gyro_reading->angle);
+ dt_closedloop_.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle);
} else {
- dt_closedloop.SetRawPosition(left_encoder, right_encoder);
+ dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
}
}
- dt_openloop.SetPosition(position);
- dt_openloop.Update();
+ dt_openloop_.SetPosition(position);
+ dt_openloop_.Update();
if (control_loop_driving) {
- dt_closedloop.Update(output == NULL, true);
- dt_closedloop.SendMotors(output);
+ dt_closedloop_.Update(output == NULL, true);
+ dt_closedloop_.SendMotors(output);
} else {
- dt_openloop.SendMotors(output);
+ dt_openloop_.SendMotors(output);
if (output) {
- dt_closedloop.SetExternalMotors(output->left_voltage,
- output->right_voltage);
+ dt_closedloop_.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
}
- dt_closedloop.Update(output == NULL, false);
+ dt_closedloop_.Update(output == NULL, false);
}
// set the output status of the control loop state
@@ -309,22 +91,22 @@
bool done = false;
if (goal) {
done = ((::std::abs(goal->left_goal -
- dt_closedloop.GetEstimatedLeftEncoder()) <
+ dt_closedloop_.GetEstimatedLeftEncoder()) <
constants::GetValues().drivetrain_done_distance) &&
(::std::abs(goal->right_goal -
- dt_closedloop.GetEstimatedRightEncoder()) <
+ dt_closedloop_.GetEstimatedRightEncoder()) <
constants::GetValues().drivetrain_done_distance));
}
status->is_done = done;
- status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->robot_speed = dt_closedloop_.GetEstimatedRobotSpeed();
+ status->filtered_left_position = dt_closedloop_.GetEstimatedLeftEncoder();
+ status->filtered_right_position = dt_closedloop_.GetEstimatedRightEncoder();
- status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
- status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
- status->output_was_capped = dt_closedloop.OutputWasCapped();
- status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
- status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
+ status->filtered_left_velocity = dt_closedloop_.loop().X_hat(1, 0);
+ status->filtered_right_velocity = dt_closedloop_.loop().X_hat(3, 0);
+ status->output_was_capped = dt_closedloop_.OutputWasCapped();
+ status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
+ status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
}
}
diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
index 87e6362..c5b731d 100644
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ b/y2014/control_loops/drivetrain/drivetrain.h
@@ -7,6 +7,8 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/controls/polytope.h"
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/control_loops/drivetrain/polydrivetrain.h"
+#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
#include "aos/common/util/log_interval.h"
namespace frc971 {
@@ -35,6 +37,9 @@
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
::aos::time::Time::InSeconds(0.25), WARNING, "no position");
+
+ PolyDrivetrain dt_openloop_;
+ DrivetrainMotorsSS dt_closedloop_;
};
} // namespace control_loops
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.cc b/y2014/control_loops/drivetrain/ssdrivetrain.cc
new file mode 100644
index 0000000..53cc2dd
--- /dev/null
+++ b/y2014/control_loops/drivetrain/ssdrivetrain.cc
@@ -0,0 +1,181 @@
+#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
+
+#include "aos/common/controls/polytope.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/logging/matrix_logging.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace control_loops {
+
+DrivetrainMotorsSS::LimitedDrivetrainLoop::LimitedDrivetrainLoop(
+ StateFeedbackLoop<4, 2, 2> &&loop)
+ : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
+ .finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+ T << 1, -1, 1, 1;
+ T_inverse = T.inverse();
+}
+
+void DrivetrainMotorsSS::LimitedDrivetrainLoop::CapU() {
+ const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
+
+ if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+ mutable_U() =
+ U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
+ LOG_MATRIX(DEBUG, "U is now", U());
+ // TODO(Austin): Figure out why the polytope stuff wasn't working and
+ // remove this hack.
+ output_was_capped_ = true;
+ return;
+
+ LOG_MATRIX(DEBUG, "U at start", U());
+ LOG_MATRIX(DEBUG, "R at start", R());
+ LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(2, 0);
+ const auto drive_error = T_inverse * position_error;
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(1, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
+
+ const auto &poly = U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K * T;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = drive_error;
+
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+ const double w45 = 0;
+
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
+
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ mutable_U() =
+ velocity_K * velocity_error + position_K * T * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
+ } else {
+ output_was_capped_ = false;
+ }
+}
+
+DrivetrainMotorsSS::DrivetrainMotorsSS()
+ : loop_(new LimitedDrivetrainLoop(
+ constants::GetValues().make_drivetrain_loop())),
+ filtered_offset_(0.0),
+ gyro_(0.0),
+ left_goal_(0.0),
+ right_goal_(0.0),
+ raw_left_(0.0),
+ raw_right_(0.0) {
+ // High gear on both.
+ loop_->set_controller_index(3);
+}
+
+void DrivetrainMotorsSS::SetGoal(double left, double left_velocity,
+ double right, double right_velocity) {
+ left_goal_ = left;
+ right_goal_ = right;
+ loop_->mutable_R() << left, left_velocity, right, right_velocity;
+}
+void DrivetrainMotorsSS::SetRawPosition(double left, double right) {
+ raw_right_ = right;
+ raw_left_ = left;
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left + filtered_offset_, right - filtered_offset_;
+ loop_->Correct(Y);
+}
+void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
+ // Decay the offset quickly because this gyro is great.
+ const double offset =
+ (right - left - gyro * constants::GetValues().turn_width) / 2.0;
+ filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
+ gyro_ = gyro;
+ SetRawPosition(left, right);
+}
+
+void DrivetrainMotorsSS::SetExternalMotors(double left_voltage,
+ double right_voltage) {
+ loop_->mutable_U() << left_voltage, right_voltage;
+}
+
+void DrivetrainMotorsSS::Update(bool stop_motors, bool enable_control_loop) {
+ if (enable_control_loop) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->mutable_U().setZero();
+ loop_->mutable_U_uncapped().setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
+ LOG_MATRIX(DEBUG, "E", E);
+}
+
+double DrivetrainMotorsSS::GetEstimatedRobotSpeed() const {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+}
+
+void DrivetrainMotorsSS::SendMotors(DrivetrainQueue::Output *output) const {
+ if (output) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ output->left_high = true;
+ output->right_high = true;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.h b/y2014/control_loops/drivetrain/ssdrivetrain.h
new file mode 100644
index 0000000..982c873
--- /dev/null
+++ b/y2014/control_loops/drivetrain/ssdrivetrain.h
@@ -0,0 +1,79 @@
+#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
+#define Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
+
+#include "aos/common/controls/polytope.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/logging/matrix_logging.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class DrivetrainMotorsSS {
+ public:
+ class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop);
+
+ bool output_was_capped() const {
+ return output_was_capped_;
+ }
+
+ private:
+ void CapU() override;
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
+ Eigen::Matrix<double, 2, 2> T, T_inverse;
+ bool output_was_capped_ = false;;
+ };
+
+ DrivetrainMotorsSS();
+
+ void SetGoal(double left, double left_velocity, double right,
+ double right_velocity);
+
+ void SetRawPosition(double left, double right);
+
+ void SetPosition(double left, double right, double gyro);
+
+ void SetExternalMotors(double left_voltage, double right_voltage);
+
+ void Update(bool stop_motors, bool enable_control_loop);
+
+ double GetEstimatedRobotSpeed() const;
+
+ double GetEstimatedLeftEncoder() const {
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() const {
+ return loop_->X_hat(2, 0);
+ }
+
+ bool OutputWasCapped() const {
+ return loop_->output_was_capped();
+ }
+
+ void SendMotors(DrivetrainQueue::Output *output) const;
+
+ const LimitedDrivetrainLoop &loop() const { return *loop_; }
+
+ private:
+ ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
+
+ double filtered_offset_;
+ double gyro_;
+ double left_goal_;
+ double right_goal_;
+ double raw_left_;
+ double raw_right_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_