Updated 2023 Autonomous
This commit includes the first draft of the autonomous
actor, which we will be testing and adjusting as needed.
This also includes sending the superstructure goals and
a preloaded cone test in the lib test file.
Signed-off-by: Logan Isaacson <100030671@mvla.net>
Change-Id: I01ec7859989a4dfd41251b44d3c1f98a8e8461b6
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 4d5d43e..8ab2f27 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -20,12 +20,26 @@
void EndEffector::RunIteration(
const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
double falcon_current, double cone_position, bool beambreak,
- double *roller_voltage) {
+ double *roller_voltage, bool preloaded_with_cone) {
*roller_voltage = 0.0;
constexpr double kMinCurrent = 40.0;
constexpr double kMaxConePosition = 0.92;
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (preloaded_with_cone) {
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ case EndEffectorState::INTAKING:
+ state_ = EndEffectorState::LOADED;
+ break;
+ case EndEffectorState::LOADED:
+ case EndEffectorState::SPITTING:
+ break;
+ }
+ }
+
// Let them switch game pieces
if (roller_goal == RollerGoal::INTAKE_CONE_UP) {
game_piece_ = vision::Class::CONE_UP;
@@ -90,8 +104,8 @@
break;
case EndEffectorState::LOADED:
timer_ = timestamp;
- // If loaded and beam break not triggered, intake
- if (!beambreak_status) {
+ // If loaded and beam break not triggered and not preloaded, intake
+ if (!beambreak_status && !preloaded_with_cone) {
state_ = EndEffectorState::INTAKING;
}
break;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 5ae96da..fe1896b 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -25,7 +25,7 @@
void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
RollerGoal roller_goal, double falcon_current,
double cone_position, bool beambreak,
- double *intake_roller_voltage);
+ double *intake_roller_voltage, bool preloaded_with_cone);
EndEffectorState state() const { return state_; }
vision::Class game_piece() const { return game_piece_; }
void Reset();
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 0f19a1e..ece0790 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -88,7 +88,8 @@
? position->roller_falcon()->torque_current()
: 0.0,
position->cone_position(), position->end_effector_cube_beam_break(),
- &output_struct.roller_voltage);
+ &output_struct.roller_voltage,
+ unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cone() : false);
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 670351a..7ac7000 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -22,6 +22,9 @@
wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
roller_goal:RollerGoal (id: 3);
+
+ // If true, we started with the cone loaded and should proceed to that state.
+ preloaded_with_cone:bool (id: 4);
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index c1c20a3..80e41f9 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -446,6 +446,26 @@
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
};
+// Test that we are able to signal that the ball was preloaded
+TEST_F(SuperstructureTest, Preloaded) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_preloaded_with_cone(true);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::LOADED);
+}
+
// Tests that the superstructure does nothing when the goal is to remain
// still.
TEST_F(SuperstructureTest, DoesNothing) {