Added turret and intake code and tests.
Tests are from Adam.
Change-Id: I5a89700cfe2e9983771b4523facc302243b5dc50
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index c6a2742..4811ab1 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -6,6 +6,8 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
+#include "y2017/control_loops/superstructure/turret/turret.h"
+#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
@@ -19,34 +21,9 @@
control_loops::SuperstructureQueue *my_superstructure =
&control_loops::superstructure_queue);
- enum State {
- // Wait for all the filters to be ready before starting the initialization
- // process.
- UNINITIALIZED = 0,
-
- // We now are ready to decide how to zero. Decide what to do once we are
- // enabled.
- DISABLED_INITIALIZED = 1,
-
- ZEROING = 2,
- // Run with full power.
- RUNNING = 3,
- // Internal error caused the superstructure to abort.
- ESTOP = 4,
- };
-
const hood::Hood &hood() const { return hood_; }
-
- bool IsRunning() const { return state_ == RUNNING; }
-
- // Returns the value to move the joint to such that it will stay below
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance);
- // Returns the value to move the joint to such that it will stay above
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance);
+ const turret::Turret &turret() const { return turret_; }
+ const intake::Intake &intake() const { return intake_; }
protected:
virtual void RunIteration(
@@ -57,8 +34,8 @@
private:
hood::Hood hood_;
-
- State state_ = UNINITIALIZED;
+ turret::Turret turret_;
+ intake::Intake intake_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};