Added hood to main superstructure loop
Change-Id: I5b57b3d08fcdcb23a04db72accba2603da3a3270
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index a3978a4..91a7586 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -12,32 +12,50 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name) {
- event_loop->SetRuntimeRealtimePriority(30);
+ name),
+ hood_(constants::GetValues().hood) {
+ event_loop->SetRuntimeRealtimePriority(30);
}
-void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
- const Position * /*position*/,
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ hood_.Reset();
}
+ OutputT output_struct;
+
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
+ hood_.Iterate(unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.hood_voltage) : nullptr,
+ status->fbb());
+
+ bool zeroed;
+ bool estopped;
+
+ const AbsoluteEncoderProfiledJointStatus *hood_status =
+ GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
+ zeroed = hood_status->zeroed();
+ estopped = hood_status->estopped();
if (output != nullptr) {
- OutputT output_struct;
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);
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_hood(hood_status_offset);
status->Send(status_builder.Finish());
}
+} // namespace superstructure
} // namespace control_loops
} // namespace y2020
-} // namespace y2020