Move subsystem simulator to 971
Signed-off-by: Nikita Narang <nikitanarang123@gmail.com>
Change-Id: Ida52062678697ae02cc71d1a9d5870871299751c
diff --git a/y2022_bot3/control_loops/superstructure/BUILD b/y2022_bot3/control_loops/superstructure/BUILD
index 9bafadc..7d89a0a 100644
--- a/y2022_bot3/control_loops/superstructure/BUILD
+++ b/y2022_bot3/control_loops/superstructure/BUILD
@@ -112,6 +112,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_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2022_bot3/control_loops/superstructure/superstructure_lib_test.cc
index c34a1d2..8eb6064 100644
--- a/y2022_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -5,6 +5,7 @@
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/subsystem_simulator.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2022_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
@@ -18,9 +19,6 @@
namespace superstructure {
namespace testing {
namespace chrono = std::chrono;
-namespace {
-constexpr double kNoiseScalar = 0.01;
-}
using ::aos::monotonic_clock;
using ::frc971::CreateProfileParameters;
@@ -29,96 +27,15 @@
CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
-using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+using PotAndAbsoluteEncoderSimulator =
+ frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
-template <typename SubsystemStatus, typename SubsystemState,
- typename SubsystemConstants>
-
-// TODO(Henry) put this in frc971
-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<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
- PotAndAbsoluteEncoderSubsystem::State,
- constants::Values::PotAndAbsEncoderConstants>;
-
-// Class which simulates the superstructure and sends out queue messages with
-// the position.
class SuperstructureSimulation {
public:
SuperstructureSimulation(::aos::EventLoop *event_loop,
@@ -557,4 +474,4 @@
} // namespace testing
} // namespace superstructure
} // namespace control_loops
-} // namespace y2022_bot3
+} // namespace y2022_bot3
\ No newline at end of file