Add intake and turret code plus superstructure tests
Signed-off-by: Milo Lin <100027790@mvla.net>
Change-Id: I9885bd1e839ba0356147606415ae915cd295faf6
Change-Id: I33bc83673645869e255136198c0789f722c881a0
Signed-off-by: Siddhartha Chatterjee <ninja.siddhartha@gmail.com>
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index ccfb0ee..9bae7c7 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -62,6 +62,7 @@
":superstructure_status_fbs",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2022:constants",
],
)
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index a7af922..bf04774 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -8,13 +8,21 @@
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-using frc971::control_loops:: RelativeEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- climber_(constants::GetValues().climber.subsystem_params) {
+
+ climber_(values->climber.subsystem_params),
+ intake_front_(values->intake_front.subsystem_params),
+ intake_back_(values->intake_back.subsystem_params),
+ turret_(values->turret.subsystem_params),
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -24,6 +32,29 @@
aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ intake_front_.Reset();
+ intake_back_.Reset();
+ turret_.Reset();
+ climber_.Reset();
+ }
+
+ drivetrain_status_fetcher_.Fetch();
+ const float velocity = robot_velocity();
+
+ double roller_speed_compensated_front = 0;
+ double roller_speed_compensated_back = 0;
+ double transfer_roller_speed = 0;
+
+ if (unsafe_goal != nullptr) {
+ roller_speed_compensated_front =
+ unsafe_goal->roller_speed_front() +
+ std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+ roller_speed_compensated_back =
+ unsafe_goal->roller_speed_back() -
+ std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+ transfer_roller_speed = unsafe_goal->transfer_roller_speed();
}
OutputT output_struct;
@@ -35,20 +66,59 @@
output != nullptr ? &(output_struct.climber_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_status_offset_front = intake_front_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
+ position->intake_front(),
+ output != nullptr ? &(output_struct.intake_voltage_front) : nullptr,
+ status->fbb());
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_status_offset_back = intake_back_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
+ position->intake_back(),
+ output != nullptr ? &(output_struct.intake_voltage_back) : nullptr,
+ status->fbb());
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ turret_status_offset = turret_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+ position->turret(),
+ output != nullptr ? &(output_struct.turret_voltage) : nullptr,
+ status->fbb());
+
if (output != nullptr) {
+ output_struct.roller_voltage_front = roller_speed_compensated_front;
+ output_struct.roller_voltage_back = roller_speed_compensated_back;
+ output_struct.transfer_roller_voltage = transfer_roller_speed;
+
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- // Climber is always zeroed; only has a pot
- status_builder.add_zeroed(climber_.zeroed());
- status_builder.add_estopped(climber_.estopped());
+ const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
+ turret_.zeroed() && climber_.zeroed();
+ const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
+ turret_.estopped() || climber_.zeroed();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_intake_front(intake_status_offset_front);
+ status_builder.add_intake_back(intake_status_offset_back);
+ status_builder.add_turret(turret_status_offset);
status_builder.add_climber(climber_status_offset);
(void)status->Send(status_builder.Finish());
}
+double Superstructure::robot_velocity() const {
+ return (drivetrain_status_fetcher_.get() != nullptr
+ ? drivetrain_status_fetcher_->robot_speed()
+ : 0.0);
+}
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2022
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 4c03a08..d97befc 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
@@ -21,9 +22,28 @@
::frc971::zeroing::RelativeEncoderZeroingEstimator,
::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
+ inline const PotAndAbsoluteEncoderSubsystem &intake_front() const {
+ return intake_front_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &intake_back() const {
+ return intake_back_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &turret() const {
+ return turret_;
+ }
+ inline const RelativeEncoderSubsystem &climber() const { return climber_; }
+
+ double robot_velocity() const;
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -31,6 +51,12 @@
private:
RelativeEncoderSubsystem climber_;
+ PotAndAbsoluteEncoderSubsystem intake_front_;
+ PotAndAbsoluteEncoderSubsystem intake_back_;
+ PotAndAbsoluteEncoderSubsystem turret_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8107363..37e63b5 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -5,6 +5,22 @@
table Goal {
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
+
+ // Goal angles of intake joints.
+ // Positive is out, 0 is up.
+ intake_front:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
+ intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
+
+ // Positive is rollers intaking.
+ roller_speed_front:double (id: 3);
+ roller_speed_back:double (id: 4);
+ // One transfer motor for both sides
+ transfer_roller_speed:double (id: 5);
+
+ // Factor to multiply robot velocity by and add to roller voltage.
+ roller_speed_compensation:double (id: 6);
+
+ turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 82c25b3..493fe95 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,6 +16,237 @@
namespace control_loops {
namespace superstructure {
namespace testing {
+namespace chrono = std::chrono;
+namespace {
+constexpr double kNoiseScalar = 0.01;
+}
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ 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; }
+
+ 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>;
+using RelativeEncoderSimulator = SubsystemSimulator<
+ frc971::control_loops::RelativeEncoderProfiledJointStatus,
+ RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")),
+ intake_front_(new CappedTestPlant(intake::MakeIntakePlant()),
+ PositionSensorSimulator(
+ values->intake_front.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->intake_front, constants::Values::kIntakeRange(),
+ values->intake_front.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ intake_back_(new CappedTestPlant(intake::MakeIntakePlant()),
+ PositionSensorSimulator(
+ values->intake_back.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->intake_back, constants::Values::kIntakeRange(),
+ values->intake_back.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ turret_(new CappedTestPlant(turret::MakeTurretPlant()),
+ PositionSensorSimulator(
+ values->turret.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->turret, constants::Values::kTurretRange(),
+ values->turret.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ climber_(new CappedTestPlant(climber::MakeClimberPlant()),
+ PositionSensorSimulator(
+ constants::Values::kClimberPotMetersPerRevolution()),
+ values->climber, constants::Values::kClimberRange(),
+ values->climber.potentiometer_offset, dt_) {
+ intake_front_.InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ intake_back_.InitializePosition(constants::Values::kIntakeRange().middle());
+ turret_.InitializePosition(constants::Values::kTurretRange().middle());
+ climber_.InitializePosition(constants::Values::kClimberRange().middle());
+
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ intake_front_.Simulate(
+ superstructure_output_fetcher_->intake_voltage_front(),
+ superstructure_status_fetcher_->intake_front());
+ intake_back_.Simulate(
+ superstructure_output_fetcher_->intake_voltage_back(),
+ superstructure_status_fetcher_->intake_back());
+ turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
+ superstructure_status_fetcher_->turret());
+ climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
+ superstructure_status_fetcher_->climber());
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
+
+ frc971::PotAndAbsolutePosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ turret_.encoder()->GetSensorValues(&turret_builder);
+
+ frc971::PotAndAbsolutePosition::Builder intake_front_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_front_offset =
+ intake_front_.encoder()->GetSensorValues(&intake_front_builder);
+
+ frc971::PotAndAbsolutePosition::Builder intake_back_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_back_offset =
+ intake_back_.encoder()->GetSensorValues(&intake_back_builder);
+
+ frc971::RelativePosition::Builder climber_builder =
+ builder.MakeBuilder<frc971::RelativePosition>();
+ flatbuffers::Offset<frc971::RelativePosition> climber_offset =
+ climber_.encoder()->GetSensorValues(&climber_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake_front(intake_front_offset);
+ position_builder.add_intake_back(intake_back_offset);
+ position_builder.add_turret(turret_offset);
+ position_builder.add_climber(climber_offset);
+
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
+ PotAndAbsoluteEncoderSimulator *intake_front() { return &intake_front_; }
+ PotAndAbsoluteEncoderSimulator *intake_back() { return &intake_back_; }
+ PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
+ RelativeEncoderSimulator *climber() { return &climber_; }
+
+ private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
+
+ PotAndAbsoluteEncoderSimulator intake_front_;
+ PotAndAbsoluteEncoderSimulator intake_back_;
+ PotAndAbsoluteEncoderSimulator turret_;
+ RelativeEncoderSimulator climber_;
+};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
public:
@@ -23,9 +254,12 @@
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2022/config.json"),
std::chrono::microseconds(5050)),
- superstructure_event_loop(MakeEventLoop("Superstructure")),
- superstructure_(superstructure_event_loop.get()),
- test_event_loop_(MakeEventLoop("test")),
+ values_(std::make_shared<constants::Values>(constants::MakeValues(
+ frc971::control_loops::testing::kTeamNumber))),
+ roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+ superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+ superstructure_(superstructure_event_loop.get(), values_),
+ test_event_loop_(MakeEventLoop("test", roborio_)),
superstructure_goal_fetcher_(
test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
@@ -37,12 +271,15 @@
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_position_sender_(
- test_event_loop_->MakeSender<Position>("/superstructure")) {
+ test_event_loop_->MakeSender<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), values_,
+ dt()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
- SetEnabled(true);
- phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
- [this](int) { SendPositionMessage(); }, dt());
+ SetEnabled(true);
if (!FLAGS_output_folder.empty()) {
unlink(FLAGS_output_folder.c_str());
@@ -52,14 +289,119 @@
}
}
- void SendPositionMessage() {
- auto builder = superstructure_position_sender_.MakeBuilder();
- Position::Builder position_builder = builder.MakeBuilder<Position>();
- builder.CheckOk(builder.Send(position_builder.Finish()));
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ // Only check the goal if there is one.
+
+ if (superstructure_goal_fetcher_->has_intake_front()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake_front()->unsafe_goal(),
+ superstructure_status_fetcher_->intake_front()->position(),
+ 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_intake_back()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake_back()->unsafe_goal(),
+ superstructure_status_fetcher_->intake_back()->position(),
+ 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_turret()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_climber()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
+ superstructure_status_fetcher_->climber()->position(), 0.001);
+ }
}
- // Because the third robot is single node, the roborio node is nullptr
- const aos::Node *const roborio_ = nullptr;
+ void CheckIfZeroed() {
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+ ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
+ } while (superstructure_status_fetcher_.get() == nullptr ||
+ !superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void SendRobotVelocity(double robot_velocity) {
+ // Send a robot velocity to test compensation
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+ auto drivetrain_status_builder = builder.MakeBuilder<DrivetrainStatus>();
+ drivetrain_status_builder.add_robot_speed(robot_velocity);
+ builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+ }
+
+ void TestRollerFront(double roller_speed_front,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_speed_front(roller_speed_front);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+ roller_speed_front + std::max((superstructure_.robot_velocity() *
+ roller_speed_compensation),
+ 0.0));
+ if (superstructure_.robot_velocity() <= 0) {
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+ roller_speed_front);
+ }
+ }
+
+ void TestRollerBack(double roller_speed_back,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_speed_back(roller_speed_back);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+ roller_speed_back - std::min(superstructure_.robot_velocity() *
+ roller_speed_compensation,
+ 0.0));
+ if (superstructure_.robot_velocity() >= 0) {
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+ roller_speed_back);
+ }
+ }
+
+ void TestTransferRoller(double transfer_roller_speed,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_transfer_roller_speed(transfer_roller_speed);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ transfer_roller_speed);
+ }
+
+ std::shared_ptr<const constants::Values> values_;
+
+ const aos::Node *const roborio_;
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
::y2022::control_loops::superstructure::Superstructure superstructure_;
@@ -72,11 +414,306 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
};
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
+TEST_F(SuperstructureTest, DoesNothing) {
+ SetEnabled(true);
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.turret()->InitializePosition(
+ constants::Values::kTurretRange().middle());
+ superstructure_plant_.climber()->InitializePosition(
+ constants::Values::kClimberRange().middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().middle());
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.turret()->InitializePosition(
+ constants::Values::kTurretRange().middle());
+ superstructure_plant_.climber()->InitializePosition(
+ constants::Values::kClimberRange().middle());
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(15));
+
+ 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(SuperstructureTest, SaturationTest) {
+ SetEnabled(true);
+ // Zero it before we move.
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().upper);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_front()->set_peak_acceleration(0.2);
+ superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_back()->set_peak_acceleration(0.2);
+
+ superstructure_plant_.turret()->set_peak_velocity(23.0);
+ superstructure_plant_.turret()->set_peak_acceleration(6.0);
+
+ superstructure_plant_.climber()->set_peak_velocity(23.0);
+ superstructure_plant_.climber()->set_peak_acceleration(0.2);
+
+ // Intake needs over 9 seconds to reach the goal
+ // TODO(Milo): Make this a sane time
+ RunFor(chrono::seconds(20));
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+ RunFor(chrono::seconds(2));
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_front().state());
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_back().state());
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.turret().state());
+ EXPECT_EQ(RelativeEncoderSubsystem::State::RUNNING,
+ superstructure_.climber().state());
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+ RunFor(chrono::seconds(2));
+ CheckIfZeroed();
+}
+
+// Makes sure the roller speeds are getting compensated correctly
+TEST_F(SuperstructureTest, RunRollers) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ SendRobotVelocity(3.0);
+ TestRollerFront(-12.0, 1.5);
+ TestRollerFront(12.0, 1.5);
+ TestRollerFront(0.0, 1.5);
+
+ SendRobotVelocity(-3.0);
+ TestRollerFront(-12.0, 1.5);
+ TestRollerFront(12.0, 1.5);
+ TestRollerFront(0.0, 1.5);
+
+ SendRobotVelocity(3.0);
+ TestRollerBack(-12.0, 1.5);
+ TestRollerBack(12.0, 1.5);
+ TestRollerBack(0.0, 1.5);
+
+ SendRobotVelocity(-3.0);
+ TestRollerBack(-12.0, 1.5);
+ TestRollerBack(12.0, 1.5);
+ TestRollerBack(0.0, 1.5);
+
+ TestTransferRoller(-12.0, 1.5);
+ TestTransferRoller(12.0, 1.5);
+ TestTransferRoller(0.0, 1.5);
+}
+
+// Make sure that the front and back intakes are never switched
+TEST_F(SuperstructureTest, RunIntakes) {
+ SetEnabled(true);
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+
+ WaitUntilZeroed();
+
+ superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_front()->set_peak_acceleration(0.3);
+ superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_back()->set_peak_acceleration(0.3);
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ // TODO(Milo): Make this a sane time
+ RunFor(chrono::seconds(10));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_front()->position(),
+ constants::Values::kIntakeRange().lower);
+ EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_back()->position(),
+ constants::Values::kIntakeRange().upper);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_main.cc b/y2022/control_loops/superstructure/superstructure_main.cc
index f53e042..c6c12b7 100644
--- a/y2022/control_loops/superstructure/superstructure_main.cc
+++ b/y2022/control_loops/superstructure/superstructure_main.cc
@@ -1,7 +1,6 @@
-#include "y2022/control_loops/superstructure/superstructure.h"
-
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
int main(int argc, char **argv) {
::aos::InitGoogle(&argc, &argv);
@@ -10,8 +9,11 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
+ std::shared_ptr<const y2022::constants::Values> values =
+ std::make_shared<const y2022::constants::Values>(
+ y2022::constants::MakeValues());
::y2022::control_loops::superstructure::Superstructure superstructure(
- &event_loop);
+ &event_loop, values);
event_loop.Run();
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index 7fa390f..aecc090 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,18 +1,28 @@
namespace y2022.control_loops.superstructure;
table Output {
- // Voltage of the climber falcon
- // - is down + is up
- climber_voltage:double (id: 0);
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_voltage:double (id: 0);
- // Voltage of the catapult falcon
- // Positive lifts the catapult to fire.
- catapult_voltage:double (id: 1);
+ // Voltage of the catapult falcon
+ // Positive lifts the catapult to fire.
+ catapult_voltage:double (id: 1);
- // Voltage of the turret falcon
- // Positive rotates the turret around the Z axis (up) according to the
- // right hand rule.
- turret_voltage:double (id: 2);
+ // Voltage of the turret falcon
+ // Positive rotates the turret around the Z axis (up) according to the
+ // right hand rule.
+ turret_voltage:double (id: 2);
+
+ // Intake joint voltages.
+ intake_voltage_front:double (id: 3);
+ intake_voltage_back:double (id: 4);
+
+ // Intake roller voltages
+ roller_voltage_front:double (id: 5);
+ roller_voltage_back:double (id: 6);
+ // One transfer motor for both sides
+ transfer_roller_voltage:double (id: 7);
}
root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index 1f1a4c5..6581758 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -1,7 +1,7 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import * as proxy from 'org_frc971/aos/network/www/proxy';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
import Connection = proxy.Connection;
@@ -9,11 +9,15 @@
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
-export function plotSuperstructure(conn: Connection, element: Element) : void {
+export function plotSuperstructure(conn: Connection, element: Element): void {
const aosPlotter = new AosPlotter(conn);
- const goal = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Goal');
- const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
- const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
- const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Position');
const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
}
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index a36f4f6..38babaf 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -4,6 +4,11 @@
table Position {
climber:frc971.RelativePosition (id: 0);
+ // Zero for the intake position value is up, and positive is
+ // down.
+ intake_front:frc971.PotAndAbsolutePosition (id: 1);
+ intake_back:frc971.PotAndAbsolutePosition (id: 2);
+ turret:frc971.PotAndAbsolutePosition (id: 3);
}
root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 0ee0994..b4bad2a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -12,6 +12,12 @@
// Subsystem statuses
climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);
+
+ // Estimated angles and angular velocities of the intakes.
+ intake_front:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
+ intake_back:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
+
+ turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
}
root_type Status;
\ No newline at end of file
diff --git a/y2022/control_loops/superstructure/turret/BUILD b/y2022/control_loops/superstructure/turret/BUILD
new file mode 100644
index 0000000..c2948d7
--- /dev/null
+++ b/y2022/control_loops/superstructure/turret/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_turret",
+ outs = [
+ "turret_plant.h",
+ "turret_plant.cc",
+ "integral_turret_plant.h",
+ "integral_turret_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:turret) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:turret",
+ ],
+)
+
+cc_library(
+ name = "turret_plants",
+ srcs = [
+ "integral_turret_plant.cc",
+ "turret_plant.cc",
+ ],
+ hdrs = [
+ "integral_turret_plant.h",
+ "turret_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)