Add end effector to superstructure
Add superstructure state machine for the end
effector, update flatbuffers to match, and send to
status.
Signed-off-by: Nathan Leong <nathanrleong@gmail.com>
Change-Id: I7f0c60f05a147ba6b3aec1e3488855c8e674c670
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 1ad2625..c59804c 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -29,7 +29,8 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- arm_(values_) {}
+ arm_(values_),
+ end_effector_() {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
@@ -41,6 +42,7 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
+ end_effector_.Reset();
}
OutputT output_struct;
@@ -66,6 +68,12 @@
status->fbb());
+ EndEffectorState end_effector_state = end_effector_.RunIteration(
+ timestamp, unsafe_goal != nullptr ? unsafe_goal->intake() : false,
+ unsafe_goal != nullptr ? unsafe_goal->spit() : false,
+ position->end_effector_cone_beam_break(),
+ position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -74,6 +82,7 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
status_builder.add_arm(arm_status_offset);
+ status_builder.add_end_effector_state(end_effector_state);
(void)status->Send(status_builder.Finish());
}