Templatize StaticZeroingSingleDOFProfiledSubsystemTest
Change-Id: Iaf7db996eb0750009ad0b8d4a4c5f9dcb75e8331
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index f3015c6..1b6b965 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -154,6 +154,44 @@
.frc971.IndexEstimatorState estimator_state;
};
+struct AbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.AbsoluteEncoderEstimatorState estimator_state;
+};
+
struct StaticZeroingSingleDOFProfiledSubsystemGoal {
double unsafe_goal;
.frc971.ProfileParameters profile_params;
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 2709315..7b7b370 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -22,10 +22,10 @@
::frc971::ProfileParameters default_profile_params;
// Maximum range of the subsystem in meters
- const ::frc971::constants::Range range;
+ ::frc971::constants::Range range;
// Zeroing constants for PotAndABsoluteEncoder estimator
- const typename ZeroingEstimator::ZeroingConstants zeroing_constants;
+ typename ZeroingEstimator::ZeroingConstants zeroing_constants;
// Function that makes the integral loop for the subsystem
::std::function<StateFeedbackLoop<3, 1, 1>()> make_integral_loop;
@@ -33,13 +33,16 @@
// Class for controlling and motion profiling a single degree of freedom
// subsystem with a zeroing strategy of not moving.
-template <typename ZeroingEstimator>
+template <typename TZeroingEstimator, typename TProfiledJointStatus>
class StaticZeroingSingleDOFProfiledSubsystem {
public:
StaticZeroingSingleDOFProfiledSubsystem(
- const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ const StaticZeroingSingleDOFProfiledSubsystemParams<TZeroingEstimator>
¶ms);
+ using ZeroingEstimator = TZeroingEstimator;
+ using ProfiledJointStatus = TProfiledJointStatus;
+
// Returns the filtered goal of the profiled subsystem (R)
double goal(int row) const { return profiled_subsystem_.goal(row, 0); }
@@ -64,9 +67,7 @@
void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position,
- double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
- *status);
+ double *output, ProfiledJointStatus *status);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -91,8 +92,8 @@
profiled_subsystem_;
};
-template <typename ZeroingEstimator>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
StaticZeroingSingleDOFProfiledSubsystem(
const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
¶ms)
@@ -101,26 +102,26 @@
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- 3, 1, 1>(params.make_integral_loop())),
+ 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();
};
-template <typename ZeroingEstimator>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Reset() {
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
profiled_subsystem_.Reset();
}
-template <typename ZeroingEstimator>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Iterate(
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
+ ProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -214,4 +215,4 @@
} // namespace control_loops
} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
\ No newline at end of file
+#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
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 df26c56..9a9c092 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
@@ -17,85 +17,102 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+using SZSDPS_PotAndAbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+using SZSDPS_AbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+typedef ::testing::Types<SZSDPS_AbsEncoder, SZSDPS_PotAndAbsEncoder> 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;
+
+constexpr double kEncoderIndexDifference = 1.0;
+
+template <typename ZeroingEstimator>
struct TestIntakeSystemValues {
- static const int kZeroingSampleSize = 200;
+ static const typename ZeroingEstimator::ZeroingConstants kZeroing;
- 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<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- kParams;
+ static const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ make_params();
};
-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};
+template <>
+const zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
+ kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
-const StaticZeroingSingleDOFProfiledSubsystemParams<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- 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});
+template <>
+const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
+ kZeroingSampleSize,
+ kEncoderIndexDifference,
+ 0.0,
+ 0.2,
+ 0.0005,
+ 20,
+ 1.9};
+template <typename ZeroingEstimator>
+const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+TestIntakeSystemValues<ZeroingEstimator>::make_params() {
+ StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params(
+ {kZeroingVoltage,
+ kOperatingVoltage,
+ {0.1, 1.0},
+ {0.3, 5.0},
+ kRange,
+ TestIntakeSystemValues::kZeroing,
+ &MakeIntegralTestIntakeSystemLoop});
+ return params;
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
struct TestIntakeSystemData {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus status;
+ ProfiledJointStatus status;
- ::frc971::PotAndAbsolutePosition position;
+ typename ZeroingEstimator::Position position;
double output;
};
} // namespace
+template <typename SZSDPS>
class TestIntakeSystemSimulation {
public:
TestIntakeSystemSimulation()
: subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
- subsystem_pot_encoder_(
- TestIntakeSystemValues::kEncoderIndexDifference) {
+ subsystem_sensor_sim_(
+ kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
- InitializeSubsystemPosition((TestIntakeSystemValues::kRange.lower +
- TestIntakeSystemValues::kRange.upper) /
- 2.0);
+ InitializeSubsystemPosition((kRange.lower + 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);
+ InitializeSensorSim(start_pos);
}
+ void InitializeSensorSim(double start_pos);
+
// Updates the position message with the position of the subsystem.
- void UpdatePositionMessage(::frc971::PotAndAbsolutePosition *position) {
- subsystem_pot_encoder_.GetSensorValues(position);
+ void UpdatePositionMessage(
+ typename SZSDPS::ZeroingEstimator::Position *position) {
+ subsystem_sensor_sim_.GetSensorValues(position);
}
double subsystem_position() const { return subsystem_plant_->X(0, 0); }
@@ -110,12 +127,9 @@
// 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<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State>(state) ==
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING)
- ? TestIntakeSystemValues::kOperatingVoltage
- : TestIntakeSystemValues::kZeroingVoltage;
+ (static_cast<typename SZSDPS::State>(state) == SZSDPS::State::RUNNING)
+ ? kOperatingVoltage
+ : kZeroingVoltage;
EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
@@ -125,21 +139,48 @@
const double position_subsystem = subsystem_plant_->Y(0, 0);
- subsystem_pot_encoder_.MoveTo(position_subsystem);
+ subsystem_sensor_sim_.MoveTo(position_subsystem);
- EXPECT_GE(position_subsystem, TestIntakeSystemValues::kRange.lower_hard);
- EXPECT_LE(position_subsystem, TestIntakeSystemValues::kRange.upper_hard);
+ EXPECT_GE(position_subsystem, kRange.lower_hard);
+ EXPECT_LE(position_subsystem, kRange.upper_hard);
}
private:
::std::unique_ptr<CappedTestPlant> subsystem_plant_;
- PositionSensorSimulator subsystem_pot_encoder_;
+ PositionSensorSimulator subsystem_sensor_sim_;
};
+template <>
+void TestIntakeSystemSimulation<SZSDPS_PotAndAbsEncoder>::InitializeSensorSim(
+ double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <>
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder>::InitializeSensorSim(
+ double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <typename TSZSDPS>
class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
protected:
+ using SZSDPS = TSZSDPS;
+ using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
+ using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
+
IntakeSystemTest()
- : subsystem_(TestIntakeSystemValues::kParams), subsystem_plant_() {}
+ : subsystem_(TestIntakeSystemValues<
+ typename SZSDPS::ZeroingEstimator>::make_params()),
+ subsystem_plant_() {}
void VerifyNearGoal() {
EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
@@ -212,14 +253,14 @@
// TestIntakeSystemData subsystem_data_;
// Create a control loop and simulation.
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- subsystem_;
- TestIntakeSystemSimulation subsystem_plant_;
+ SZSDPS subsystem_;
+ TestIntakeSystemSimulation<SZSDPS> subsystem_plant_;
StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
- TestIntakeSystemData subsystem_data_;
+ TestIntakeSystemData<typename SZSDPS::ZeroingEstimator,
+ typename SZSDPS::ProfiledJointStatus>
+ subsystem_data_;
private:
// The acceleration limits to check for while moving.
@@ -230,80 +271,81 @@
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));
+TYPED_TEST_CASE_P(IntakeSystemTest);
- VerifyNearGoal();
+// Tests that the subsystem does nothing when the goal is zero.
+TYPED_TEST_P(IntakeSystemTest, DoesNothing) {
+ // Intake system uses 0.05 to test for 0.
+ this->subsystem_data_.goal.unsafe_goal = 0.05;
+ this->RunForTime(chrono::seconds(5));
+
+ this->VerifyNearGoal();
}
// Tests that the subsystem loop can reach a goal.
-TEST_F(IntakeSystemTest, ReachesGoal) {
+TYPED_TEST_P(IntakeSystemTest, ReachesGoal) {
// Set a reasonable goal.
- auto &goal = subsystem_data_.goal;
+ auto &goal = this->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));
+ this->RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->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) {
+TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
// Zero it before we move.
- auto &goal = subsystem_data_.goal;
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
- RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ auto &goal = this->subsystem_data_.goal;
+ goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(8));
+ this->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.unsafe_goal = 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);
+ this->set_peak_subsystem_velocity(23.0);
+ this->set_peak_subsystem_acceleration(0.2);
- RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->RunForTime(chrono::seconds(8));
+ this->VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.unsafe_goal = 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));
+ this->set_peak_subsystem_velocity(0.2);
+ this->set_peak_subsystem_acceleration(103);
+ this->RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->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;
+TYPED_TEST_P(IntakeSystemTest, RespectsRange) {
+ auto &goal = this->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));
+ this->RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(TestIntakeSystemValues::kRange.upper,
- subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(kRange.upper, this->subsystem_data_.status.position, 0.001);
// Set some ridiculous goals to test lower limits.
{
@@ -312,181 +354,163 @@
goal.profile_params.max_acceleration = 0.5;
}
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(TestIntakeSystemValues::kRange.lower,
- subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(kRange.lower, this->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;
+TYPED_TEST_P(IntakeSystemTest, ZeroTest) {
+ auto &goal = this->subsystem_data_.goal;
{
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.unsafe_goal = kRange.upper;
goal.profile_params.max_velocity = 1;
goal.profile_params.max_acceleration = 0.5;
}
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
-TEST_F(IntakeSystemTest, ZeroNoGoal) {
- RunForTime(chrono::seconds(5));
+TYPED_TEST_P(IntakeSystemTest, ZeroNoGoal) {
+ this->RunForTime(chrono::seconds(5));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->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));
+TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->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));
+TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->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));
+TYPED_TEST_P(IntakeSystemTest, ResetTest) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper - 0.1;
+ this->RunForTime(chrono::seconds(10));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
- VerifyNearGoal();
- SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ this->VerifyNearGoal();
+ this->SimulateSensorReset();
+ this->RunForTime(chrono::milliseconds(100));
- EXPECT_EQ(
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::UNINITIALIZED,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::UNINITIALIZED,
+ this->subsystem_.state());
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
- VerifyNearGoal();
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->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;
- }
+TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
+ this->subsystem_data_.goal.unsafe_goal = 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));
+ this->RunForTime(chrono::milliseconds(100), false);
+ EXPECT_EQ(0.0, this->subsystem_.goal(0));
// Now make sure they move correctly
- RunForTime(chrono::seconds(4), true);
- EXPECT_NE(0.0, subsystem_.goal(0));
+ this->RunForTime(chrono::seconds(4), true);
+ EXPECT_NE(0.0, this->subsystem_.goal(0));
}
// Tests that zeroing while disabled works.
-TEST_F(IntakeSystemTest, DisabledZeroTest) {
- { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower; }
+TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower;
// Run disabled for 2 seconds
- RunForTime(chrono::seconds(2), false);
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ this->RunForTime(chrono::seconds(2), false);
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
- RunForTime(chrono::seconds(4), true);
+ this->RunForTime(chrono::seconds(4), true);
- VerifyNearGoal();
+ this->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);
+TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower_hard;
+ this->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);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+ this->subsystem_.set_min_position(kRange.lower + 0.05);
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower + 0.05);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+ this->subsystem_.clear_min_position();
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+TYPED_TEST_P(IntakeSystemTest, MaxPositionTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper_hard;
+ this->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);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+ this->subsystem_.set_max_position(kRange.upper - 0.05);
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper - 0.05);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+ this->subsystem_.clear_max_position();
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
+ EXPECT_NEAR(this->subsystem_data_.status.position, 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);
+TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.05;
+ this->RunForTime(chrono::seconds(2), true);
- VerifyNearGoal();
+ this->VerifyNearGoal();
// Run with a null goal
- RunForTime(chrono::seconds(2), true, true);
+ this->RunForTime(chrono::seconds(2), true, true);
// Check that the subsystem has not moved
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
+REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
+ SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
+ LowerHardstopStartup, UpperHardstopStartup,
+ ResetTest, DisabledGoalTest, DisabledZeroTest,
+ MinPositionTest, MaxPositionTest, NullGoalTest);
+INSTANTIATE_TYPED_TEST_CASE_P(My, IntakeSystemTest, TestTypes);
+
} // namespace control_loops
} // namespace frc971