Create/support "continuous" control loops
This supplies a wrap_point argument to the control loops code that
makes it so that you can have a system that spins infinitely (as the
swerve modules do) and still control them.
TODO: I observed some idiosyncracies in wrapping behavior during
testing; this likely requires additional tests to be written to validate
that we handle wrapping correctly.
Change-Id: Id4b9065de2b3334c0e8097b28a32916c47a54258
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 785d2c7..409c75a 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -346,6 +346,7 @@
],
deps = [
"//aos:macros",
+ "//frc971/zeroing:wrap",
"@org_tuxfamily_eigen//:eigen",
] + select({
"@platforms//os:linux": ["//aos/logging"],
@@ -611,6 +612,39 @@
],
)
+genrule(
+ name = "genrule_wrapped_subsystem_test",
+ outs = [
+ "wrapped_subsystem_test_plant.h",
+ "wrapped_subsystem_test_plant.cc",
+ "wrapped_subsystem_test_plant.json",
+ "wrapped_subsystem_test_integral_plant.h",
+ "wrapped_subsystem_test_integral_plant.cc",
+ "wrapped_subsystem_test_integral_plant.json",
+ ],
+ cmd = "$(location //frc971/control_loops/python:wrapped_subsystem_test) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//frc971/control_loops/python:wrapped_subsystem_test",
+ ],
+)
+
+cc_library(
+ name = "wrapped_subsystem_test_plants",
+ srcs = [
+ "wrapped_subsystem_test_integral_plant.cc",
+ "wrapped_subsystem_test_plant.cc",
+ ],
+ hdrs = [
+ "wrapped_subsystem_test_integral_plant.h",
+ "wrapped_subsystem_test_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":state_feedback_loop",
+ ],
+)
+
cc_library(
name = "static_zeroing_single_dof_profiled_subsystem_test_plants",
srcs = [
@@ -713,10 +747,12 @@
":static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position_fbs",
":static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_fbs",
":static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_fbs",
+ ":wrapped_subsystem_test_plants",
"//aos/testing:googletest",
"//frc971/control_loops:control_loop_test",
"//frc971/zeroing:absolute_and_absolute_encoder",
"//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:continuous_absolute_encoder",
"//frc971/zeroing:pot_and_absolute_encoder",
],
)
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 92f62b2..132216f 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -136,7 +136,8 @@
Eigen::Matrix<double, 4, 1>::Constant(-1),
Eigen::Matrix<double, 4, 2>::Zero(),
Eigen::Matrix<double, 4, 1>::Constant(0),
- frc971::controls::kLoopFrequency, false);
+ frc971::controls::kLoopFrequency, false,
+ Eigen::Matrix<double, 7, 1>::Zero());
// Build a plant.
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 7f458b5..0e0cdb6 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -262,6 +262,22 @@
)
py_binary(
+ name = "wrapped_subsystem_test",
+ srcs = [
+ "wrapped_subsystem_test.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":angular_system",
+ ":controls",
+ ":python_init",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+py_binary(
name = "static_zeroing_single_dof_profiled_subsystem_test",
srcs = [
"static_zeroing_single_dof_profiled_subsystem_test.py",
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 16773fa..9c5484c 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -24,7 +24,8 @@
radius=None,
dt=0.00505,
enable_voltage_error=True,
- delayed_u=0):
+ delayed_u=0,
+ wrap_point=0.0):
"""Constructs an AngularSystemParams object.
Args:
@@ -46,6 +47,7 @@
self.dt = dt
self.enable_voltage_error = enable_voltage_error
self.delayed_u = delayed_u
+ self.wrap_point = wrap_point
class AngularSystem(control_loop.ControlLoop):
@@ -144,6 +146,8 @@
self.InitializeState()
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
class IntegralAngularSystem(AngularSystem):
@@ -195,6 +199,8 @@
self.InitializeState()
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
def RunTest(plant,
end_goal,
diff --git a/frc971/control_loops/python/angular_system_current.py b/frc971/control_loops/python/angular_system_current.py
index 585f54a..0365a19 100755
--- a/frc971/control_loops/python/angular_system_current.py
+++ b/frc971/control_loops/python/angular_system_current.py
@@ -22,7 +22,8 @@
kalman_q_voltage,
kalman_r_position,
radius=None,
- dt=0.00505):
+ dt=0.00505,
+ wrap_point=0.0):
"""Constructs an AngularSystemCurrentParams object.
Args:
@@ -38,6 +39,8 @@
kalman_r_position: float, std deviation of the position measurement
radius: float, radius of the mechanism in meters
dt: float, length of the control loop period in seconds
+ wrap_point: float, point at which the position will wrap (if non-zero)
+ Will typically be 2 * pi if set.
"""
self.name = name
self.motor = motor
@@ -51,6 +54,7 @@
self.kalman_r_position = kalman_r_position
self.radius = radius
self.dt = dt
+ self.wrap_point = wrap_point
# An angular system that uses current control instead of voltage
@@ -160,6 +164,8 @@
self.delayed_u = 1
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
self.InitializeState()
@@ -216,6 +222,8 @@
self.InitializeState()
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
def RunTest(plant,
end_goal,
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index a6fcd3e..ec44092 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -353,6 +353,7 @@
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.last_U = numpy.matrix(
numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
+ self.wrap_point = numpy.matrix(numpy.zeros(self.Y.shape))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -451,7 +452,8 @@
"u_max": MatrixToJson(self.U_max),
"u_limit_coefficient": MatrixToJson(self.U_limit_coefficient),
"u_limit_constant": MatrixToJson(self.U_limit_constant),
- "delayed_u": self.delayed_u
+ "delayed_u": self.delayed_u,
+ "wrap_point": MatrixToJson(self.wrap_point)
}
if plant_coefficient_type.startswith('StateFeedbackPlant'):
result["a"] = MatrixToJson(self.A)
@@ -502,11 +504,13 @@
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
+ ans.append(
+ self._DumpMatrix('wrap_point', self.wrap_point, scalar_type))
ans.append(' const std::chrono::nanoseconds dt(%d);\n' %
(self.dt * 1e9))
ans.append(
' return %s'
- '(A, B, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, dt, %s);\n'
+ '(A, B, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, dt, %s, wrap_point);\n'
% (plant_coefficient_type, delayed_u_string))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index a529378..02a61f1 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -170,6 +170,7 @@
self.U_max = self._drivetrain.U_max
self.U_min = self._drivetrain.U_min
+ self.wrap_point = numpy.matrix(numpy.zeros((2, 1)))
@property
def robot_radius_l(self):
diff --git a/frc971/control_loops/python/wrapped_subsystem_test.py b/frc971/control_loops/python/wrapped_subsystem_test.py
new file mode 100644
index 0000000..0a68103
--- /dev/null
+++ b/frc971/control_loops/python/wrapped_subsystem_test.py
@@ -0,0 +1,55 @@
+#!/usr/bin/python3
+
+# Generates profiled subsystem for use in
+# static_zeroing_single_dof_profiled_subsystem_test
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kWrappedSystem = angular_system.AngularSystemParams(
+ name='TestWrappedSystem',
+ motor=control_loop.Vex775Pro(),
+ G=(1.0 / 35.0) * (20.0 / 40.0),
+ radius=16.0 * 0.25 / (2.0 * numpy.pi) * 0.0254,
+ J=5.4,
+ q_pos=0.015,
+ q_vel=0.3,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.00,
+ kalman_q_voltage=40.0,
+ kalman_r_position=0.05,
+ wrap_point=2.0 * numpy.pi)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.1], [0.0]])
+ angular_system.PlotMotion(kWrappedSystem, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 7:
+ glog.fatal(
+ 'Expected .h, .cc, and .json filenames and .json file name for the \
+ static_zeroing_single_dof_profiled_subsystem_test and integral \
+ static_zeroing_single_dof_profiled_subsystem_test.')
+ else:
+ namespaces = ['frc971', 'control_loops']
+ angular_system.WriteAngularSystem(kWrappedSystem, argv[1:4], argv[4:7],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/state_feedback_loop.fbs b/frc971/control_loops/state_feedback_loop.fbs
index 7c9bff8..4ab267d 100644
--- a/frc971/control_loops/state_feedback_loop.fbs
+++ b/frc971/control_loops/state_feedback_loop.fbs
@@ -17,6 +17,7 @@
u_limit_constant:frc971.fbs.Matrix (id: 7);
dt:uint64 (id: 8);
delayed_u:uint64 (id: 9);
+ wrap_point:frc971.fbs.Matrix (id: 10);
}
table StateFeedbackHybridPlantCoefficients {
@@ -29,6 +30,7 @@
u_limit_coefficient:frc971.fbs.Matrix (id: 6);
u_limit_constant:frc971.fbs.Matrix (id: 7);
delayed_u:uint64 (id: 8);
+ wrap_point:frc971.fbs.Matrix (id: 9);
}
table StateFeedbackControllerCoefficients {
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 32f27f8..b6604d4 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -18,6 +18,7 @@
#include "aos/logging/logging.h"
#endif
#include "aos/macros.h"
+#include "frc971/zeroing/wrap.h"
template <int number_of_states, int number_of_inputs, int number_of_outputs,
typename PlantType, typename ObserverType, typename Scalar>
@@ -46,7 +47,8 @@
U_limit_coefficient(other.U_limit_coefficient),
U_limit_constant(other.U_limit_constant),
dt(other.dt),
- delayed_u(other.delayed_u) {}
+ delayed_u(other.delayed_u),
+ wrap_point(other.wrap_point) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
@@ -58,7 +60,8 @@
const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
&U_limit_coefficient,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_limit_constant,
- const std::chrono::nanoseconds dt, size_t delayed_u)
+ const std::chrono::nanoseconds dt, size_t delayed_u,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &wrap_point)
: A(A),
B(B),
C(C),
@@ -68,7 +71,8 @@
U_limit_coefficient(U_limit_coefficient),
U_limit_constant(U_limit_constant),
dt(dt),
- delayed_u(delayed_u) {}
+ delayed_u(delayed_u),
+ wrap_point(wrap_point) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
@@ -79,6 +83,7 @@
const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
U_limit_coefficient;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_limit_constant;
+
const std::chrono::nanoseconds dt;
// If true, this adds a single cycle output delay model to the plant. This is
@@ -86,6 +91,11 @@
// then queue the outputs to be ready to be executed when the next cycle
// happens.
const size_t delayed_u;
+
+ // We will assume that each element of the Y vector wraps at the
+ // specified point. For any given element that is zero, we will
+ // assume no wrapping.
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> wrap_point;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -250,6 +260,23 @@
return A() * X + B() * U;
}
+ // Takes the provided state and wraps all the individual values such that,
+ // for any given i in [0, number_of_states), the wrapped X[i] will be in
+ // the range [-wrap_point[i] / 2, wrap_point[i] / 2] if wrap_point[i] > 0.
+ Eigen::Matrix<Scalar, number_of_outputs, 1> HandleWrapping(
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) const {
+ Eigen::Matrix<Scalar, number_of_outputs, 1> wrapped;
+ for (int index = 0; index < number_of_outputs; ++index) {
+ const Scalar wrap_point = coefficients().wrap_point[index];
+ wrapped[index] =
+ wrap_point > 0
+ ? frc971::zeroing::Wrap(/*nearest=*/static_cast<Scalar>(0.0),
+ Y(index), wrap_point)
+ : Y(index);
+ }
+ return wrapped;
+ }
+
protected:
// these are accessible from non-templated subclasses
static const int kNumStates = number_of_states;
@@ -438,7 +465,9 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
- mutable_X_hat() += KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
+ mutable_X_hat() +=
+ KalmanGain() *
+ plant.HandleWrapping(Y - plant.C() * X_hat() - plant.D() * U);
}
// Sets the current controller to be index, clamped to be within range.
diff --git a/frc971/control_loops/state_feedback_loop_converters.h b/frc971/control_loops/state_feedback_loop_converters.h
index 5e0117a..9c34199 100644
--- a/frc971/control_loops/state_feedback_loop_converters.h
+++ b/frc971/control_loops/state_feedback_loop_converters.h
@@ -37,7 +37,8 @@
ToEigenOrDie<number_of_inputs, number_of_states>(
*coefficients.u_limit_coefficient()),
ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_limit_constant()),
- std::chrono::nanoseconds(coefficients.dt()), coefficients.delayed_u());
+ std::chrono::nanoseconds(coefficients.dt()), coefficients.delayed_u(),
+ ToEigenOrDie<number_of_outputs, 1>(*coefficients.wrap_point()));
}
template <int number_of_states, int number_of_inputs, int number_of_outputs>
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index c17b734..de24a09 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -18,6 +18,7 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/continuous_absolute_encoder.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
@@ -38,33 +39,49 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+using SZSDPS_Continuous = StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::ContinuousAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
using FBB = flatbuffers::FlatBufferBuilder;
-struct PotAndAbsoluteEncoderQueueGroup {
+struct PotAndAbsoluteEncoderTestParams {
typedef zeroing::testing::SubsystemPotAndAbsoluteEncoderProfiledJointStatus
Status;
typedef zeroing::testing::SubsystemPotAndAbsolutePosition Position;
typedef PotAndAbsolutePosition RealPosition;
typedef ::frc971::control_loops::zeroing::testing::SubsystemGoal Goal;
typedef ::frc971::control_loops::zeroing::testing::SubsystemOutput Output;
+ static constexpr bool kRespectsHardstops = true;
+ static constexpr bool kIsContinuous = false;
};
-struct AbsoluteEncoderQueueGroup {
+struct AbsoluteEncoderTestParams {
typedef zeroing::testing::SubsystemAbsoluteEncoderProfiledJointStatus Status;
typedef zeroing::testing::SubsystemAbsolutePosition Position;
typedef AbsolutePosition RealPosition;
typedef zeroing::testing::SubsystemGoal Goal;
typedef zeroing::testing::SubsystemOutput Output;
+ static constexpr bool kRespectsHardstops = true;
+ static constexpr bool kIsContinuous = false;
+};
+
+struct ContinuousAbsoluteEncoderTestParams {
+ typedef zeroing::testing::SubsystemAbsoluteEncoderProfiledJointStatus Status;
+ typedef zeroing::testing::SubsystemAbsolutePosition Position;
+ typedef AbsolutePosition RealPosition;
+ typedef zeroing::testing::SubsystemGoal Goal;
+ typedef zeroing::testing::SubsystemOutput Output;
+ static constexpr bool kRespectsHardstops = false;
+ static constexpr bool kIsContinuous = true;
};
typedef ::testing::Types<
- ::std::pair<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>,
- ::std::pair<SZSDPS_PotAndAbsEncoder, PotAndAbsoluteEncoderQueueGroup>>
+ ::std::pair<SZSDPS_AbsEncoder, AbsoluteEncoderTestParams>,
+ ::std::pair<SZSDPS_PotAndAbsEncoder, PotAndAbsoluteEncoderTestParams>,
+ ::std::pair<SZSDPS_Continuous, ContinuousAbsoluteEncoderTestParams>>
TestTypes;
-constexpr ::frc971::constants::Range kRange{
- .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
-
constexpr double kZeroingVoltage = 2.5;
constexpr double kOperatingVoltage = 12.0;
const int kZeroingSampleSize = 200;
@@ -74,6 +91,7 @@
template <typename ZeroingEstimator>
struct TestIntakeSystemValues {
static const typename ZeroingEstimator::ZeroingConstants kZeroing;
+ static const ::frc971::constants::Range kRange;
static const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
make_params();
@@ -92,6 +110,25 @@
{}, kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20,
1.9};
+template <>
+const frc971::zeroing::ContinuousAbsoluteEncoderZeroingEstimator::
+ ZeroingConstants TestIntakeSystemValues<
+ frc971::zeroing::ContinuousAbsoluteEncoderZeroingEstimator>::kZeroing{
+ {}, kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.0005, 20, 1.9};
+
+template <>
+const ::frc971::constants::Range TestIntakeSystemValues<
+ frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kRange{
+ .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
+template <>
+const ::frc971::constants::Range TestIntakeSystemValues<
+ frc971::zeroing::AbsoluteEncoderZeroingEstimator>::kRange{
+ .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
+template <>
+const ::frc971::constants::Range TestIntakeSystemValues<
+ frc971::zeroing::ContinuousAbsoluteEncoderZeroingEstimator>::kRange{
+ .lower_hard = -3.01, .upper_hard = 3.1, .lower = -3.00, .upper = 3.0};
+
template <typename ZeroingEstimator>
const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
TestIntakeSystemValues<ZeroingEstimator>::make_params() {
@@ -100,7 +137,7 @@
kOperatingVoltage,
{{}, 0.1, 1.0},
{{}, 0.3, 5.0},
- kRange,
+ TestIntakeSystemValues::kRange,
TestIntakeSystemValues::kZeroing,
&MakeIntegralTestIntakeSystemLoop};
return params;
@@ -108,14 +145,16 @@
} // namespace
-template <typename SZSDPS, typename QueueGroup>
+template <typename SZSDPS, typename TestParams>
class TestIntakeSystemSimulation {
public:
- typedef typename QueueGroup::Goal GoalType;
- typedef typename QueueGroup::Status StatusType;
- typedef typename QueueGroup::Position PositionType;
- typedef typename QueueGroup::RealPosition RealPositionType;
- typedef typename QueueGroup::Output OutputType;
+ typedef typename TestParams::Goal GoalType;
+ typedef typename TestParams::Status StatusType;
+ typedef typename TestParams::Position PositionType;
+ typedef typename TestParams::RealPosition RealPositionType;
+ typedef typename TestParams::Output OutputType;
+ const ::frc971::constants::Range kRange =
+ TestIntakeSystemValues<typename SZSDPS::ZeroingEstimator>::kRange;
TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
chrono::nanoseconds dt)
@@ -247,7 +286,7 @@
template <>
void TestIntakeSystemSimulation<
SZSDPS_PotAndAbsEncoder,
- PotAndAbsoluteEncoderQueueGroup>::InitializeSensorSim(double start_pos) {
+ PotAndAbsoluteEncoderTestParams>::InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
TestIntakeSystemValues<
@@ -256,7 +295,7 @@
}
template <>
-void TestIntakeSystemSimulation<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>::
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder, AbsoluteEncoderTestParams>::
InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
@@ -265,23 +304,33 @@
.measured_absolute_position);
}
+template <>
+void TestIntakeSystemSimulation<SZSDPS_Continuous,
+ ContinuousAbsoluteEncoderTestParams>::
+ InitializeSensorSim(double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<typename SZSDPS_Continuous::ZeroingEstimator>::
+ kZeroing.measured_absolute_position);
+}
+
// Class to represent a module using a subsystem. This lets us use event loops
// to wrap it.
-template <typename QueueGroup, typename SZSDPS>
+template <typename TestParams, typename SZSDPS>
class Subsystem
: public ::frc971::controls::ControlLoop<
- typename QueueGroup::Goal, typename QueueGroup::Position,
- typename QueueGroup::Status, typename QueueGroup::Output> {
+ typename TestParams::Goal, typename TestParams::Position,
+ typename TestParams::Status, typename TestParams::Output> {
public:
- typedef typename QueueGroup::Goal GoalType;
- typedef typename QueueGroup::Status StatusType;
- typedef typename QueueGroup::Position PositionType;
- typedef typename QueueGroup::Output OutputType;
+ typedef typename TestParams::Goal GoalType;
+ typedef typename TestParams::Status StatusType;
+ typedef typename TestParams::Position PositionType;
+ typedef typename TestParams::Output OutputType;
Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
: frc971::controls::ControlLoop<
- typename QueueGroup::Goal, typename QueueGroup::Position,
- typename QueueGroup::Status, typename QueueGroup::Output>(
+ typename TestParams::Goal, typename TestParams::Position,
+ typename TestParams::Status, typename TestParams::Output>(
event_loop, name),
subsystem_(TestIntakeSystemValues<
typename SZSDPS::ZeroingEstimator>::make_params()) {}
@@ -360,14 +409,19 @@
class IntakeSystemTest : public ::frc971::testing::ControlLoopTest {
protected:
using SZSDPS = typename TSZSDPS::first_type;
- using QueueGroup = typename TSZSDPS::second_type;
+ using TestParams = typename TSZSDPS::second_type;
using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
- typedef typename QueueGroup::Goal GoalType;
- typedef typename QueueGroup::Status StatusType;
- typedef typename QueueGroup::Position PositionType;
- typedef typename QueueGroup::Output OutputType;
+ typedef typename TestParams::Goal GoalType;
+ typedef typename TestParams::Status StatusType;
+ typedef typename TestParams::Position PositionType;
+ typedef typename TestParams::Output OutputType;
+
+ static constexpr bool kRespectsHardstops = TestParams::kRespectsHardstops;
+ static constexpr bool kIsContinuous = TestParams::kIsContinuous;
+ const ::frc971::constants::Range kRange =
+ TestIntakeSystemValues<ZeroingEstimator>::kRange;
IntakeSystemTest()
: ::frc971::testing::ControlLoopTest(
@@ -392,7 +446,8 @@
EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
- subsystem_status_fetcher_->status()->position(), 0.001);
+ subsystem_status_fetcher_->status()->position(), 0.001)
+ << aos::FlatbufferToJson(subsystem_status_fetcher_.get());
EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
subsystem_plant_.subsystem_position(), 0.001);
EXPECT_NEAR(subsystem_status_fetcher_->status()->velocity(), 0, 0.001);
@@ -414,10 +469,10 @@
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> subsystem_event_loop_;
- Subsystem<QueueGroup, SZSDPS> subsystem_;
+ Subsystem<TestParams, SZSDPS> subsystem_;
::std::unique_ptr<::aos::EventLoop> subsystem_plant_event_loop_;
- TestIntakeSystemSimulation<SZSDPS, QueueGroup> subsystem_plant_;
+ TestIntakeSystemSimulation<SZSDPS, TestParams> subsystem_plant_;
};
TYPED_TEST_SUITE_P(IntakeSystemTest);
@@ -458,6 +513,68 @@
this->VerifyNearGoal();
}
+// Tests that the subsystem loop can reach a goal.
+TYPED_TEST_P(IntakeSystemTest, ContinuousReachesGoal) {
+ if (!this->kIsContinuous) {
+ return;
+ }
+ // Set a reasonable goal.
+ auto send_goal = [this](double goal_pos) {
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), goal_pos, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
+ };
+ // Deliberately start the subsystem at an offset from zero so that we can
+ // observe that we are not able to zero the "true" absolute position of the
+ // subsystem.
+ this->subsystem_plant_.InitializeSubsystemPosition(kEncoderIndexDifference +
+ 0.01);
+ EXPECT_FLOAT_EQ(1.01, this->subsystem_plant_.subsystem_position())
+ << "Sanity check of initial system state failed.";
+ this->SetEnabled(true);
+ auto verify_near_value = [this](double goal, std::string_view message) {
+ SCOPED_TRACE(message);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+
+ // Because the subsystem starts at a position of 1.01 and we only have
+ // an absolute encoder, the status will always output positions that are
+ // 1 period behind the "actual" position.
+ const double expected_status_offset = -kEncoderIndexDifference;
+ EXPECT_NEAR(goal + expected_status_offset,
+ this->subsystem_status_fetcher_->status()->position(), 0.001)
+ << aos::FlatbufferToJson(this->subsystem_status_fetcher_.get());
+ EXPECT_NEAR(goal, this->subsystem_plant_.subsystem_position(), 0.001)
+ << aos::FlatbufferToJson(this->subsystem_status_fetcher_.get());
+ EXPECT_NEAR(this->subsystem_status_fetcher_->status()->velocity(), 0,
+ 0.001);
+ };
+
+ // Note that while the continuous subsystem controller does not know which
+ // revolution it started on, it does not attempt to wrap requested goals.
+ send_goal(0.9);
+ this->RunFor(chrono::seconds(8));
+ verify_near_value(1.9, "initial goal");
+
+ send_goal(1.1);
+ this->RunFor(chrono::seconds(8));
+ verify_near_value(2.1, "increment");
+
+ // Sending a goal that is offset by 1 should result in us driving the
+ // subsystem by one period.
+ send_goal(0.1);
+ this->RunFor(chrono::seconds(8));
+ verify_near_value(1.1, "offset by one period");
+ // Check that we can handle negative goals.
+ send_goal(-0.9);
+ this->RunFor(chrono::seconds(8));
+ verify_near_value(0.1, "send negative goal");
+}
+
// Tests that the subsystem loop can reach a goal when the profile is disabled.
TYPED_TEST_P(IntakeSystemTest, FunctionsWhenProfileDisabled) {
this->SetEnabled(true);
@@ -528,11 +645,11 @@
// Zero it before we move.
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
- kRange.upper)),
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.upper)),
aos::RawSender::Error::kOk);
}
- this->RunFor(chrono::seconds(8));
+ this->RunFor(chrono::seconds(20));
this->VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
@@ -543,14 +660,15 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(20.0);
profile_builder.add_max_acceleration(0.1);
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower, profile_builder.Finish())),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(
+ message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.lower, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
- this->RunFor(chrono::seconds(8));
+ this->RunFor(chrono::seconds(20));
this->VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
@@ -560,14 +678,16 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(0.1);
profile_builder.add_max_acceleration(100.0);
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper, profile_builder.Finish())),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(
+ message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.upper, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->set_peak_subsystem_velocity(0.2);
this->set_peak_subsystem_acceleration(103);
- this->RunFor(chrono::seconds(8));
+ this->RunFor(chrono::seconds(static_cast<int>(
+ std::ceil((this->kRange.upper - this->kRange.lower) / 0.1 * 1.1))));
this->VerifyNearGoal();
}
@@ -592,7 +712,7 @@
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper,
+ EXPECT_NEAR(this->kRange.upper,
this->subsystem_status_fetcher_->status()->position(), 0.001);
// Set some ridiculous goals to test lower limits.
@@ -607,11 +727,11 @@
aos::RawSender::Error::kOk);
}
- this->RunFor(chrono::seconds(10));
+ this->RunFor(chrono::seconds(20));
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower,
+ EXPECT_NEAR(this->kRange.lower,
this->subsystem_status_fetcher_->status()->position(), 0.001);
}
@@ -625,9 +745,10 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(1.0);
profile_builder.add_max_acceleration(0.5);
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper, profile_builder.Finish())),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(
+ message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.upper, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -644,12 +765,15 @@
}
TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+ if (!this->kRespectsHardstops) {
+ return;
+ }
this->SetEnabled(true);
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
+ this->subsystem_plant_.InitializeSubsystemPosition(this->kRange.lower_hard);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
- kRange.upper)),
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.upper)),
aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -659,13 +783,16 @@
// Tests that starting at the upper hardstops doesn't cause an abort.
TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+ if (!this->kRespectsHardstops) {
+ return;
+ }
this->SetEnabled(true);
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
+ this->subsystem_plant_.InitializeSubsystemPosition(this->kRange.upper_hard);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
- kRange.upper)),
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.upper)),
aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -677,11 +804,10 @@
TYPED_TEST_P(IntakeSystemTest, ResetTest) {
this->SetEnabled(true);
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper - 0.1)),
+ EXPECT_EQ(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.1)),
aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -706,7 +832,7 @@
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower + 0.03)),
+ *message.fbb(), this->kRange.lower + 0.03)),
aos::RawSender::Error::kOk);
}
@@ -724,8 +850,8 @@
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
- kRange.lower)),
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), this->kRange.lower)),
aos::RawSender::Error::kOk);
}
@@ -734,7 +860,7 @@
EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
this->SetEnabled(true);
- this->RunFor(chrono::seconds(4));
+ this->RunFor(chrono::seconds(12));
this->VerifyNearGoal();
}
@@ -745,31 +871,32 @@
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower_hard)),
+ *message.fbb(), this->kRange.lower_hard)),
aos::RawSender::Error::kOk);
}
- this->RunFor(chrono::seconds(2));
+ this->RunFor(chrono::seconds(12));
- // Check that kRange.lower is used as the default min position
- EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ // Check that this->kRange.lower is used as the default min position
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower,
+ EXPECT_NEAR(this->kRange.lower,
this->subsystem_status_fetcher_->status()->position(), 0.001);
// Set min position and check that the subsystem increases to that position
- this->subsystem()->set_min_position(kRange.lower + 0.05);
+ this->subsystem()->set_min_position(this->kRange.lower + 0.05);
this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.lower + 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower + 0.05,
+ EXPECT_NEAR(this->kRange.lower + 0.05,
this->subsystem_status_fetcher_->status()->position(), 0.001);
- // Clear min position and check that the subsystem returns to kRange.lower
+ // Clear min position and check that the subsystem returns to
+ // this->kRange.lower
this->subsystem()->clear_min_position();
this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower,
+ EXPECT_NEAR(this->kRange.lower,
this->subsystem_status_fetcher_->status()->position(), 0.001);
}
@@ -780,31 +907,32 @@
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper_hard)),
+ *message.fbb(), this->kRange.upper_hard)),
aos::RawSender::Error::kOk);
}
- this->RunFor(chrono::seconds(2));
+ this->RunFor(chrono::seconds(12));
- // Check that kRange.upper is used as the default max position
- EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ // Check that this->kRange.upper is used as the default max position
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper,
+ EXPECT_NEAR(this->kRange.upper,
this->subsystem_status_fetcher_->status()->position(), 0.001);
// Set max position and check that the subsystem lowers to that position
- this->subsystem()->set_max_position(kRange.upper - 0.05);
+ this->subsystem()->set_max_position(this->kRange.upper - 0.05);
this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.upper - 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper - 0.05,
+ EXPECT_NEAR(this->kRange.upper - 0.05,
this->subsystem_status_fetcher_->status()->position(), 0.001);
- // Clear max position and check that the subsystem returns to kRange.upper
+ // Clear max position and check that the subsystem returns to
+ // this->kRange.upper
this->subsystem()->clear_max_position();
this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ EXPECT_EQ(this->subsystem()->goal(0), this->kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper,
+ EXPECT_NEAR(this->kRange.upper,
this->subsystem_status_fetcher_->status()->position(), 0.001);
}
@@ -812,11 +940,12 @@
TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
this->SetEnabled(true);
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+ this->subsystem_plant_.InitializeSubsystemPosition(this->kRange.upper);
this->RunFor(chrono::seconds(5));
- EXPECT_NEAR(kRange.upper, this->subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(this->kRange.upper, this->subsystem_plant_.subsystem_position(),
+ 0.001);
EXPECT_NEAR(this->subsystem_plant_.subsystem_velocity(), 0, 0.001);
}
@@ -838,7 +967,7 @@
LowerHardstopStartup, UpperHardstopStartup,
ResetTest, DisabledGoalTest, DisabledZeroTest,
MinPositionTest, MaxPositionTest, NullGoalTest,
- ZeroingErrorTest);
+ ZeroingErrorTest, ContinuousReachesGoal);
INSTANTIATE_TYPED_TEST_SUITE_P(My, IntakeSystemTest, TestTypes);
} // namespace frc971::control_loops