Add StaticZeroingSingleDOFProfiledSubsystem
Change-Id: I7ad67ed8719a229f4f08d8255d4675fa3ee49f18
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index d12d3da..37957ec 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -341,3 +341,75 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "capped_test_plant",
+ testonly = True,
+ srcs = [
+ "capped_test_plant.cc",
+ ],
+ hdrs = [
+ "capped_test_plant.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem.cc",
+ ],
+ hdrs = [
+ "static_zeroing_single_dof_profiled_subsystem.h",
+ ],
+ deps = [
+ "//frc971/control_loops:profiled_subsystem",
+ ],
+)
+
+genrule(
+ name = "genrule_static_zeroing_single_dof_profiled_subsystem_test",
+ outs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.cc",
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.cc",
+ ],
+ cmd = "$(location //frc971/control_loops/python:static_zeroing_single_dof_profiled_subsystem_test) $(OUTS)",
+ tools = [
+ "//frc971/control_loops/python:static_zeroing_single_dof_profiled_subsystem_test",
+ ],
+)
+
+cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_plants",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.cc",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.cc",
+ ],
+ hdrs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ ],
+)
+
+cc_test(
+ name = "static_zeroing_single_dof_profiled_subsystem_test",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.cc",
+ ],
+ deps = [
+ ":capped_test_plant",
+ ":position_sensor_sim",
+ ":static_zeroing_single_dof_profiled_subsystem",
+ ":static_zeroing_single_dof_profiled_subsystem_test_plants",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/frc971/control_loops/capped_test_plant.cc b/frc971/control_loops/capped_test_plant.cc
new file mode 100644
index 0000000..8f32551
--- /dev/null
+++ b/frc971/control_loops/capped_test_plant.cc
@@ -0,0 +1 @@
+#include "frc971/control_loops/capped_test_plant.h"
\ No newline at end of file
diff --git a/frc971/control_loops/capped_test_plant.h b/frc971/control_loops/capped_test_plant.h
new file mode 100644
index 0000000..65bc7cc
--- /dev/null
+++ b/frc971/control_loops/capped_test_plant.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Basic state feedback plant for use in tests.
+class CappedTestPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit CappedTestPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) 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;
+};
+
+} // namespace frc971
+} // namespace control_loops
+#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
\ No newline at end of file
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index d1356da..784b92f 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -153,3 +153,8 @@
// State of the estimator.
.frc971.IndexEstimatorState estimator_state;
};
+
+struct StaticZeroingSingleDOFProfiledSubsystemGoal {
+ double unsafe_goal;
+ .frc971.ProfileParameters profile_params;
+};
\ No newline at end of file
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index a0ee825..b3aa2c0 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -97,7 +97,7 @@
deps = [
":libspline",
":python_init",
- ]
+ ],
)
py_library(
@@ -166,15 +166,15 @@
py_binary(
name = "path_edit",
srcs = [
- "path_edit.py",
"basic_window.py",
"color.py",
+ "path_edit.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
- ":python_init",
":libspline",
+ ":python_init",
"@python_gtk",
],
)
@@ -183,10 +183,10 @@
name = "basic_window",
srcs = [
"basic_window.py",
- "color.py"
+ "color.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
":python_init",
"@python_gtk",
@@ -198,9 +198,25 @@
srcs = [
"color.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
":python_init",
],
)
+
+py_binary(
+ name = "static_zeroing_single_dof_profiled_subsystem_test",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":controls",
+ ":linear_system",
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ ],
+)
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
new file mode 100644
index 0000000..caa3082
--- /dev/null
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -0,0 +1,56 @@
+#!/usr/bin/python
+
+# 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 linear_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
+
+kIntake = linear_system.LinearSystemParams(
+ name='TestIntakeSystem',
+ motor=control_loop.Vex775Pro(),
+ # (1 / 35.0) * (20.0 / 40.0) -> 16 tooth sprocket on #25 chain
+ G=(1.0 / 35.0) * (20.0 / 40.0),
+ radius=16.0 * 0.25 / (2.0 * numpy.pi) * 0.0254,
+ mass=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)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.1], [0.0]])
+ linear_system.PlotMotion(kIntake, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc 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']
+ linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.cc
new file mode 100644
index 0000000..ca8f38b
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.cc
@@ -0,0 +1,125 @@
+#include "static_zeroing_single_dof_profiled_subsystem.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StaticZeroingSingleDOFProfiledSubsystem::
+ StaticZeroingSingleDOFProfiledSubsystem(
+ const StaticZeroingSingleDOFProfiledSubsystemParams ¶ms)
+ : params_(params),
+ profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(params.make_integral_loop())),
+ params.zeroing_constants, params.range,
+ params.default_profile_params.max_velocity,
+ params.default_profile_params.max_acceleration) {
+ Reset();
+};
+
+void StaticZeroingSingleDOFProfiledSubsystem::Reset() {
+ state_ = State::UNINITIALIZED;
+ clear_min_position();
+ clear_max_position();
+ profiled_subsystem_.Reset();
+}
+
+void StaticZeroingSingleDOFProfiledSubsystem::Iterate(
+ const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename zeroing::PotAndAbsoluteEncoderZeroingEstimator::Position
+ *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disabled = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disabled = true;
+ break;
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disabled) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(params_.zeroing_profile_params);
+
+ // We are not ready to start doing anything yet.
+ disabled = true;
+ break;
+ case State::ZEROING:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disabled) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disabled) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ if (goal) {
+ profiled_subsystem_.AdjustProfile(goal->profile_params);
+
+ double safe_goal = goal->unsafe_goal;
+ if (safe_goal < min_position_) {
+ LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
+ safe_goal = min_position_;
+ }
+ if (safe_goal > max_position_) {
+ LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
+ safe_goal = max_position_;
+ }
+ profiled_subsystem_.set_unprofiled_goal(safe_goal);
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disabled = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage = (state_ == State::RUNNING)
+ ? params_.operating_voltage
+ : params_.zeroing_voltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disabled);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace control_loops
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
new file mode 100644
index 0000000..9926c24
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -0,0 +1,97 @@
+#ifndef FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
+#define FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+
+namespace frc971 {
+namespace control_loops {
+
+struct StaticZeroingSingleDOFProfiledSubsystemParams {
+ // Maximum voltage while the subsystem is zeroing
+ double zeroing_voltage;
+
+ // Maximum voltage while the subsystem is running
+ double operating_voltage;
+
+ // Maximum velocity (units/s) and acceleration while State::ZEROING
+ ::frc971::ProfileParameters zeroing_profile_params;
+
+ // Maximum velocity (units/s) and acceleration while State::RUNNING if max
+ // velocity or acceleration in goal profile_params is 0
+ ::frc971::ProfileParameters default_profile_params;
+
+ // Maximum range of the subsystem in meters
+ const ::frc971::constants::Range range;
+
+ // Zeroing constants for PotAndABsoluteEncoder estimator
+ const typename zeroing::PotAndAbsoluteEncoderZeroingEstimator::
+ ZeroingConstants zeroing_constants;
+
+ // Function that makes the integral loop for the subsystem
+ ::std::function<StateFeedbackLoop<3, 1, 1>()> make_integral_loop;
+};
+
+// Class for controlling and motion profiling a single degree of freedom
+// subsystem with a zeroing strategy of not moving.
+class StaticZeroingSingleDOFProfiledSubsystem {
+ public:
+ StaticZeroingSingleDOFProfiledSubsystem(
+ const StaticZeroingSingleDOFProfiledSubsystemParams ¶ms);
+
+ // Returns the filtered goal of the profiled subsystem (R)
+ double goal(int row) const { return profiled_subsystem_.goal(row, 0); }
+
+ // Returns the zeroing voltage of the subsystem
+ double zeroing_voltage() { return params_.zeroing_voltage; }
+
+ // Returns the operating voltage of the subsystem
+ double operating_voltage() { return params_.operating_voltage; }
+
+ // Sets further constraints on the range constant
+ void set_min_position(double min_position) { min_position_ = min_position; }
+
+ void set_max_position(double max_position) { max_position_ = max_position; }
+
+ // Resets constrained min/max position
+ void clear_min_position() { min_position_ = params_.range.lower_hard; }
+
+ void clear_max_position() { max_position_ = params_.range.upper_hard; }
+
+ // Returns the current position
+ double position() const { return profiled_subsystem_.position(); }
+
+ void Iterate(
+ const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename zeroing::PotAndAbsoluteEncoderZeroingEstimator::Position
+ *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+
+ // Resets the profiled subsystem and returns to uninitialized
+ void Reset();
+
+ enum class State : int32_t {
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_ = State::UNINITIALIZED;
+ double min_position_, max_position_;
+
+ const StaticZeroingSingleDOFProfiledSubsystemParams params_;
+
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ profiled_subsystem_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
\ No newline at end of file
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
new file mode 100644
index 0000000..0acb21d
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -0,0 +1,479 @@
+#include "gtest/gtest.h"
+
+#include "aos/controls/control_loop_test.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
+
+using ::frc971::control_loops::PositionSensorSimulator;
+
+namespace frc971 {
+namespace control_loops {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+struct TestIntakeSystemValues {
+ static const int kZeroingSampleSize = 200;
+
+ static constexpr double kEncoderRatio =
+ (16.0 * 0.25) * (20.0 / 40.0) / (2.0 * M_PI) * 0.0254;
+ static constexpr double kEncoderIndexDifference = 2.0 * M_PI * kEncoderRatio;
+ static constexpr ::frc971::constants::Range kRange{
+ .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
+
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ static const ::frc971::ProfileParameters kDefaultParams;
+ static const ::frc971::ProfileParameters kZeroingParams;
+
+ static constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ kZeroing{kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
+
+ static const StaticZeroingSingleDOFProfiledSubsystemParams kParams;
+};
+
+constexpr ::frc971::constants::Range TestIntakeSystemValues::kRange;
+constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ TestIntakeSystemValues::kZeroing;
+const ::frc971::ProfileParameters TestIntakeSystemValues::kDefaultParams{0.3,
+ 5.0};
+const ::frc971::ProfileParameters TestIntakeSystemValues::kZeroingParams{0.1,
+ 1.0};
+
+const StaticZeroingSingleDOFProfiledSubsystemParams
+ TestIntakeSystemValues::kParams(
+ {.zeroing_voltage = TestIntakeSystemValues::kZeroingVoltage,
+ .operating_voltage = TestIntakeSystemValues::kOperatingVoltage,
+ .zeroing_profile_params = TestIntakeSystemValues::kZeroingParams,
+ .default_profile_params = TestIntakeSystemValues::kDefaultParams,
+ .range = TestIntakeSystemValues::kRange,
+ .zeroing_constants = TestIntakeSystemValues::kZeroing,
+ .make_integral_loop = MakeIntegralTestIntakeSystemLoop});
+
+struct TestIntakeSystemData {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+
+ ::frc971::control_loops::AbsoluteProfiledJointStatus status;
+
+ ::frc971::PotAndAbsolutePosition position;
+
+ double output;
+};
+
+} // namespace
+
+class TestIntakeSystemSimulation {
+ public:
+ TestIntakeSystemSimulation()
+ : subsystem_plant_(new CappedTestPlant(
+ ::frc971::control_loops::MakeTestIntakeSystemPlant())),
+ subsystem_pot_encoder_(
+ TestIntakeSystemValues::kEncoderIndexDifference) {
+ // Start the subsystem out in the middle by default.
+ InitializeSubsystemPosition((TestIntakeSystemValues::kRange.lower +
+ TestIntakeSystemValues::kRange.upper) /
+ 2.0);
+ }
+
+ void InitializeSubsystemPosition(double start_pos) {
+ subsystem_plant_->mutable_X(0, 0) = start_pos;
+ subsystem_plant_->mutable_X(1, 0) = 0.0;
+
+ subsystem_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues::kZeroing.measured_absolute_position);
+ }
+
+ // Updates the position message with the position of the subsystem.
+ void UpdatePositionMessage(::frc971::PotAndAbsolutePosition *position) {
+ subsystem_pot_encoder_.GetSensorValues(position);
+ }
+
+ double subsystem_position() const { return subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
+
+ // Sets the difference between the commanded and applied powers.
+ // This lets us test that the integrators work.
+ void set_subsystem_voltage_offset(double voltage_offset) {
+ subsystem_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ // Simulates the subsystem for a single timestep.
+ void Simulate(const double output_voltage, const int32_t state) {
+ const double voltage_check_subsystem =
+ (static_cast<StaticZeroingSingleDOFProfiledSubsystem::State>(state) ==
+ StaticZeroingSingleDOFProfiledSubsystem::State::RUNNING)
+ ? TestIntakeSystemValues::kOperatingVoltage
+ : TestIntakeSystemValues::kZeroingVoltage;
+
+ EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
+
+ ::Eigen::Matrix<double, 1, 1> subsystem_U;
+ subsystem_U << output_voltage + subsystem_plant_->voltage_offset();
+ subsystem_plant_->Update(subsystem_U);
+
+ const double position_subsystem = subsystem_plant_->Y(0, 0);
+
+ subsystem_pot_encoder_.MoveTo(position_subsystem);
+
+ EXPECT_GE(position_subsystem, TestIntakeSystemValues::kRange.lower_hard);
+ EXPECT_LE(position_subsystem, TestIntakeSystemValues::kRange.upper_hard);
+ }
+
+ private:
+ ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
+ PositionSensorSimulator subsystem_pot_encoder_;
+};
+
+class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ IntakeSystemTest()
+ : subsystem_(TestIntakeSystemValues::kParams), subsystem_plant_() {}
+
+ void VerifyNearGoal() {
+ EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
+ subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
+ subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(subsystem_data_.status.velocity, 0, 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true, bool null_goal = false) {
+ SendMessages(enabled);
+ subsystem_plant_.UpdatePositionMessage(&subsystem_data_.position);
+
+ // Checks if the robot was reset and resets the subsystem.
+ // Required since there is no ControlLoop to reset it (ie. a superstructure)
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() &&
+ sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ subsystem_.Reset();
+ }
+
+ sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
+ subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
+
+ subsystem_.Iterate(null_goal ? nullptr : &subsystem_goal_,
+ &subsystem_data_.position, &subsystem_data_.output,
+ &subsystem_data_.status);
+
+ subsystem_plant_.Simulate(subsystem_data_.output,
+ subsystem_data_.status.state);
+
+ TickTime(::std::chrono::microseconds(5050));
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
+ bool null_goal = false) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
+ const auto loop_start_time = monotonic_clock::now();
+ double begin_subsystem_velocity = subsystem_plant_.subsystem_velocity();
+
+ RunIteration(enabled, null_goal);
+
+ const double loop_time = chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time)
+ .count();
+ const double subsystem_acceleration =
+ (subsystem_plant_.subsystem_velocity() - begin_subsystem_velocity) /
+ loop_time;
+ EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
+ EXPECT_NEAR(subsystem_plant_.subsystem_velocity(), 0.0,
+ peak_subsystem_velocity_);
+ }
+ }
+
+ void set_peak_subsystem_acceleration(double value) {
+ peak_subsystem_acceleration_ = value;
+ }
+ void set_peak_subsystem_velocity(double value) {
+ peak_subsystem_velocity_ = value;
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory
+ // that is no longer valid.
+ // TestIntakeSystemData subsystem_data_;
+
+ // Create a control loop and simulation.
+ StaticZeroingSingleDOFProfiledSubsystem subsystem_;
+ TestIntakeSystemSimulation subsystem_plant_;
+
+ StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
+
+ TestIntakeSystemData subsystem_data_;
+
+ private:
+ // The acceleration limits to check for while moving.
+ double peak_subsystem_acceleration_ = 1e10;
+ // The velocity limits to check for while moving.
+ double peak_subsystem_velocity_ = 1e10;
+
+ int32_t sensor_reader_pid_ = 0;
+};
+
+// Tests that the subsystem does nothing when the goal is zero.
+TEST_F(IntakeSystemTest, DoesNothing) {
+ // Intake system uses 0.05 to test for 0.
+ subsystem_data_.goal.unsafe_goal = 0.05;
+ RunForTime(chrono::seconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the subsystem loop can reach a goal.
+TEST_F(IntakeSystemTest, ReachesGoal) {
+ // Set a reasonable goal.
+ auto &goal = subsystem_data_.goal;
+ goal.unsafe_goal = 0.1;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+TEST_F(IntakeSystemTest, SaturationTest) {
+ // Zero it before we move.
+ auto &goal = subsystem_data_.goal;
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.lower;
+ goal.profile_params.max_velocity = 20.0;
+ goal.profile_params.max_acceleration = 0.1;
+ }
+ set_peak_subsystem_velocity(23.0);
+ set_peak_subsystem_acceleration(0.2);
+
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Now do a high acceleration move with a low velocity limit.
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.profile_params.max_velocity = 0.1;
+ goal.profile_params.max_acceleration = 100;
+ }
+
+ set_peak_subsystem_velocity(0.2);
+ set_peak_subsystem_acceleration(103);
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Tests that the subsystem loop doesn't try and go beyond it's physical range
+// of the mechanisms.
+TEST_F(IntakeSystemTest, RespectsRange) {
+ auto &goal = subsystem_data_.goal;
+ // Set some ridiculous goals to test upper limits.
+ {
+ goal.unsafe_goal = 100.0;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ EXPECT_NEAR(TestIntakeSystemValues::kRange.upper,
+ subsystem_data_.status.position, 0.001);
+
+ // Set some ridiculous goals to test lower limits.
+ {
+ goal.unsafe_goal = -100.0;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ EXPECT_NEAR(TestIntakeSystemValues::kRange.lower,
+ subsystem_data_.status.position, 0.001);
+}
+
+// Tests that the subsystem loop zeroes when run for a while.
+TEST_F(IntakeSystemTest, ZeroTest) {
+ auto &goal = subsystem_data_.goal;
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(IntakeSystemTest, ZeroNoGoal) {
+ RunForTime(chrono::seconds(5));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem::State::RUNNING,
+ subsystem_.state());
+}
+
+TEST_F(IntakeSystemTest, LowerHardstopStartup) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.lower_hard);
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(IntakeSystemTest, UpperHardstopStartup) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.upper_hard);
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(IntakeSystemTest, ResetTest) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.upper);
+ {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.upper - 0.1;
+ }
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem::State::RUNNING,
+ subsystem_.state());
+
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(chrono::milliseconds(100));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem::State::UNINITIALIZED,
+ subsystem_.state());
+
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem::State::RUNNING,
+ subsystem_.state());
+ VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(IntakeSystemTest, DisabledGoalTest) {
+ {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.lower + 0.03;
+ }
+
+ // Checks that the subsystem has not moved from its starting position at 0
+ RunForTime(chrono::milliseconds(100), false);
+ EXPECT_EQ(0.0, subsystem_.goal(0));
+
+ // Now make sure they move correctly
+ RunForTime(chrono::seconds(4), true);
+ EXPECT_NE(0.0, subsystem_.goal(0));
+}
+
+// Tests that zeroing while disabled works.
+TEST_F(IntakeSystemTest, DisabledZeroTest) {
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower; }
+
+ // Run disabled for 2 seconds
+ RunForTime(chrono::seconds(2), false);
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem::State::RUNNING,
+ subsystem_.state());
+
+ RunForTime(chrono::seconds(4), true);
+
+ VerifyNearGoal();
+}
+
+// Tests that set_min_position limits range properly
+TEST_F(IntakeSystemTest, MinPositionTest) {
+ subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower_hard;
+ RunForTime(chrono::seconds(2), true);
+
+ // Check that kRange.lower is used as the default min position
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower, 0.001);
+
+ // Set min position and check that the subsystem increases to that position
+ subsystem_.set_min_position(TestIntakeSystemValues::kRange.lower + 0.05);
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower + 0.05);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower + 0.05, 0.001);
+
+ // Clear min position and check that the subsystem returns to kRange.lower
+ subsystem_.clear_min_position();
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower, 0.001);
+}
+
+// Tests that set_max_position limits range properly
+TEST_F(IntakeSystemTest, MaxPositionTest) {
+ subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper_hard;
+ RunForTime(chrono::seconds(2), true);
+
+ // Check that kRange.upper is used as the default max position
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper, 0.001);
+
+ // Set max position and check that the subsystem lowers to that position
+ subsystem_.set_max_position(TestIntakeSystemValues::kRange.upper - 0.05);
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper - 0.05);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper - 0.05, 0.001);
+
+ // Clear max position and check that the subsystem returns to kRange.upper
+ subsystem_.clear_max_position();
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper, 0.001);
+}
+
+// Tests that the subsystem maintains its current position when sent a null goal
+TEST_F(IntakeSystemTest, NullGoalTest) {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.lower + 0.05;
+ RunForTime(chrono::seconds(2), true);
+
+ VerifyNearGoal();
+
+ // Run with a null goal
+ RunForTime(chrono::seconds(2), true, true);
+
+ // Check that the subsystem has not moved
+ VerifyNearGoal();
+}
+
+} // namespace control_loops
+} // namespace frc971