Added A_continuous and B_continuous to StateFeedbackPlantCoefficients
Change-Id: I0c2649e0ef4986c6b9122bf59adc8ad1d45572f4
diff --git a/y2015/control_loops/fridge/BUILD b/y2015/control_loops/fridge/BUILD
index a9bb737..dbecb75 100644
--- a/y2015/control_loops/fridge/BUILD
+++ b/y2015/control_loops/fridge/BUILD
@@ -75,6 +75,21 @@
],
)
+genrule(
+ name = 'genrule_arm_motor',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015/control_loops/python:arm) $(OUTS)',
+ tools = [
+ '//y2015/control_loops/python:arm',
+ ],
+ outs = [
+ 'arm_motor_plant.h',
+ 'arm_motor_plant.cc',
+ 'integral_arm_plant.h',
+ 'integral_arm_plant.cc',
+ ],
+)
+
cc_test(
name = 'fridge_lib_test',
srcs = [
diff --git a/y2015/control_loops/fridge/arm_motor_plant.cc b/y2015/control_loops/fridge/arm_motor_plant.cc
deleted file mode 100644
index 6e3205a..0000000
--- a/y2015/control_loops/fridge/arm_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/control_loops/fridge/arm_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, -0.18154390621, 0.919241022297;
- Eigen::Matrix<double, 4, 2> B;
- B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 1, 0, 1, 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> MakeArmController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.759390396955, -0.759390396955, 54.1048167043, -54.1048167043;
- Eigen::Matrix<double, 2, 4> K;
- K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.197397150694, 1.08682415753;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeArmPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeArmPlantCoefficients()));
- return StateFeedbackPlant<4, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeArmLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeArmController()));
- return StateFeedbackLoop<4, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2015/control_loops/fridge/arm_motor_plant.h b/y2015/control_loops/fridge/arm_motor_plant.h
deleted file mode 100644
index 3bf8d09..0000000
--- a/y2015/control_loops/fridge/arm_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
-#define Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeArmController();
-
-StateFeedbackPlant<4, 2, 2> MakeArmPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeArmLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 4c24a1f..518d41f 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -54,7 +54,7 @@
Fridge::Fridge(FridgeQueue *fridge)
: aos::controls::ControlLoop<FridgeQueue>(fridge),
arm_loop_(new CappedStateFeedbackLoop<5>(StateFeedbackLoop<5, 2, 2>(
- ::frc971::control_loops::MakeIntegralArmLoop()))),
+ MakeIntegralArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop<4>(
StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 6acc983..16a1f15 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -34,8 +34,7 @@
static constexpr double kNoiseScalar = 0.1;
// Constructs a simulation.
FridgeSimulation()
- : arm_plant_(new StateFeedbackPlant<4, 2, 2>(
- ::frc971::control_loops::MakeArmPlant())),
+ : arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
elevator_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
left_arm_pot_encoder_(
constants::GetValues().fridge.left_arm_zeroing.index_difference),
diff --git a/y2015/control_loops/fridge/integral_arm_plant.cc b/y2015/control_loops/fridge/integral_arm_plant.cc
deleted file mode 100644
index da269d7..0000000
--- a/y2015/control_loops/fridge/integral_arm_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/control_loops/fridge/integral_arm_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients() {
- Eigen::Matrix<double, 5, 5> A;
- A << 1.0, 0.00479642025454, 0.0, 0.0, 4.92993559969e-05, 0.0, 0.919688585028, 0.0, 0.0, 0.0194484035162, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, 0.0, -0.18154390621, 0.919241022297, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 5, 2> B;
- B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532, 0.0, 0.0;
- Eigen::Matrix<double, 2, 5> C;
- C << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.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<5, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<5, 2, 2> MakeIntegralArmController() {
- Eigen::Matrix<double, 5, 2> L;
- L << 0.461805946837, 0.461805946837, 5.83483983392, 5.83483983392, 0.429725467802, -0.429725467802, 0.18044816586, -0.18044816586, 31.0623964848, 31.0623964848;
- Eigen::Matrix<double, 2, 5> K;
- K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 1.0, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119, 1.0;
- Eigen::Matrix<double, 5, 5> A_inv;
- A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 5.21292341391e-05, 0.0, 1.08732457517, 0.0, 0.0, -0.0211467270909, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.0, 0.197397150694, 1.08682415753, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
- return StateFeedbackController<5, 2, 2>(L, K, A_inv, MakeIntegralArmPlantCoefficients());
-}
-
-StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>(new StateFeedbackPlantCoefficients<5, 2, 2>(MakeIntegralArmPlantCoefficients()));
- return StateFeedbackPlant<5, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<5, 2, 2>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<5, 2, 2>>(new StateFeedbackController<5, 2, 2>(MakeIntegralArmController()));
- return StateFeedbackLoop<5, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2015/control_loops/fridge/integral_arm_plant.h b/y2015/control_loops/fridge/integral_arm_plant.h
deleted file mode 100644
index 80a4876..0000000
--- a/y2015/control_loops/fridge/integral_arm_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
-#define Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients();
-
-StateFeedbackController<5, 2, 2> MakeIntegralArmController();
-
-StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant();
-
-StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_