Added arm pickup FSM
We can now automatically pick up a box when it is in the intake.
Change-Id: Ib93ce376717a79fc3a8747681bb7e907a4b92851
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index d44b6b2..4020094 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -95,6 +95,8 @@
// The current state machine state.
uint32_t state;
+ uint32_t grab_state;
+
// The number of times the LQR solver failed.
uint32_t failed_solutions;
};
@@ -126,6 +128,8 @@
// Used to identiy a position in the planned set of positions on the arm.
uint32_t arm_goal_position;
+ // If true, start the grab box sequence.
+ bool grab_box;
bool open_claw;