Add Intake Superstructure
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: Iaaffa7e398bb10681a8aa2f3ae38d1e89503f159
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index bb89021..366a8db 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -24,11 +24,15 @@
name),
values_(values),
constants_fetcher_(event_loop),
+ robot_constants_(CHECK_NOTNULL(&constants_fetcher_.constants())),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ intake_pivot_(
+ robot_constants_->common()->intake_pivot(),
+ robot_constants_->robot()->intake_constants()->intake_pivot_zero()) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -40,27 +44,80 @@
event_loop()->context().monotonic_event_time;
(void)timestamp;
- (void)unsafe_goal;
(void)position;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ intake_pivot_.Reset();
}
OutputT output_struct;
+
+ double intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->retracted();
+
+ if (unsafe_goal != nullptr &&
+ unsafe_goal->intake_pivot_goal() == IntakePivotGoal::EXTENDED) {
+ intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->extended();
+ }
+
+ IntakeRollerState intake_roller_state = IntakeRollerState::NONE;
+
+ switch (unsafe_goal != nullptr ? unsafe_goal->intake_roller_goal()
+ : IntakeRollerGoal::NONE) {
+ case IntakeRollerGoal::NONE:
+ output_struct.intake_roller_voltage = 0.0;
+ break;
+ case IntakeRollerGoal::SPIT:
+ intake_roller_state = IntakeRollerState::SPITTING;
+ output_struct.intake_roller_voltage =
+ robot_constants_->common()->intake_roller_voltages()->spitting();
+ break;
+ case IntakeRollerGoal::INTAKE:
+ intake_roller_state = IntakeRollerState::INTAKING;
+ output_struct.intake_roller_voltage =
+ robot_constants_->common()->intake_roller_voltages()->intaking();
+ break;
+ }
+
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
}
+
drivetrain_status_fetcher_.Fetch();
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ intake_pivot_goal_buffer;
+
+ intake_pivot_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *intake_pivot_goal_buffer.fbb(), intake_pivot_position));
+
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *intake_pivot_goal = &intake_pivot_goal_buffer.message();
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_pivot_status_offset = intake_pivot_.Iterate(
+ intake_pivot_goal, position->intake_pivot(),
+ output != nullptr ? &output_struct.intake_pivot_voltage : nullptr,
+ status->fbb());
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
- status_builder.add_estopped(false);
+
+ const bool zeroed = intake_pivot_.zeroed();
+ const bool estopped = intake_pivot_.estopped();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+ status_builder.add_intake_roller_state(intake_roller_state);
+ status_builder.add_intake_pivot_state(intake_pivot_status_offset);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index b3ab37e..277ba73 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -21,10 +21,19 @@
class Superstructure
: public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ 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_pivot() const {
+ return intake_pivot_;
+ }
+
double robot_velocity() const;
protected:
@@ -35,13 +44,15 @@
private:
std::shared_ptr<const constants::Values> values_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
-
+ const Constants *robot_constants_;
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ PotAndAbsoluteEncoderSubsystem intake_pivot_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index bcda3ae..c253f62 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
#include "frc971/zeroing/absolute_encoder.h"
#include "y2024/constants/simulated_constants_sender.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
#include "y2024/control_loops/superstructure/superstructure.h"
DEFINE_string(output_folder, "",
@@ -29,10 +30,19 @@
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+using PotAndAbsoluteEncoderSimulator =
+ frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
class SuperstructureSimulation {
public:
- SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ SuperstructureSimulation(::aos::EventLoop *event_loop,
+ const Constants *simulated_robot_constants,
+ chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
@@ -40,14 +50,42 @@
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<Output>("/superstructure")) {
- (void)dt_;
+ event_loop_->MakeFetcher<Output>("/superstructure")),
+ intake_pivot_(
+ new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->intake_constants()
+ ->intake_pivot_zero()
+ ->one_revolution_distance()),
+ {.subsystem_params =
+ {simulated_robot_constants->common()->intake_pivot(),
+ simulated_robot_constants->robot()
+ ->intake_constants()
+ ->intake_pivot_zero()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->intake_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->intake_pivot()->range()),
+ simulated_robot_constants->robot()
+ ->intake_constants()
+ ->intake_pivot_zero()
+ ->measured_absolute_position(),
+ dt_) {
+ intake_pivot_.InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->intake_pivot()->range())
+ .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_pivot_.Simulate(
+ superstructure_output_fetcher_->intake_pivot_voltage(),
+ superstructure_status_fetcher_->intake_pivot_state());
}
first_ = false;
SendPositionMessage();
@@ -60,11 +98,21 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePosition::Builder intake_pivot_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_pivot_offset =
+ intake_pivot_.encoder()->GetSensorValues(&intake_pivot_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake_pivot(intake_pivot_offset);
+
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
+ PotAndAbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -74,6 +122,7 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
+ PotAndAbsoluteEncoderSimulator intake_pivot_;
bool first_ = true;
};
@@ -91,6 +140,9 @@
superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
superstructure_(superstructure_event_loop.get(), (values_)),
test_event_loop_(MakeEventLoop("test", roborio_)),
+ constants_fetcher_(test_event_loop_.get()),
+ simulated_robot_constants_(
+ CHECK_NOTNULL(&constants_fetcher_.constants())),
superstructure_goal_fetcher_(
test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
@@ -106,8 +158,8 @@
drivetrain_status_sender_(
test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
- superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
- (void)values_;
+ superstructure_plant_(superstructure_plant_event_loop_.get(),
+ simulated_robot_constants_, dt()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
SetEnabled(true);
@@ -123,10 +175,34 @@
void VerifyNearGoal() {
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
+ superstructure_output_fetcher_.Fetch();
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr)
+ << ": No output";
+
+ double set_point = simulated_robot_constants_->common()
+ ->intake_pivot_set_points()
+ ->retracted();
+
+ if (superstructure_goal_fetcher_->intake_pivot_goal() ==
+ IntakePivotGoal::EXTENDED) {
+ set_point = simulated_robot_constants_->common()
+ ->intake_pivot_set_points()
+ ->extended();
+ }
+
+ EXPECT_NEAR(
+ set_point,
+ superstructure_status_fetcher_->intake_pivot_state()->position(),
+ 0.001);
+
+ if (superstructure_status_fetcher_->intake_roller_state() ==
+ IntakeRollerState::NONE) {
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
+ }
}
void CheckIfZeroed() {
@@ -179,6 +255,9 @@
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const Constants *simulated_robot_constants_;
+
::aos::Fetcher<Goal> superstructure_goal_fetcher_;
::aos::Sender<Goal> superstructure_goal_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
@@ -208,23 +287,33 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+
RunFor(chrono::seconds(10));
- VerifyNearGoal();
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+
+ VerifyNearGoal();
}
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
+ superstructure_plant_.intake_pivot()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->intake_pivot()->range())
+ .middle());
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -247,6 +336,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(20));
@@ -258,11 +349,13 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- // TODO(Milo): Make this a sane time
- RunFor(chrono::seconds(20));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
@@ -271,6 +364,9 @@
SetEnabled(true);
WaitUntilZeroed();
RunFor(chrono::seconds(2));
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_pivot().state());
}
// Tests that running disabled works
@@ -278,4 +374,64 @@
RunFor(chrono::seconds(2));
CheckIfZeroed();
}
-} // namespace y2024::control_loops::superstructure::testing
+
+// Tests Intake in multiple scenarios
+TEST_F(SuperstructureTest, IntakeGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ superstructure_plant_.intake_pivot()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->intake_pivot()->range())
+ .middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::SPIT);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+}
+} // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file