Converted state feedback controller and plant over to being gain scheduled, and used them in the indexer. Also wrote the python to make it easy to design gain scheduled controllers.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 4053eae..72597c6 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -253,6 +253,11 @@
status->ready_to_intake = false;
+ // Set the controller to use to be the one designed for the current number of
+ // discs in the hopper. This is safe since the controller prevents the index
+ // from being set out of bounds and picks the closest controller.
+ wrist_loop_->set_controller_index(frisbees_.size());
+
// Compute a safe index position that we can use.
if (position) {
wrist_loop_->Y << position->index_position;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index 1172ee2..3e2e674 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -588,6 +588,7 @@
void Simulate() {
EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ index_plant_->set_plant_index(frisbees.size());
index_plant_->U << my_index_loop_.output->index_voltage;
index_plant_->Update();
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
index 51661fb..10fa60c 100644
--- a/frc971/control_loops/index/index_motor_plant.cc
+++ b/frc971/control_loops/index/index_motor_plant.cc
@@ -1,12 +1,29 @@
#include "frc971/control_loops/index/index_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00832470485812, 0.0, 0.68478614982;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.06201698456, 11.6687573378;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
-StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +36,116 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00901822957243, 0.0, 0.810292182273;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0363437103863, 7.02270693014;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00927953099869, 0.0, 0.859452713637;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0266707124098, 5.20285570613;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex0DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.58478614982, 48.4122215588;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.90251621122, 0.0460029989298;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex0DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.64731520998, 56.0569452572;
Eigen::Matrix<double, 1, 2> K;
- K << 2.40538224198, 0.0619371641882;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeIndexPlant());
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex1DiscPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex2DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71029218227, 64.1044007344;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 3.16117420545, 0.0947502706704;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex3DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.75945271364, 70.6153894746;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 4.26688750446, 0.137549804289;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex4DiscPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(5);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex0DiscPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex1DiscPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex2DiscPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex3DiscPlantCoefficients());
+ plants[4] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex4DiscPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(5);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeIndex0DiscController());
+ controllers[1] = new StateFeedbackController<2, 1, 1>(MakeIndex1DiscController());
+ controllers[2] = new StateFeedbackController<2, 1, 1>(MakeIndex2DiscController());
+ controllers[3] = new StateFeedbackController<2, 1, 1>(MakeIndex3DiscController());
+ controllers[4] = new StateFeedbackController<2, 1, 1>(MakeIndex4DiscController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
index 4600ccf..e82db6a 100644
--- a/frc971/control_loops/index/index_motor_plant.h
+++ b/frc971/control_loops/index/index_motor_plant.h
@@ -6,11 +6,31 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex0DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController();
+
StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
index 9333f6a..0852b26 100644
--- a/frc971/control_loops/index/transfer_motor_plant.cc
+++ b/frc971/control_loops/index/transfer_motor_plant.cc
@@ -1,12 +1,13 @@
#include "frc971/control_loops/index/transfer_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-
-StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +20,28 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+StateFeedbackController<2, 1, 1> MakeTransferController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.64731520998, 56.0569452572;
Eigen::Matrix<double, 1, 2> K;
K << 1.06905877421, 0.0368709177253;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeTransferPlant());
+ return StateFeedbackController<2, 1, 1>(L, K, MakeTransferPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeTransferPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeTransferController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
index 565e8c7..596f9b3 100644
--- a/frc971/control_loops/index/transfer_motor_plant.h
+++ b/frc971/control_loops/index/transfer_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeTransferController();
+
StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_