Add Transfer Roller Superstructure
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I494a75aab2c206ef0e762e12937d09c32e53e7f7
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 366a8db..9a703e3 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -30,6 +30,7 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ transfer_goal_(TransferRollerGoal::NONE),
intake_pivot_(
robot_constants_->common()->intake_pivot(),
robot_constants_->robot()->intake_constants()->intake_pivot_zero()) {
@@ -70,17 +71,44 @@
output_struct.intake_roller_voltage = 0.0;
break;
case IntakeRollerGoal::SPIT:
+ transfer_goal_ = TransferRollerGoal::TRANSFER_OUT;
intake_roller_state = IntakeRollerState::SPITTING;
output_struct.intake_roller_voltage =
robot_constants_->common()->intake_roller_voltages()->spitting();
break;
case IntakeRollerGoal::INTAKE:
+ transfer_goal_ = TransferRollerGoal::TRANSFER_IN;
intake_roller_state = IntakeRollerState::INTAKING;
output_struct.intake_roller_voltage =
robot_constants_->common()->intake_roller_voltages()->intaking();
break;
}
+ TransferRollerState transfer_roller_state = TransferRollerState::NONE;
+
+ switch (unsafe_goal != nullptr ? transfer_goal_ : TransferRollerGoal::NONE) {
+ case TransferRollerGoal::NONE:
+ output_struct.transfer_roller_voltage = 0.0;
+ break;
+ case TransferRollerGoal::TRANSFER_IN:
+ if (position->transfer_beambreak()) {
+ transfer_goal_ = TransferRollerGoal::NONE;
+ transfer_roller_state = TransferRollerState::NONE;
+ output_struct.transfer_roller_voltage = 0.0;
+ break;
+ }
+ transfer_roller_state = TransferRollerState::TRANSFERING_IN;
+ output_struct.transfer_roller_voltage =
+ robot_constants_->common()->transfer_roller_voltages()->transfer_in();
+ break;
+ case TransferRollerGoal::TRANSFER_OUT:
+ transfer_roller_state = TransferRollerState::TRANSFERING_OUT;
+ output_struct.transfer_roller_voltage = robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_out();
+ break;
+ }
+
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
@@ -118,6 +146,7 @@
status_builder.add_estopped(estopped);
status_builder.add_intake_roller_state(intake_roller_state);
status_builder.add_intake_pivot_state(intake_pivot_status_offset);
+ status_builder.add_transfer_roller_state(transfer_roller_state);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 277ba73..67a8328 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -51,6 +51,7 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ TransferRollerGoal transfer_goal_;
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 c253f62..26e75e4 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -51,6 +51,7 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
+ transfer_beambreak_(false),
intake_pivot_(
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
@@ -105,12 +106,17 @@
Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_transfer_beambreak(transfer_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
+ void set_transfer_beambreak(bool triggered) {
+ transfer_beambreak_ = triggered;
+ }
+
PotAndAbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
private:
@@ -122,6 +128,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
+ bool transfer_beambreak_;
+
PotAndAbsoluteEncoderSimulator intake_pivot_;
bool first_ = true;
};
@@ -375,7 +383,7 @@
CheckIfZeroed();
}
-// Tests Intake in multiple scenarios
+// Tests intake and transfer in multiple scenarios
TEST_F(SuperstructureTest, IntakeGoal) {
SetEnabled(true);
WaitUntilZeroed();
@@ -398,6 +406,8 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -415,10 +425,17 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_out());
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -430,8 +447,39 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_transfer_beambreak(false);
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_in());
+
+ superstructure_plant_.set_transfer_beambreak(true);
+
+ RunFor(chrono::seconds(2));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
}
} // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file