Move subsystem simulator to 971
Signed-off-by: Nikita Narang <nikitanarang123@gmail.com>
Change-Id: Ida52062678697ae02cc71d1a9d5870871299751c
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 459929b..ee53c4a 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -5,6 +5,21 @@
package(default_visibility = ["//visibility:public"])
cc_library(
+ name = "subsystem_simulator",
+ testonly = True,
+ hdrs = [
+ "subsystem_simulator.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971:constants",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ ],
+)
+
+cc_library(
name = "control_loop_test",
testonly = True,
srcs = [
diff --git a/frc971/control_loops/subsystem_simulator.h b/frc971/control_loops/subsystem_simulator.h
new file mode 100644
index 0000000..29d62c7
--- /dev/null
+++ b/frc971/control_loops/subsystem_simulator.h
@@ -0,0 +1,91 @@
+#include <chrono>
+#include <memory>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+
+namespace frc971::control_loops {
+
+template <typename SubsystemStatus, typename SubsystemState,
+ typename SubsystemConstants>
+
+// Class used for simulating a single degree of freedom subsystem in test.
+// Simulates the state of the subsystem as a voltage is applied
+class SubsystemSimulator {
+ public:
+ SubsystemSimulator(CappedTestPlant *plant, PositionSensorSimulator encoder,
+ const SubsystemConstants subsystem_constants,
+ const frc971::constants::Range range,
+ double encoder_offset, const std::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, 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 std::chrono::nanoseconds dt_;
+};
+
+} // namespace frc971::control_loops
\ No newline at end of file
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>;
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