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/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index b154334..599ddde 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -29,6 +29,8 @@
// Returns the control loop calculated voltage.
double voltage() const;
+ double output_position() const { return loop_->X_hat(0); }
+
// Executes the control loop for a cycle.
void Update(bool disabled, const double *unsafe_goal);
@@ -91,6 +93,10 @@
State state() const { return state_; }
+ bool clear_of_box() const {
+ return controller_.output_position() < -0.2;
+ }
+
private:
IntakeController controller_;