Add code to intake a cube
Voltage is flipped for cubes.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I16b07e4c6b21a173c10fa682f39103aff8aa9379
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 3bd9488..f59626c 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -76,7 +76,7 @@
output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
status->fbb());
- EndEffectorState end_effector_state = end_effector_.RunIteration(
+ end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
position->has_roller_falcon()
@@ -94,7 +94,9 @@
status_builder.add_estopped(wrist_.estopped() || arm_.estopped());
status_builder.add_arm(arm_status_offset);
status_builder.add_wrist(wrist_offset);
- status_builder.add_end_effector_state(end_effector_state);
+ status_builder.add_end_effector_state(end_effector_.state());
+ // TODO(milind): integrate this with ML game piece detection somehow
+ status_builder.add_game_piece(end_effector_.game_piece());
(void)status->Send(status_builder.Finish());
}