added support for both drivetrains with the same code
Previously, we were just switching back and forth manually, which
doesn't work very well. The python code for (both) drivetrain loops now
generates constants for both robots and frc971/constants knows which one
to give the actual drivetrain control loop code.
Various other cleanups to things that were so hard to read it made it
difficult to figure out how to do this too.
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index ea09d11..94c73d0 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -5,7 +5,6 @@
'type': 'static_library',
'sources': [
#'state_feedback_loop.h'
- #'StateFeedbackLoop.h'
],
'dependencies': [
'<(EXTERNALS):eigen',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 815f05e..2b16c0b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -11,8 +11,6 @@
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -76,8 +74,9 @@
class DrivetrainMotorsSS {
public:
- DrivetrainMotorsSS ()
- : loop_(new StateFeedbackLoop<4, 2, 2>(MakeDrivetrainLoop())) {
+ DrivetrainMotorsSS()
+ : loop_(new StateFeedbackLoop<4, 2, 2>(
+ constants::GetValues().make_drivetrain_loop())) {
_offset = 0;
_integral_offset = 0;
_left_goal = 0.0;
@@ -181,7 +180,8 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
- loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())),
+ loop_(new StateFeedbackLoop<2, 2, 2>(
+ constants::GetValues().make_v_drivetrain_loop())),
left_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
right_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
ttrust_(1.1),
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 2b98578..53bb16c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -18,12 +18,26 @@
'includes': ['../../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'polydrivetrain_plants',
+ 'type': 'static_library',
+ 'sources': [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_clutch_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'drivetrain_clutch_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
'target_name': 'drivetrain_lib',
'type': 'static_library',
'sources': [
'drivetrain.cc',
- 'drivetrain_motor_plant.cc',
- 'polydrivetrain_motor_plant.cc',
'polydrivetrain_cim_plant.cc',
],
'dependencies': [
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
new file mode 100644
index 0000000..b3aa088
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00876671940282, 0.0, 0.000204905465153, 0.0, 0.764245148008, 0.0, 0.0373841350548, 0.0, 0.000204905465153, 1.0, 0.00876671940282, 0.0, 0.0373841350548, 0.0, 0.764245148008;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000157874070659, -2.62302512161e-05, 0.0301793267864, -0.00478559834045, -2.62302512161e-05, 0.000157874070659, -0.00478559834045, 0.0301793267864;
+ 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> MakeClutchDrivetrainController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.60424514801, 0.0373841350548, 53.4463554671, 4.58647914599, 0.0373841350548, 1.60424514801, 4.58647914599, 53.4463554671;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 292.330461448, 10.4890095334, -85.5980253252, -0.517234397951, -58.0206391358, -1.5636023242, 153.384904309, 5.5616531565;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeClutchDrivetrainPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClutchDrivetrainPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClutchDrivetrainController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
new file mode 100644
index 0000000..e9444e6
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController();
+
+StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
new file mode 100644
index 0000000..7e514b5
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00927226867138, 0.0, 9.67653917682e-05, 0.0, 0.858140318975, 0.0, 0.0183781356125, 0.0, 9.67653917682e-05, 1.0, 0.00927226867138, 0.0, 0.0183781356125, 0.0, 0.858140318975;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000120661741455, -1.60442188265e-05, 0.0235210928559, -0.00304719305118, -1.60442188265e-05, 0.000120661741455, -0.00304719305118, 0.0235210928559;
+ 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> MakeDogDrivetrainController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.69814031898, 0.0183781356125, 65.3030659057, 2.40312922858, 0.0183781356125, 1.69814031898, 2.40312922858, 65.3030659057;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 257.282562369, 15.135374919, 33.3313439577, 2.7466676405, 33.3313439577, 2.7466676405, 257.282562369, 15.135374919;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDogDrivetrainPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDogDrivetrainController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
new file mode 100644
index 0000000..ba3d584
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDogDrivetrainController();
+
+StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 43b6443..0f8c87e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,7 +10,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -37,9 +37,10 @@
class DrivetrainSimulation {
public:
// Constructs a motor simulation.
+ // 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>(MakeDogDrivetrainPlant())),
my_drivetrain_loop_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
deleted file mode 100644
index 071b257..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00948696019317, 0.0, 3.55966215909e-06, 0.0, 0.899177606502, 0.0, 0.000686937184856, 0.0, 3.55966215909e-06, 1.0, 0.00948696019317, 0.0, 0.000686937184856, 0.0, 0.899177606502;
- Eigen::Matrix<double, 4, 2> B;
- B << 9.50723568824e-05, -6.59647588097e-07, 0.0186835844877, -0.000127297602107, -6.59647588097e-07, 9.50723568824e-05, -0.000127297602107, 0.0186835844877;
- 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> MakeDrivetrainController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.7391776065, 0.000686937184856, 70.7236123469, 0.0920942992696, 0.000686937184856, 1.7391776065, 0.0920942992696, 70.7236123469;
- Eigen::Matrix<double, 2, 4> K;
- K << 318.476158856, 20.8163224602, 2.1698861574, 0.178798182963, 2.1698861574, 0.178798182963, 318.476158856, 20.8163224602;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
deleted file mode 100644
index 2060b6f..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainController();
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
new file mode 100644
index 0000000..82962f0
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
@@ -0,0 +1,125 @@
+#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.764245148008, 0.0373841350548, 0.0373841350548, 0.764245148008;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0301793267864, -0.00478559834045, -0.00478559834045, 0.0301793267864;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.763446428918, 0.00494258902788, 0.042202491067, 0.968991856576;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0302815719967, -0.00184882243178, -0.00540240320973, 0.011598890947;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.968991856576, 0.042202491067, 0.00494258902788, 0.763446428918;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.011598890947, -0.00540240320973, -0.00184882243178, 0.0302815719967;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.968881997557, 0.00555499847336, 0.00555499847336, 0.968881997557;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0116399847578, -0.0020779000091, -0.0020779000091, 0.0116399847578;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.744245148008, 0.0373841350548, 0.0373841350548, 0.744245148008;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 5.78417881324, 2.15594244513, 2.15594244513, 5.78417881324;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.742469928763, 0.0421768815418, 0.0421768815418, 0.949968356732;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 5.78418649682, 2.16715237139, 6.33258809821, 32.8220766317;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.954934950673, 0.00591596315544, 0.00591596315544, 0.737503334821;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 32.8220766317, 6.33258809821, 2.16715237139, 5.78418649682;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.948881997557, 0.00555499847336, 0.00555499847336, 0.948881997557;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 32.8220767657, 6.33643373411, 6.33643373411, 32.8220767657;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
+ return StateFeedbackPlant<2, 2, 2>(plants);
+}
+
+StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighController());
+ return StateFeedbackLoop<2, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
new file mode 100644
index 0000000..85c87c1
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController();
+
+StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant();
+
+StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
new file mode 100644
index 0000000..d8421a2
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -0,0 +1,125 @@
+#include "frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.858140318975, 0.0183781356125, 0.0183781356125, 0.858140318975;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0235210928559, -0.00304719305118, -0.00304719305118, 0.0235210928559;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.857986344025, 0.00408284222985, 0.0195370380139, 0.968532564253;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0235466227252, -0.00148084354724, -0.00323934525959, 0.0114132132839;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.968532564253, 0.0195370380139, 0.00408284222985, 0.857986344025;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0114132132839, -0.00323934525959, -0.00148084354724, 0.0235466227252;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.968497658015, 0.0043351996773, 0.0043351996773, 0.968497658015;
+ Eigen::Matrix<double, 2, 2> B;
+ B << 0.0114258737482, -0.00157237338764, -0.00157237338764, 0.0114258737482;
+ Eigen::Matrix<double, 2, 2> C;
+ C << 1.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.838140318975, 0.0183781356125, 0.0183781356125, 0.838140318975;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 11.2651379288, 2.240762619, 2.240762619, 11.2651379288;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.838267905918, 0.0195319064993, 0.0195319064993, 0.94825100236;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 11.2651385254, 2.24416423621, 4.90910926672, 32.9269398272;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.950927164662, 0.00446324489791, 0.00446324489791, 0.835591743616;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 32.9269398272, 4.90910926672, 2.24416423621, 11.2651385254;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
+ Eigen::Matrix<double, 2, 2> L;
+ L << 0.948497658015, 0.0043351996773, 0.0043351996773, 0.948497658015;
+ Eigen::Matrix<double, 2, 2> K;
+ K << 32.9269398561, 4.91066546682, 4.91066546682, 32.9269398561;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ return StateFeedbackPlant<2, 2, 2>(plants);
+}
+
+StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighHighController());
+ return StateFeedbackLoop<2, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
new file mode 100644
index 0000000..613bff4
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController();
+
+StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant();
+
+StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
deleted file mode 100644
index 4b1a1ba..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.8793262727, 0.0205382709865, 0.0205382709865, 0.8793262727;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0223622719242, -0.00380598503859, -0.00380598503859, 0.0223622719242;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.879138263008, 0.00451814985125, 0.0216200529991, 0.97348145244;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0223971123485, -0.00183152094495, -0.00400645206708, 0.0107498150538;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.97348145244, 0.0216200529991, 0.00451814985125, 0.879138263008;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0107498150538, -0.00400645206708, -0.00183152094495, 0.0223971123485;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.973439382316, 0.00475228155545, 0.00475228155545, 0.973439382316;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0107668690064, -0.00192643083821, -0.00192643083821, 0.0107668690064;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 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<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.8593262727, 0.0205382709865, 0.0205382709865, 0.8593262727;
- Eigen::Matrix<double, 2, 2> K;
- K << 13.0245570096, 3.13517071687, 3.13517071687, 13.0245570096;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.859606856614, 0.0216072038404, 0.0216072038404, 0.953012858834;
- Eigen::Matrix<double, 2, 2> K;
- K << 13.0245575441, 3.13849145977, 6.86545006825, 35.9127730204;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.956890244313, 0.00522697730081, 0.00522697730081, 0.855729471134;
- Eigen::Matrix<double, 2, 2> K;
- K << 35.9127730204, 6.86545006825, 3.13849145977, 13.0245575441;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.953439382316, 0.00475228155545, 0.00475228155545, 0.953439382316;
- Eigen::Matrix<double, 2, 2> K;
- K << 35.9127730463, 6.86696893904, 6.86696893904, 35.9127730463;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h
deleted file mode 100644
index 99b113e..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c6a3f05..c39886a 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -50,8 +50,8 @@
class Drivetrain(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True):
- super(Drivetrain, self).__init__("Drivetrain")
+ def __init__(self, left_low=True, right_low=True, is_clutch=False):
+ super(Drivetrain, self).__init__(("Clutch" if is_clutch else "Dog" )+"Drivetrain")
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
@@ -62,7 +62,10 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ if is_clutch:
+ self.J = 4.5
+ else:
+ self.J = 4.9
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
@@ -77,8 +80,12 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 16.0 / 60.0 * 19.0 / 50.0
- self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+ if is_clutch:
+ self.G_low = 14.0 / 60.0 * 15.0 / 50.0
+ self.G_high = 30.0 / 44.0 * 15.0 / 50.0
+ else:
+ self.G_low = 16.0 / 60.0 * 17.0 / 50.0
+ self.G_high = 28.0 / 48.0 * 17.0 / 50.0
if left_low:
self.Gl = self.G_low
else:
@@ -200,14 +207,23 @@
#pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 3:
+ dog_drivetrain = Drivetrain(is_clutch=False)
+ clutch_drivetrain = Drivetrain(is_clutch=True)
+
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
- loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
+ dog_loop_writer = control_loop.ControlLoopWriter("DogDrivetrain", [dog_drivetrain])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ dog_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ clutch_loop_writer = control_loop.ControlLoopWriter("ClutchDrivetrain", [clutch_drivetrain])
+ if argv[3][-3:] == '.cc':
+ clutch_loop_writer.Write(argv[4], argv[3])
+ else:
+ clutch_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index f28cf42..5ffcff4 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -96,10 +96,11 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel", is_clutch=False):
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
- right_low=right_low)
+ right_low=right_low,
+ is_clutch=is_clutch)
self.dt = 0.01
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
@@ -137,12 +138,13 @@
SHIFTING_UP = 'up'
SHIFTING_DOWN = 'down'
- def __init__(self):
+ def __init__(self, is_clutch):
+ prefix = 'Clutch' if is_clutch else 'Dog'
self.drivetrain_low_low = VelocityDrivetrainModel(
- left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
- self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
- self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
- self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+ left_low=True, right_low=True, name=prefix+'VelocityDrivetrainLowLow', is_clutch=is_clutch)
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name=prefix+'VelocityDrivetrainLowHigh', is_clutch=is_clutch)
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = prefix+'VelocityDrivetrainHighLow', is_clutch=is_clutch)
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = prefix+'VelocityDrivetrainHighHigh', is_clutch=is_clutch)
# X is [lvel, rvel]
self.X = numpy.matrix(
@@ -390,29 +392,41 @@
def main(argv):
- vdrivetrain = VelocityDrivetrain()
+ dog_vdrivetrain = VelocityDrivetrain(False)
+ clutch_vdrivetrain = VelocityDrivetrain(True)
- if len(argv) != 5:
+ if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
- loop_writer = control_loop.ControlLoopWriter(
- "VDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VDogDrivetrain", [dog_vdrivetrain.drivetrain_low_low,
+ dog_vdrivetrain.drivetrain_low_high,
+ dog_vdrivetrain.drivetrain_high_low,
+ dog_vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ dog_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ clutch_loop_writer = control_loop.ControlLoopWriter(
+ "VClutchDrivetrain", [clutch_vdrivetrain.drivetrain_low_low,
+ clutch_vdrivetrain.drivetrain_low_high,
+ clutch_vdrivetrain.drivetrain_high_low,
+ clutch_vdrivetrain.drivetrain_high_high])
+
+ if argv[3][-3:] == '.cc':
+ clutch_loop_writer.Write(argv[4], argv[3])
+ else:
+ clutch_loop_writer.Write(argv[3], argv[4])
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
+ if argv[5][-3:] == '.cc':
+ cim_writer.Write(argv[6], argv[5])
else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[5], argv[6])
return
vl_plot = []
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 4c35419..38ca89c 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -5,9 +5,6 @@
#include <vector>
-// Stupid vxworks system headers define it which blows up Eigen...
-#undef m_data
-
#include "Eigen/Dense"
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -223,15 +220,14 @@
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
Eigen::Matrix<double, number_of_outputs, 1> Y;
- const StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>
- &controller() const {
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &controller() const {
return *controllers_[controller_index_];
}
- const StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>
- &controller(int index) const {
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &controller(
+ int index) const {
return *controllers_[index];
}
@@ -244,22 +240,17 @@
Y.setZero();
}
- StateFeedbackLoop(
- const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &controller)
+ StateFeedbackLoop(const StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs> &controller)
: controller_index_(0) {
- controllers_.push_back(
- new StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>(controller));
+ controllers_.push_back(new StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>(controller));
Reset();
}
- StateFeedbackLoop(
- const ::std::vector<StateFeedbackController<
- number_of_states, number_of_inputs,
- number_of_outputs> *> &controllers)
- : controllers_(controllers),
- controller_index_(0) {
+ StateFeedbackLoop(const ::std::vector<StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
+ : controllers_(controllers), controller_index_(0) {
Reset();
}
diff --git a/frc971/control_loops/update_drivetrain.sh b/frc971/control_loops/update_drivetrain.sh
index 9d17e99..bad1074 100755
--- a/frc971/control_loops/update_drivetrain.sh
+++ b/frc971/control_loops/update_drivetrain.sh
@@ -1,6 +1,10 @@
#!/bin/bash
#
-# Updates the drivetrain controller.
+# Updates the drivetrain controllers (for both robots).
-./python/drivetrain.py drivetrain/drivetrain_motor_plant.h \
- drivetrain/drivetrain_motor_plant.cc
+cd $(dirname $0)
+
+./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
+ drivetrain/drivetrain_dog_motor_plant.cc \
+ drivetrain/drivetrain_clutch_motor_plant.h \
+ drivetrain/drivetrain_clutch_motor_plant.cc
diff --git a/frc971/control_loops/update_polydrivetrain.sh b/frc971/control_loops/update_polydrivetrain.sh
index cd0d71e..2e2748e 100755
--- a/frc971/control_loops/update_polydrivetrain.sh
+++ b/frc971/control_loops/update_polydrivetrain.sh
@@ -1,8 +1,12 @@
#!/bin/bash
#
-# Updates the polydrivetrain controller and CIM models.
+# Updates the polydrivetrain controllers (for both robots) and CIM models.
-./python/polydrivetrain.py drivetrain/polydrivetrain_motor_plant.h \
- drivetrain/polydrivetrain_motor_plant.cc \
+cd $(dirname $0)
+
+./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
+ drivetrain/polydrivetrain_dog_motor_plant.cc \
+ drivetrain/polydrivetrain_clutch_motor_plant.h \
+ drivetrain/polydrivetrain_clutch_motor_plant.cc \
drivetrain/polydrivetrain_cim_plant.h \
drivetrain/polydrivetrain_cim_plant.cc