Move subsystem simulator to 971
Signed-off-by: Nikita Narang <nikitanarang123@gmail.com>
Change-Id: Ida52062678697ae02cc71d1a9d5870871299751c
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index caf56ab..3bc1297 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -126,6 +126,7 @@
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:control_loop_test",
"//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:subsystem_simulator",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
],
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 3d997da..7e2acab 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -13,6 +13,7 @@
#include "y2022/control_loops/superstructure/intake/intake_plant.h"
#include "y2022/control_loops/superstructure/superstructure.h"
#include "y2022/control_loops/superstructure/turret/turret_plant.h"
+#include "frc971/control_loops/subsystem_simulator.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
@@ -22,9 +23,6 @@
namespace superstructure {
namespace testing {
namespace chrono = std::chrono;
-namespace {
-constexpr double kNoiseScalar = 0.01;
-}
using ::aos::monotonic_clock;
using ::frc971::CreateProfileParameters;
@@ -37,89 +35,11 @@
PotAndAbsoluteEncoderSubsystem;
typedef Superstructure::RelativeEncoderSubsystem RelativeEncoderSubsystem;
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
-
-template <typename SubsystemStatus, typename SubsystemState,
- typename SubsystemConstants>
-class SubsystemSimulator {
- public:
- SubsystemSimulator(CappedTestPlant *plant, PositionSensorSimulator encoder,
- const SubsystemConstants subsystem_constants,
- const frc971::constants::Range range,
- double encoder_offset, const chrono::nanoseconds dt)
- : plant_(plant),
- encoder_(encoder),
- subsystem_constants_(subsystem_constants),
- range_(range),
- encoder_offset_(encoder_offset),
- dt_(dt) {}
-
- void InitializePosition(double start_pos) {
- plant_->mutable_X(0, 0) = start_pos;
- plant_->mutable_X(1, 0) = 0.0;
-
- encoder_.Initialize(start_pos, kNoiseScalar, 0.0, encoder_offset_);
- }
-
- // Simulates the superstructure for a single timestep.
- void Simulate(double voltage, const SubsystemStatus *status) {
- double last_velocity = plant_->X(1, 0);
-
- const double voltage_check =
- (static_cast<SubsystemState>(status->state()) ==
- SubsystemState::RUNNING)
- ? subsystem_constants_.subsystem_params.operating_voltage
- : subsystem_constants_.subsystem_params.zeroing_voltage;
-
- EXPECT_NEAR(voltage, 0.0, voltage_check);
-
- ::Eigen::Matrix<double, 1, 1> U;
- U << voltage + plant_->voltage_offset();
- plant_->Update(U);
-
- const double position = plant_->Y(0, 0);
-
- encoder_.MoveTo(position);
-
- EXPECT_GE(position, range_.lower_hard);
- EXPECT_LE(position, range_.upper_hard);
-
- const double loop_time = ::aos::time::DurationInSeconds(dt_);
-
- const double velocity = plant_->X(1, 0);
- const double acceleration = (velocity - last_velocity) / loop_time;
-
- EXPECT_GE(peak_acceleration_, acceleration);
- EXPECT_LE(-peak_acceleration_, acceleration);
- EXPECT_GE(peak_velocity_, velocity);
- EXPECT_LE(-peak_velocity_, velocity);
- }
-
- void set_peak_acceleration(double value) { peak_acceleration_ = value; }
- void set_peak_velocity(double value) { peak_velocity_ = value; }
-
- void set_controller_index(size_t index) { plant_->set_index(index); }
-
- PositionSensorSimulator *encoder() { return &encoder_; }
-
- private:
- std::unique_ptr<CappedTestPlant> plant_;
- PositionSensorSimulator encoder_;
- const SubsystemConstants subsystem_constants_;
- const frc971::constants::Range range_;
-
- double encoder_offset_ = 0.0;
-
- double peak_velocity_ = std::numeric_limits<double>::infinity();
- double peak_acceleration_ = std::numeric_limits<double>::infinity();
-
- const chrono::nanoseconds dt_;
-};
-
-using PotAndAbsoluteEncoderSimulator = SubsystemSimulator<
+using PotAndAbsoluteEncoderSimulator = frc971::control_loops::SubsystemSimulator<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
PotAndAbsoluteEncoderSubsystem::State,
constants::Values::PotAndAbsEncoderConstants>;
-using RelativeEncoderSimulator = SubsystemSimulator<
+using RelativeEncoderSimulator = frc971::control_loops::SubsystemSimulator<
frc971::control_loops::RelativeEncoderProfiledJointStatus,
RelativeEncoderSubsystem::State, constants::Values::PotConstants>;