Converted drivetrain to codegen coeffs at buildtime
Change-Id: I41a77751f71d2f990ac49b19c85bc166b10a563b
diff --git a/y2015_bot3/control_loops/drivetrain/BUILD b/y2015_bot3/control_loops/drivetrain/BUILD
index ad1ec39..330626a 100644
--- a/y2015_bot3/control_loops/drivetrain/BUILD
+++ b/y2015_bot3/control_loops/drivetrain/BUILD
@@ -39,6 +39,19 @@
],
)
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015_bot3/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2015_bot3/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ ],
+)
+
cc_library(
name = 'drivetrain_lib',
srcs = [
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.cc b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
index a3e7c24..81a4be5 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
@@ -143,7 +143,7 @@
DrivetrainMotorsSS()
: loop_(new LimitedDrivetrainLoop(
- ::y2015_bot3::control_loops::MakeDrivetrainLoop())),
+ ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop())),
filtered_offset_(0.0),
gyro_(0.0),
left_goal_(0.0),
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
deleted file mode 100644
index 25e728a..0000000
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(4);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients()));
- plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients()));
- plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients()));
- plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients()));
- return StateFeedbackPlant<4, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(4);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController()));
- controllers[1] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController()));
- controllers[2] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController()));
- controllers[3] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController()));
- return StateFeedbackLoop<4, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace y2015_bot3
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
deleted file mode 100644
index 041e06e..0000000
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController();
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace y2015_bot3
-
-#endif // Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
index 4e66302..a3c86e3 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -46,7 +46,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(drivetrain::MakeDrivetrainPlant())),
my_drivetrain_queue_(".y2015_bot3.control_loops.drivetrain", 0x8a8dde77,
".y2015_bot3.control_loops.drivetrain.goal",
".y2015_bot3.control_loops.drivetrain.position",