Merge "Add test to validate voltage saturation behaviour"
diff --git a/aos/common/util/BUILD b/aos/common/util/BUILD
index b62e93c..4e0455b 100644
--- a/aos/common/util/BUILD
+++ b/aos/common/util/BUILD
@@ -188,6 +188,24 @@
)
cc_library(
+ name = 'global_factory',
+ hdrs = [
+ 'global_factory.h'
+ ],
+)
+
+cc_test(
+ name = 'global_factory_test',
+ srcs = [
+ 'global_factory_test.cc'
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':global_factory'
+ ],
+)
+
+cc_library(
name = 'linked_list',
hdrs = [
'linked_list.h',
diff --git a/aos/common/util/global_factory.h b/aos/common/util/global_factory.h
new file mode 100644
index 0000000..148fc1e
--- /dev/null
+++ b/aos/common/util/global_factory.h
@@ -0,0 +1,88 @@
+#ifndef AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
+#define AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// // File Usage Description:
+// class ExampleBaseClass { virtual ~ExampleBaseClass(); }
+// class ExampleSubClass : public ExampleBaseClass {}
+//
+// // At namespace scope in header file:
+// SETUP_FACTORY(ExampleBaseClass);
+// // At namespace scope in cc file:
+// REGISTER_SUBCLASS("ExampleSubClass", ExampleBaseClass, ExampleSubClass);
+//
+// // When you want an object of type "ExampleSubClass".
+// std::unique_ptr<ExampleBaseClass> constructed_item =
+// ExampleBaseClassGlobalFactory::Get("ExampleSubClass")();
+
+// Helper macro to set up a Factory Family for a particular type.
+// Put this is the header file along-side the base class.
+#define SETUP_FACTORY(BaseClass, ...) \
+ using BaseClass##GlobalFactory = \
+ ::aos::GlobalFactory<BaseClass, ##__VA_ARGS__>
+
+// Helper macro to set up a Factory for a subtype. For BaseClass
+// This should happen in a .cc file not a header file to avoid multiple
+// linkage.
+#define REGISTER_SUBCLASS_BY_KEY(key, BaseClass, SubClass) \
+ BaseClass##GlobalFactory::SubClassRegisterer<SubClass> \
+ register_for_##SubClass(key)
+
+// Proxy to above but where SubClass name is the key.
+#define REGISTER_SUBCLASS(BaseClass, SubClass) \
+ REGISTER_SUBCLASS_BY_KEY(#SubClass, BaseClass, SubClass)
+
+namespace aos {
+
+// Maintains a static std::unordered_map<std::string, FactoryFunction> for
+// BaseClass. Best to use with macros above.
+template <typename BaseClass, typename... FactoryArgs>
+class GlobalFactory {
+ public:
+ using FactoryFunction =
+ std::function<std::unique_ptr<BaseClass>(FactoryArgs&&...)>;
+
+ // Gets the factory function by named. This will return a null factory
+ // std::function if the factory is not available, so one would be wise
+ // to check this function before use.
+ // It is an error to call this during static initialization.
+ static const FactoryFunction &Get(const std::string &name) {
+ const auto &map = *GetMap();
+ auto item = map.find(name);
+ if (item == map.end()) {
+ static FactoryFunction null_create_fn;
+ return null_create_fn;
+ }
+ return item->second;
+ }
+
+ // Installs a factory function for constructing SubClass
+ // using name "name". It is an error not call this at namespace scope
+ // through the REGISTER_SUBCLASS macro above.
+ template <typename SubClass>
+ class SubClassRegisterer {
+ public:
+ explicit SubClassRegisterer(const char *name) {
+ (*GetMap())[name] = [](FactoryArgs&&... args) {
+ return std::unique_ptr<BaseClass>(
+ new SubClass(std::forward<FactoryArgs>(args)...));
+ };
+ }
+ };
+
+ private:
+ // Actual map. (Protected by static from concurrent construction
+ // if there is nothing registered at static linkage time).
+ static std::unordered_map<std::string, FactoryFunction> *GetMap() {
+ static std::unordered_map<std::string, FactoryFunction> map;
+ return ↦
+ }
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
diff --git a/aos/common/util/global_factory_test.cc b/aos/common/util/global_factory_test.cc
new file mode 100644
index 0000000..8da7dcb
--- /dev/null
+++ b/aos/common/util/global_factory_test.cc
@@ -0,0 +1,67 @@
+#include "aos/common/util/global_factory.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+
+namespace test_a {
+class BaseType {
+ public:
+ virtual ~BaseType() {}
+
+ virtual std::pair<int, int> Get() = 0;
+};
+
+SETUP_FACTORY(BaseType, int, int);
+
+class BaseTypeNoArgs {
+ public:
+ virtual ~BaseTypeNoArgs() {}
+
+ virtual int Get() = 0;
+};
+
+SETUP_FACTORY(BaseTypeNoArgs);
+
+} // namespace test_a
+
+namespace test_b {
+
+class SubType : public test_a::BaseType {
+ public:
+ SubType(int t1, int t2) : value_(t1, t2) {}
+ std::pair<int, int> Get() override { return value_; }
+
+ private:
+ std::pair<int, int> value_;
+};
+
+REGISTER_SUBCLASS(test_a::BaseType, SubType);
+
+} // namespace test_b
+
+namespace {
+
+class SubType1 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 1; }
+};
+
+class SubType2 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 2; }
+};
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType1);
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType2);
+
+TEST(GlobalFactoryTest, CheckFactory) {
+ auto val = test_a::BaseTypeGlobalFactory::Get("SubType")(2, 7)->Get();
+ EXPECT_EQ(val.first, 2);
+ EXPECT_EQ(val.second, 7);
+}
+TEST(GlobalFactoryTest, CheckFactoryNoArgs) {
+ EXPECT_EQ(1, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType1")()->Get());
+ EXPECT_EQ(2, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType2")()->Get());
+}
+
+} // namespace
+} // namespace aos
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index d6bd85f..bb34b85 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -276,15 +276,17 @@
num_states, num_inputs, num_outputs, self._name)]
ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
- num_outputs))
+ '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
+ num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index e535ae7..a81f2cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -26,7 +26,9 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A_(other.A()),
+ A_continuous_(other.A_continuous()),
B_(other.B()),
+ B_continuous_(other.B_continuous()),
C_(other.C()),
D_(other.D()),
U_min_(other.U_min()),
@@ -34,21 +36,41 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &A_continuous,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ &B_continuous,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A_(A), B_(B), C_(C), D_(D), U_min_(U_min), U_max_(U_max) {}
+ : A_(A),
+ A_continuous_(A_continuous),
+ B_(B),
+ B_continuous_(B_continuous),
+ C_(C),
+ D_(D),
+ U_min_(U_min),
+ U_max_(U_max) {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
return A_;
}
double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_continuous()
+ const {
+ return A_continuous_;
+ }
+ double A_continuous(int i, int j) const { return A_continuous()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
return B_;
}
double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B_continuous() const {
+ return B_continuous_;
+ }
+ double B_continuous(int i, int j) const { return B_continuous()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
return C_;
}
@@ -68,7 +90,9 @@
private:
const Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous_;
const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous_;
const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 6f97bc7..a08841e 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -14,6 +14,8 @@
// compile or have assertion failures at runtime.
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index a4767d1..6a6bb3e 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -108,6 +108,16 @@
A_unaugmented = self.A
B_unaugmented = self.B
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
+
self.A = numpy.matrix([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
@@ -147,6 +157,16 @@
A_unaugmented = self.A
B_unaugmented = self.B
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
+
self.A = numpy.matrix([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
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_
diff --git a/y2015/control_loops/python/arm.py b/y2015/control_loops/python/arm.py
index 9fa270e..d3d77c5 100755
--- a/y2015/control_loops/python/arm.py
+++ b/y2015/control_loops/python/arm.py
@@ -1,18 +1,22 @@
#!/usr/bin/python
-import control_loop
-import controls
-import polytope
-import polydrivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import math
import sys
-import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", mass=None):
+ def __init__(self, name='Arm', mass=None):
super(Arm, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -63,12 +67,12 @@
[0, 0, 0, 1],
[0, 0, -self.C1 * 2.0, -self.C3]])
- print 'Full speed is', self.C2 / self.C3 * 12.0
+ glog.debug('Full speed is %f', self.C2 / self.C3 * 12.0)
- print 'Stall arm difference is', 12.0 * self.C2 / self.C1
- print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
+ glog.debug('Stall arm difference is %f', 12.0 * self.C2 / self.C1)
+ glog.debug('Stall arm difference first principles is %f', self.stall_torque * self.G / self.spring)
- print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
+ glog.debug('5 degrees of arm error is %f', self.spring / self.r * (math.pi * 5.0 / 180.0))
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -86,8 +90,8 @@
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
- print 'Rank of augmented controllability matrix.', numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix. %d', numpy.linalg.matrix_rank(
+ controllability))
q_pos = 0.02
q_vel = 0.300
@@ -101,11 +105,10 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'Controller'
- print self.K
+ glog.debug('Controller\n %s', repr(self.K))
- print 'Controller Poles'
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('Controller Poles\n %s',
+ numpy.linalg.eig(self.A - self.B * self.K)[0])
self.rpl = 0.20
self.ipl = 0.05
@@ -119,13 +122,14 @@
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
- print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
+ glog.debug('Observer (Converted to a KF):\n%s',
+ repr(numpy.linalg.inv(self.A) * self.L))
self.InitializeState()
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", mass=None):
+ def __init__(self, name='IntegralArm', mass=None):
super(IntegralArm, self).__init__(name=name, mass=mass)
self.A_continuous_unaugmented = self.A_continuous
@@ -144,9 +148,9 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print 'A cont', self.A_continuous
- print 'B cont', self.B_continuous
- print 'A discrete', self.A
+ glog.debug('A cont: %s', repr(self.A_continuous))
+ glog.debug('B cont %s', repr(self.B_continuous))
+ glog.debug('A discrete %s', repr(self.A))
q_pos = 0.08
q_vel = 0.40
@@ -175,7 +179,7 @@
self.K[0:2, 0:4] = self.K_unaugmented
self.K[0, 4] = 1
self.K[1, 4] = 1
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
self.InitializeState()
@@ -264,10 +268,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(arm.A)
- print "delta time is ", arm.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(arm.A)))
+ glog.debug('delta time is %f', arm.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -350,7 +354,7 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print 'End is', observer_arm.X_hat[4, 0]
+ glog.debug('End is %f', observer_arm.X_hat[4, 0])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -370,40 +374,40 @@
def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- arm = Arm(mass=13 + loaded_mass)
- #arm_controller = Arm(mass=13 + 15)
- #observer_arm = Arm(mass=13 + 15)
- #observer_arm = None
+ if FLAGS.plot:
+ loaded_mass = 25
+ #loaded_mass = 0
+ arm = Arm(mass=13 + loaded_mass)
+ #arm_controller = Arm(mass=13 + 15)
+ #observer_arm = Arm(mass=13 + 15)
+ #observer_arm = None
- integral_arm = IntegralArm(mass=13 + loaded_mass)
- integral_arm.X_hat[0, 0] += 0.02
- integral_arm.X_hat[2, 0] += 0.02
- integral_arm.X_hat[4] = 0
+ integral_arm = IntegralArm(mass=13 + loaded_mass)
+ integral_arm.X_hat[0, 0] += 0.02
+ integral_arm.X_hat[2, 0] += 0.02
+ integral_arm.X_hat[4] = 0
- # Test moving the arm with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
+ # Test moving the arm with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
# Write the generated constants out to a file.
if len(argv) != 5:
- print "Expected .h file name and .cc file name for the arm and augmented arm."
+ glog.fatal('Expected .h file name and .cc file name for the arm and augmented arm.')
else:
- arm = Arm("Arm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ namespaces = ['y2015', 'control_loops', 'fridge']
+ arm = Arm('Arm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('Arm', [arm],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_arm = IntegralArm("IntegralArm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
- if argv[3][-3:] == '.cc':
- loop_writer.Write(argv[4], argv[3])
- else:
- loop_writer.Write(argv[3], argv[4])
+ integral_arm = IntegralArm('IntegralArm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('IntegralArm', [integral_arm],
+ namespaces=namespaces)
+ loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/claw.py b/y2015/control_loops/python/claw.py
index 86a261d..9987089 100755
--- a/y2015/control_loops/python/claw.py
+++ b/y2015/control_loops/python/claw.py
@@ -18,7 +18,7 @@
pass
class Claw(control_loop.ControlLoop):
- def __init__(self, name="Claw", mass=None):
+ def __init__(self, name='Claw', mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -74,7 +74,7 @@
controllability = controls.ctrb(self.A, self.B)
- print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+ glog.debug('Free speed is %f', self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.15
q_vel = 2.5
@@ -84,15 +84,15 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'K', self.K
- print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('K: %s', repr(self.K))
+ glog.debug('Poles are: %s', repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- print 'L is', self.L
+ glog.debug('L is: %s', repr(self.L))
q_pos = 0.05
q_vel = 2.65
@@ -105,9 +105,9 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
- print 'KalL is', self.L
+ glog.debug('KalL is: %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -118,7 +118,7 @@
def run_test(claw, initial_X, goal, max_separation_error=0.01,
- show_graph=False, iterations=200, controller_claw=None,
+ iterations=200, controller_claw=None,
observer_claw=None):
"""Runs the claw plant with an initial condition and goal.
@@ -131,8 +131,6 @@
claw: claw object to use.
initial_X: starting state.
goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
iterations: Number of timesteps to run the model for.
controller_claw: claw object to get K from, or None if we should
use claw.
@@ -177,32 +175,32 @@
t.append(i * claw.dt)
u.append(U[0, 0])
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x, label='x')
- if observer_claw is not None:
- pylab.plot(t, x_hat, label='x_hat')
- pylab.legend()
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x, label='x')
+ if observer_claw is not None:
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u, label='u')
- pylab.legend()
- pylab.show()
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u, label='u')
+ pylab.legend()
+ pylab.show()
def main(argv):
- loaded_mass = 0
- #loaded_mass = 0
- claw = Claw(mass=4 + loaded_mass)
- claw_controller = Claw(mass=5 + 0)
- observer_claw = Claw(mass=5 + 0)
- #observer_claw = None
+ if FLAGS.plot:
+ loaded_mass = 0
+ #loaded_mass = 0
+ claw = Claw(mass=4 + loaded_mass)
+ claw_controller = Claw(mass=5 + 0)
+ observer_claw = Claw(mass=5 + 0)
+ #observer_claw = None
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0]])
- run_test(claw, initial_X, R, controller_claw=claw_controller,
- observer_claw=observer_claw)
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ run_test(claw, initial_X, R, controller_claw=claw_controller,
+ observer_claw=observer_claw)
# Write the generated constants out to a file.
if len(argv) != 3:
@@ -215,4 +213,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/elevator.py b/y2015/control_loops/python/elevator.py
index dc9caa1..d522034 100755
--- a/y2015/control_loops/python/elevator.py
+++ b/y2015/control_loops/python/elevator.py
@@ -8,6 +8,14 @@
import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
class Elevator(control_loop.ControlLoop):
def __init__(self, name="Elevator", mass=None):
super(Elevator, self).__init__(name)
@@ -57,7 +65,7 @@
[0, 0, 0, 1],
[0, 0, -C1 * 2.0, -C3]])
- print "Full speed is", C2 / C3 * 12.0
+ glog.debug('Full speed is', C2 / C3 * 12.0)
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -74,11 +82,11 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print self.A
+ glog.debug(repr(self.A))
controllability = controls.ctrb(self.A, self.B)
- print "Rank of augmented controllability matrix.", numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix: %d',
+ numpy.linalg.matrix_rank(controllability))
q_pos = 0.02
q_vel = 0.400
@@ -92,9 +100,9 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
+ glog.debug(repr(self.K))
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug(repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.20
self.ipl = 0.05
@@ -194,10 +202,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(elevator.A)))
+ glog.debug('delta time is %f', elevator.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -232,7 +240,7 @@
# Write the generated constants out to a file.
if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
+ glog.fatal('Expected .h file name and .cc file name for the elevator.')
else:
namespaces = ['y2015', 'control_loops', 'fridge']
elevator = Elevator("Elevator")
@@ -244,4 +252,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015_bot3/control_loops/python/elevator3.py b/y2015_bot3/control_loops/python/elevator3.py
index 4b15bd0..e67ed47 100755
--- a/y2015_bot3/control_loops/python/elevator3.py
+++ b/y2015_bot3/control_loops/python/elevator3.py
@@ -80,8 +80,8 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- glog.info('K %s', str(self.K))
- glog.info('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', str(self.K))
+ glog.debug('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
@@ -100,7 +100,7 @@
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
- glog.info('KalL is %s', str(self.L))
+ glog.debug('KalL is %s', str(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -237,8 +237,6 @@
def main(argv):
- argv = FLAGS(argv)
-
loaded_mass = 7+4.0
#loaded_mass = 0
#observer_elevator = None
@@ -255,7 +253,7 @@
for i in xrange(0, 7):
elevator = Elevator(mass=i*totemass + loaded_mass)
- glog.info('Actual poles are %s', str(numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]))
+ glog.debug('Actual poles are %s', str(numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]))
elevator.X = initial_X
scenario_plotter.run_test(elevator, goal=up_R, controller_elevator=elevator_controller,
@@ -282,4 +280,6 @@
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 3e71a0f..8684c98 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -112,7 +112,7 @@
glog.debug('K %s', repr(self.K))
glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
q_pos = 0.10
q_vel = 1.65
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 01d9797..6b1d59a 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -13,6 +13,8 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+gflags.DEFINE_bool('stall', False, 'If true, stall the indexer.')
+
class VelocityIndexer(control_loop.ControlLoop):
def __init__(self, name='VelocityIndexer'):
super(VelocityIndexer, self).__init__(name)
@@ -74,8 +76,11 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.82])
- glog.debug(repr(self.K))
+ self.PlaceControllerPoles([.80])
+ glog.debug('K: %s', repr(self.K))
+
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.PlaceObserverPoles([0.3])
@@ -127,7 +132,7 @@
class IntegralIndexer(Indexer):
- def __init__(self, name="IntegralIndexer"):
+ def __init__(self, name="IntegralIndexer", voltage_error_noise=None):
super(IntegralIndexer, self).__init__(name=name)
self.A_continuous_unaugmented = self.A_continuous
@@ -147,9 +152,12 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 2.0
- q_vel = 0.001
- q_voltage = 10.0
+ q_pos = 0.01
+ q_vel = 2.0
+ q_voltage = 0.4
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
+
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
@@ -179,6 +187,7 @@
self.x = []
self.v = []
self.a = []
+ self.stall_ratio = []
self.x_hat = []
self.u = []
self.offset = []
@@ -212,7 +221,10 @@
if observer_indexer is not None:
X_hat = observer_indexer.X_hat
+ observer_indexer.Y = indexer.Y
+ observer_indexer.CorrectObserver(numpy.matrix([[0.0]]))
self.x_hat.append(observer_indexer.X_hat[1, 0])
+ self.offset.append(observer_indexer.X_hat[2, 0])
ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
@@ -220,7 +232,6 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(indexer.X[0, 0])
-
if self.v:
last_v = self.v[-1]
else:
@@ -229,17 +240,25 @@
self.v.append(indexer.X[1, 0])
self.a.append((self.v[-1] - last_v) / indexer.dt)
- if observer_indexer is not None:
- observer_indexer.Y = indexer.Y
- observer_indexer.CorrectObserver(U)
- self.offset.append(observer_indexer.X_hat[2, 0])
-
applied_U = U.copy()
- if i > 30:
- applied_U += 2
- indexer.Update(applied_U)
+ if i >= 40:
+ applied_U -= 2
+
+ if FLAGS.stall and i >= 40:
+ indexer.X[1, 0] = 0.0
+ else:
+ indexer.Update(applied_U)
if observer_indexer is not None:
+ clipped_u = U[0, 0]
+ clip_u_value = 3.0
+ if clipped_u < 0:
+ clipped_u = min(clipped_u, -clip_u_value)
+ else:
+ clipped_u = max(clipped_u, clip_u_value)
+
+ self.stall_ratio.append(10 * (-self.offset[-1] / clipped_u))
+
observer_indexer.PredictObserver(U)
self.t.append(initial_t + i * indexer.dt)
@@ -254,6 +273,10 @@
pylab.subplot(3, 1, 2)
pylab.plot(self.t, self.u, label='u')
pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.plot(self.t, self.stall_ratio, label='stall_ratio')
+ pylab.plot(self.t,
+ [10.0 if x > 6.0 else 0.0 for x in self.stall_ratio],
+ label='is_stalled')
pylab.legend()
pylab.subplot(3, 1, 3)
@@ -278,8 +301,22 @@
if FLAGS.plot:
scenario_plotter.Plot()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
+ scenario_plotter = ScenarioPlotter()
+
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer(voltage_error_noise=1.5)
+ observer_indexer = IntegralIndexer(voltage_error_noise=1.5)
+
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer, iterations=200)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
indexer = Indexer('Indexer')
@@ -296,6 +333,11 @@
'IntegralIndexer', [integral_indexer], namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
+ stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer', voltage_error_noise=1.5)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralIndexer', [stuck_integral_indexer], namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
+
if __name__ == '__main__':
argv = FLAGS(sys.argv)
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 25e4b24..6c0f2f3 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import scipy
import sys
from matplotlib import pylab
@@ -58,6 +59,10 @@
self.PlaceControllerPoles([.90])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
self.PlaceObserverPoles([0.3])
self.U_max = numpy.matrix([[12.0]])
@@ -128,9 +133,14 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 2.0
- q_vel = 0.001
- q_voltage = 10.0
+ glog.debug('A: \n%s', repr(self.A_continuous))
+ glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
+ glog.debug('schur(A): \n%s', repr(scipy.linalg.schur(self.A_continuous)))
+ glog.debug('A_dt(A): \n%s', repr(self.A))
+
+ q_pos = 0.01
+ q_vel = 2.0
+ q_voltage = 0.2
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
@@ -226,8 +236,6 @@
self.t.append(initial_t + i * shooter.dt)
self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
-
def Plot(self):
pylab.subplot(3, 1, 1)
pylab.plot(self.t, self.v, label='x')
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 6c8291d..ebcc164 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -26,9 +26,10 @@
':superstructure_queue',
'//aos/common/controls:control_loop',
'//y2017/control_loops/superstructure/hood',
- '//y2017/control_loops/superstructure/turret',
+ '//y2017/control_loops/superstructure/indexer',
'//y2017/control_loops/superstructure/intake',
'//y2017/control_loops/superstructure/shooter',
+ '//y2017/control_loops/superstructure/turret',
'//y2017:constants',
],
)
@@ -49,6 +50,7 @@
'//frc971/control_loops:position_sensor_sim',
'//frc971/control_loops:team_number_test_environment',
'//y2017/control_loops/superstructure/hood:hood_plants',
+ '//y2017/control_loops/superstructure/indexer:indexer_plants',
'//y2017/control_loops/superstructure/intake:intake_plants',
'//y2017/control_loops/superstructure/shooter:shooter_plants',
'//y2017/control_loops/superstructure/turret:turret_plants',
diff --git a/y2017/control_loops/superstructure/indexer/BUILD b/y2017/control_loops/superstructure/indexer/BUILD
index 3df0810..10b726f 100644
--- a/y2017/control_loops/superstructure/indexer/BUILD
+++ b/y2017/control_loops/superstructure/indexer/BUILD
@@ -9,6 +9,8 @@
'indexer_plant.cc',
'indexer_integral_plant.h',
'indexer_integral_plant.cc',
+ 'stuck_indexer_integral_plant.h',
+ 'stuck_indexer_integral_plant.cc',
],
)
@@ -18,12 +20,31 @@
srcs = [
'indexer_plant.cc',
'indexer_integral_plant.cc',
+ 'stuck_indexer_integral_plant.cc',
],
hdrs = [
'indexer_plant.h',
'indexer_integral_plant.h',
+ 'stuck_indexer_integral_plant.h',
],
deps = [
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'indexer',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'indexer.cc',
+ ],
+ hdrs = [
+ 'indexer.h',
+ ],
+ deps = [
+ ':indexer_plants',
+ '//aos/common/controls:control_loop',
+ '//aos/common:math',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
new file mode 100644
index 0000000..cab9864
--- /dev/null
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -0,0 +1,206 @@
+#include "y2017/control_loops/superstructure/indexer/indexer.h"
+
+#include <chrono>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/indexer/stuck_indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace indexer {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+namespace {
+constexpr double kTolerance = 10.0;
+constexpr double kMinStuckVoltage = 3.0;
+constexpr chrono::milliseconds kForwardTimeout{500};
+constexpr chrono::milliseconds kReverseTimeout{500};
+constexpr chrono::milliseconds kReverseMinTimeout{100};
+} // namespace
+
+// TODO(austin): Pseudo current limit?
+
+IndexerController::IndexerController()
+ : loop_(new StateFeedbackLoop<3, 1, 1>(
+ superstructure::indexer::MakeIntegralIndexerLoop())),
+ stuck_indexer_detector_(new StateFeedbackLoop<3, 1, 1>(
+ superstructure::indexer::MakeStuckIntegralIndexerLoop())) {
+ history_.fill(0);
+ Y_.setZero();
+ X_hat_current_.setZero();
+ stuck_indexer_X_hat_current_.setZero();
+}
+
+void IndexerController::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+}
+
+void IndexerController::set_position(double current_position) {
+ // Update position in the model.
+ Y_ << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ dt_velocity_ = (current_position - last_position_) /
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+ last_position_ = current_position;
+}
+
+double IndexerController::voltage() const { return loop_->U(0, 0); }
+
+double IndexerController::StuckRatio() const {
+ double applied_voltage = voltage();
+ if (applied_voltage < 0) {
+ applied_voltage = ::std::min(applied_voltage, -kMinStuckVoltage);
+ } else {
+ applied_voltage = ::std::max(applied_voltage, kMinStuckVoltage);
+ }
+ // Look at the ratio of the current controller power to the voltage_error
+ // term. If our output is dominated by the voltage_error, then we are likely
+ // pretty stuck and should try reversing.
+ // We don't want to worry about dividing by zero, so keep the applied voltage
+ // away from 0 though a min/max.
+ return -stuck_indexer_X_hat_current_(2, 0) / applied_voltage;
+}
+bool IndexerController::IsStuck() const { return StuckRatio() > 0.6; }
+
+void IndexerController::Reset() { reset_ = true; }
+
+void IndexerController::PartialReset() { loop_->mutable_X_hat(2, 0) = 0.0; }
+
+void IndexerController::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (::std::abs(loop_->R(1, 0)) < 0.1) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ if (reset_) {
+ loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ loop_->mutable_X_hat(1, 0) = 0.0;
+ loop_->mutable_X_hat(2, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(0, 0) = Y_(0, 0);
+ stuck_indexer_detector_->mutable_X_hat(1, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(2, 0) = 0.0;
+ reset_ = false;
+ }
+
+ loop_->Correct(Y_);
+ stuck_indexer_detector_->Correct(Y_);
+
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ // Compute the distance moved over that time period.
+ average_angular_velocity_ =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count() *
+ static_cast<double>(kHistoryLength - 1));
+
+ // Ready if average angular velocity is close to the goal.
+ error_ = average_angular_velocity_ - loop_->next_R(1, 0);
+
+ ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+
+ X_hat_current_ = loop_->X_hat();
+ stuck_indexer_X_hat_current_ = stuck_indexer_detector_->X_hat();
+ position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
+
+ loop_->Update(disabled);
+ stuck_indexer_detector_->UpdateObserver(loop_->U());
+}
+
+void IndexerController::SetStatus(IndexerStatus *status) {
+ status->avg_angular_velocity = average_angular_velocity_;
+
+ status->angular_velocity = X_hat_current_(1, 0);
+ status->ready = ready_;
+
+ status->voltage_error = X_hat_current_(2, 0);
+ status->stuck_voltage_error = stuck_indexer_X_hat_current_(2, 0);
+ status->position_error = position_error_;
+ status->instantaneous_velocity = dt_velocity_;
+
+ status->stuck = IsStuck();
+
+ status->stuck_ratio = StuckRatio();
+}
+
+void Indexer::Reset() { indexer_.Reset(); }
+
+void Indexer::Iterate(const control_loops::IndexerGoal *goal,
+ const double *position, double *output,
+ control_loops::IndexerStatus *status) {
+ if (goal) {
+ // Start indexing at the suggested velocity.
+ // If a "stuck" event is detected, reverse. Stay reversed until either
+ // unstuck, or 0.5 seconds have elapsed.
+ // Then, start going forwards. Don't detect stuck for 0.5 seconds.
+
+ monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ switch (state_) {
+ case State::RUNNING:
+ // Pass the velocity goal through.
+ indexer_.set_goal(goal->angular_velocity);
+ // If we are stuck and weren't just reversing, try reversing to unstick
+ // us. We don't want to chatter back and forth too fast if reversing
+ // isn't working.
+ if (indexer_.IsStuck() &&
+ monotonic_now > kForwardTimeout + last_transition_time_) {
+ state_ = State::REVERSING;
+ last_transition_time_ = monotonic_now;
+ indexer_.Reset();
+ }
+ break;
+ case State::REVERSING:
+ // "Reverse" "slowly".
+ indexer_.set_goal(-5.0 * aos::sign(goal->angular_velocity));
+
+ // If we've timed out or are no longer stuck, try running again.
+ if ((!indexer_.IsStuck() &&
+ monotonic_now > last_transition_time_ + kReverseMinTimeout) ||
+ monotonic_now > kReverseTimeout + last_transition_time_) {
+ state_ = State::RUNNING;
+ last_transition_time_ = monotonic_now;
+
+ // Only reset if we got stuck going this way too.
+ if (monotonic_now > kReverseTimeout + last_transition_time_) {
+ indexer_.Reset();
+ }
+ }
+ break;
+ }
+ }
+
+ indexer_.set_position(*position);
+
+ indexer_.Update(output == nullptr);
+
+ indexer_.SetStatus(status);
+ status->state = static_cast<int32_t>(state_);
+
+ if (output) {
+ *output = indexer_.voltage();
+ }
+}
+
+} // namespace indexer
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
new file mode 100644
index 0000000..5c1cc1b
--- /dev/null
+++ b/y2017/control_loops/superstructure/indexer/indexer.h
@@ -0,0 +1,121 @@
+#ifndef Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
+#define Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace indexer {
+
+class IndexerController {
+ public:
+ IndexerController();
+
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ // Sets the current encoder position in radians
+ void set_position(double current_position);
+
+ // Populates the status structure.
+ void SetStatus(control_loops::IndexerStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ double dt_velocity() const { return dt_velocity_; }
+
+ double error() const { return error_; }
+
+ // Returns true if the indexer is stuck.
+ bool IsStuck() const;
+ double StuckRatio() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ // Resets the kalman filter and any other internal state.
+ void Reset();
+ // Resets the voltage error for when we reverse directions.
+ void PartialReset();
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // The stuck indexer detector
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> stuck_indexer_detector_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 5;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+
+ double error_ = 0.0;
+ double dt_velocity_ = 0.0;
+ double last_position_ = 0.0;
+ double average_angular_velocity_ = 0.0;
+ double position_error_ = 0.0;
+
+ Eigen::Matrix<double, 3, 1> X_hat_current_;
+ Eigen::Matrix<double, 3, 1> stuck_indexer_X_hat_current_;
+
+ bool ready_ = false;
+ bool reset_ = false;
+
+ DISALLOW_COPY_AND_ASSIGN(IndexerController);
+};
+
+class Indexer {
+ public:
+ Indexer() {}
+
+ enum class State {
+ // The system is running correctly, no stuck condition detected.
+ RUNNING = 0,
+ // The system is reversing to unjam.
+ REVERSING = 1
+ };
+
+ // Iterates the indexer control loop one cycle. position and status must
+ // never be nullptr. goal can be nullptr if no goal exists, and output should
+ // be nullptr if disabled.
+ void Iterate(const control_loops::IndexerGoal *goal, const double *position,
+ double *output, control_loops::IndexerStatus *status);
+
+ // Sets the indexer up to reset the kalman filter next time Iterate is called.
+ void Reset();
+
+ State state() const { return state_; }
+
+ private:
+ IndexerController indexer_;
+
+ State state_ = State::RUNNING;
+
+ // The last time that we transitioned from RUNNING to REVERSING or the
+ // reverse. Used to implement the timeouts.
+ ::aos::monotonic_clock::time_point last_transition_time_ =
+ ::aos::monotonic_clock::min_time;
+
+ DISALLOW_COPY_AND_ASSIGN(Indexer);
+};
+
+} // namespace indexer
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index d30a45c..c4d1630 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -90,9 +90,11 @@
}
}
if (reset_) {
- loop_->mutable_X_hat(2, 0) = 0.0;
- loop_->mutable_X_hat(1, 0) = dt_velocity_;
loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ // TODO(austin): This should be 0 for the WPILib reset since dt_velocity_
+ // will be rubbish.
+ loop_->mutable_X_hat(1, 0) = dt_velocity_;
+ loop_->mutable_X_hat(2, 0) = 0.0;
reset_ = false;
}
last_ready_ = ready_;
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 99de502..3cad298 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -27,6 +27,7 @@
turret_.Reset();
intake_.Reset();
shooter_.Reset();
+ indexer_.Reset();
}
hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
@@ -46,6 +47,10 @@
&(position->theta_shooter),
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
+ indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
+ &(position->theta_indexer),
+ output != nullptr ? &(output->voltage_indexer) : nullptr,
+ &(status->indexer));
}
} // namespace superstructure
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 4dc99a5..575d1d3 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -6,10 +6,11 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
-#include "y2017/control_loops/superstructure/turret/turret.h"
+#include "y2017/control_loops/superstructure/indexer/indexer.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/turret/turret.h"
namespace y2017 {
namespace control_loops {
@@ -25,6 +26,8 @@
const hood::Hood &hood() const { return hood_; }
const turret::Turret &turret() const { return turret_; }
const intake::Intake &intake() const { return intake_; }
+ const shooter::Shooter &shooter() const { return shooter_; }
+ const indexer::Indexer &indexer() const { return indexer_; }
protected:
virtual void RunIteration(
@@ -38,6 +41,7 @@
turret::Turret turret_;
intake::Intake intake_;
shooter::Shooter shooter_;
+ indexer::Indexer indexer_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 4a5edd8..49f93e5 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -64,8 +64,23 @@
// directly so there isn't confusion on if the goal is up to date.
bool ready;
- // If true, we have aborted.
- bool estopped;
+ // True if the indexer is stuck.
+ bool stuck;
+ float stuck_ratio;
+
+ // The state of the indexer state machine.
+ int32_t state;
+
+ // The estimated voltage error from the kalman filter in volts.
+ double voltage_error;
+ // The estimated voltage error from the stuck indexer kalman filter.
+ double stuck_voltage_error;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ double instantaneous_velocity;
+
+ // The error between our measurement and expected measurement in radians.
+ double position_error;
};
struct ShooterStatus {
@@ -79,9 +94,6 @@
// directly so there isn't confusion on if the goal is up to date.
bool ready;
- // If true, we have aborted.
- bool estopped;
-
// The estimated voltage error from the kalman filter in volts.
double voltage_error;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 7f71c4c..d03c48c 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
#include "gtest/gtest.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/hood/hood_plant.h"
+#include "y2017/control_loops/superstructure/indexer/indexer_plant.h"
#include "y2017/control_loops/superstructure/intake/intake_plant.h"
#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
#include "y2017/control_loops/superstructure/turret/turret_plant.h"
@@ -48,6 +49,25 @@
double voltage_offset_ = 0.0;
};
+class IndexerPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit IndexerPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU() override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
class HoodPlant : public StateFeedbackPlant<2, 1, 1> {
public:
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
@@ -125,6 +145,9 @@
shooter_plant_(new ShooterPlant(::y2017::control_loops::superstructure::
shooter::MakeShooterPlant())),
+ indexer_plant_(new IndexerPlant(::y2017::control_loops::superstructure::
+ indexer::MakeIndexerPlant())),
+
superstructure_queue_(".y2017.control_loops.superstructure", 0xdeadbeef,
".y2017.control_loops.superstructure.goal",
".y2017.control_loops.superstructure.position",
@@ -182,6 +205,7 @@
turret_pot_encoder_.GetSensorValues(&position->turret);
intake_pot_encoder_.GetSensorValues(&position->intake);
position->theta_shooter = shooter_plant_->Y(0, 0);
+ position->theta_indexer = indexer_plant_->Y(0, 0);
position.Send();
}
@@ -196,6 +220,8 @@
double shooter_velocity() const { return shooter_plant_->X(1, 0); }
+ double indexer_velocity() const { return indexer_plant_->X(1, 0); }
+
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_hood_power_error(double power_error) {
@@ -214,6 +240,15 @@
shooter_plant_->set_voltage_offset(power_error);
}
+ void set_indexer_voltage_offset(double power_error) {
+ indexer_plant_->set_voltage_offset(power_error);
+ }
+
+ // Triggers the indexer to freeze in position.
+ void set_freeze_indexer(bool freeze_indexer) {
+ freeze_indexer_ = freeze_indexer;
+ }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
@@ -262,10 +297,20 @@
<< superstructure_queue_.output->voltage_shooter +
shooter_plant_->voltage_offset();
+ indexer_plant_->mutable_U()
+ << superstructure_queue_.output->voltage_indexer +
+ indexer_plant_->voltage_offset();
+
hood_plant_->Update();
turret_plant_->Update();
intake_plant_->Update();
shooter_plant_->Update();
+ if (freeze_indexer_) {
+ indexer_plant_->mutable_X(1, 0) = 0.0;
+ } else {
+ indexer_plant_->Update();
+ }
+
const double angle_hood = hood_plant_->Y(0, 0);
const double angle_turret = turret_plant_->Y(0, 0);
@@ -295,6 +340,9 @@
::std::unique_ptr<ShooterPlant> shooter_plant_;
+ ::std::unique_ptr<IndexerPlant> indexer_plant_;
+ bool freeze_indexer_ = false;
+
SuperstructureQueue superstructure_queue_;
};
@@ -332,6 +380,8 @@
EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
superstructure_plant_.intake_position(), 0.001);
+ // Check that the angular velocity, average angular velocity, and estimated
+ // angular velocity match when we are done for the shooter.
EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
superstructure_queue_.status->shooter.angular_velocity, 0.1);
EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
@@ -339,6 +389,16 @@
0.1);
EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
superstructure_plant_.shooter_velocity(), 0.1);
+
+ // Check that the angular velocity, average angular velocity, and estimated
+ // angular velocity match when we are done for the indexer.
+ EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
+ superstructure_queue_.status->indexer.angular_velocity, 0.1);
+ EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
+ superstructure_queue_.status->indexer.avg_angular_velocity,
+ 0.1);
+ EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
+ superstructure_plant_.indexer_velocity(), 0.1);
}
// Runs one iteration of the whole simulation.
@@ -771,7 +831,7 @@
// Tests that the shooter spins up to speed and that it then spins down
// without applying any power.
-TEST_F(SuperstructureTest, SpinUpAndDown) {
+TEST_F(SuperstructureTest, ShooterSpinUpAndDown) {
// Spin up.
{
auto goal = superstructure_queue_.goal.MakeMessage();
@@ -779,6 +839,7 @@
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
goal->shooter.angular_velocity = 300.0;
+ goal->indexer.angular_velocity = 20.0;
ASSERT_TRUE(goal.Send());
}
@@ -792,6 +853,7 @@
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
goal->shooter.angular_velocity = 0.0;
+ goal->indexer.angular_velocity = 0.0;
ASSERT_TRUE(goal.Send());
}
@@ -806,13 +868,14 @@
}
// Tests that the shooter can spin up nicely after being disabled for a while.
-TEST_F(SuperstructureTest, Disabled) {
+TEST_F(SuperstructureTest, ShooterDisabled) {
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
goal->shooter.angular_velocity = 200.0;
+ goal->indexer.angular_velocity = 20.0;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(5), false);
@@ -823,6 +886,160 @@
VerifyNearGoal();
}
+// Tests that when the indexer gets stuck, it detects it and unjams.
+TEST_F(SuperstructureTest, StuckIndexerTest) {
+ // Spin up.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
+ goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->shooter.angular_velocity = 0.0;
+ goal->indexer.angular_velocity = 5.0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5));
+ VerifyNearGoal();
+ EXPECT_TRUE(superstructure_queue_.status->indexer.ready);
+
+ // Now, stick it.
+ const auto stuck_start_time = monotonic_clock::now();
+ superstructure_plant_.set_freeze_indexer(true);
+ while (monotonic_clock::now() < stuck_start_time + chrono::seconds(1)) {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ if (static_cast<indexer::Indexer::State>(
+ superstructure_queue_.status->indexer.state) ==
+ indexer::Indexer::State::REVERSING) {
+ break;
+ }
+ }
+
+ // Make sure it detected it reasonably fast.
+ const auto stuck_detection_time = monotonic_clock::now();
+ EXPECT_TRUE(stuck_detection_time - stuck_start_time <
+ chrono::milliseconds(200));
+
+ // Grab the position we were stuck at.
+ superstructure_queue_.position.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
+ const double indexer_position = superstructure_queue_.position->theta_indexer;
+
+ // Now, unstick it.
+ superstructure_plant_.set_freeze_indexer(false);
+ const auto unstuck_start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < unstuck_start_time + chrono::seconds(1)) {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ if (static_cast<indexer::Indexer::State>(
+ superstructure_queue_.status->indexer.state) ==
+ indexer::Indexer::State::RUNNING) {
+ break;
+ }
+ }
+
+ // Make sure it took some time, but not too much to detect us not being stuck anymore.
+ const auto unstuck_detection_time = monotonic_clock::now();
+ EXPECT_TRUE(unstuck_detection_time - unstuck_start_time <
+ chrono::milliseconds(200));
+ EXPECT_TRUE(unstuck_detection_time - unstuck_start_time >
+ chrono::milliseconds(40));
+
+ // Verify that it actually moved.
+ superstructure_queue_.position.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
+ const double unstuck_indexer_position =
+ superstructure_queue_.position->theta_indexer;
+ EXPECT_LT(unstuck_indexer_position, indexer_position - 0.1);
+
+ // Now, verify that everything works as expected.
+ RunForTime(chrono::seconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that when the indexer gets stuck forever, it switches back and forth at
+// a reasonable rate.
+TEST_F(SuperstructureTest, ReallyStuckIndexerTest) {
+ // Spin up.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
+ goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->shooter.angular_velocity = 0.0;
+ goal->indexer.angular_velocity = 5.0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5));
+ VerifyNearGoal();
+ EXPECT_TRUE(superstructure_queue_.status->indexer.ready);
+
+ // Now, stick it.
+ const auto stuck_start_time = monotonic_clock::now();
+ superstructure_plant_.set_freeze_indexer(true);
+ while (monotonic_clock::now() < stuck_start_time + chrono::seconds(1)) {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ if (static_cast<indexer::Indexer::State>(
+ superstructure_queue_.status->indexer.state) ==
+ indexer::Indexer::State::REVERSING) {
+ break;
+ }
+ }
+
+ // Make sure it detected it reasonably fast.
+ const auto stuck_detection_time = monotonic_clock::now();
+ EXPECT_TRUE(stuck_detection_time - stuck_start_time <
+ chrono::milliseconds(200));
+
+ // Now, try to unstick it.
+ const auto unstuck_start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < unstuck_start_time + chrono::seconds(1)) {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ if (static_cast<indexer::Indexer::State>(
+ superstructure_queue_.status->indexer.state) ==
+ indexer::Indexer::State::RUNNING) {
+ break;
+ }
+ }
+
+ // Make sure it took some time, but not too much to detect us not being stuck
+ // anymore.
+ const auto unstuck_detection_time = monotonic_clock::now();
+ EXPECT_TRUE(unstuck_detection_time - unstuck_start_time <
+ chrono::milliseconds(600));
+ EXPECT_TRUE(unstuck_detection_time - unstuck_start_time >
+ chrono::milliseconds(400));
+
+ // Now, make sure it transitions to stuck again after a delay.
+ const auto restuck_start_time = monotonic_clock::now();
+ superstructure_plant_.set_freeze_indexer(true);
+ while (monotonic_clock::now() < restuck_start_time + chrono::seconds(1)) {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ if (static_cast<indexer::Indexer::State>(
+ superstructure_queue_.status->indexer.state) ==
+ indexer::Indexer::State::REVERSING) {
+ break;
+ }
+ }
+
+ // Make sure it detected it reasonably fast.
+ const auto restuck_detection_time = monotonic_clock::now();
+ EXPECT_TRUE(restuck_detection_time - restuck_start_time >
+ chrono::milliseconds(400));
+ EXPECT_TRUE(restuck_detection_time - restuck_start_time <
+ chrono::milliseconds(600));
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops