Added column and tests
The column works! We can also shut the intake down for hanging.
Change-Id: I4369d489d1a07a688f204fd9bb00ef7ad787f5a3
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index c6990bc..941b4ce 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
@@ -16,6 +17,17 @@
return profiled_subsystem_.goal(row, col);
}
+ // Sets the minimum position we'll allow the intake to be at. This forces the
+ // intake to stay out far enough to avoid collisions.
+ void set_min_position(double min_position) { min_position_ = min_position; }
+
+ // Moves min_position_ to a position which won't affect any other goal requests.
+ void clear_min_position() {
+ min_position_ = constants::Values::kIntakeRange.lower_hard;
+ }
+
+ double position() const { return profiled_subsystem_.position(); }
+
// The zeroing and operating voltages.
static constexpr double kZeroingVoltage = 2.5;
static constexpr double kOperatingVoltage = 12.0;
@@ -37,7 +49,11 @@
State state() const { return state_; }
private:
- State state_;
+ State state_ = State::UNINITIALIZED;
+ double min_position_;
+
+ // The number of times in a row we've been asked to disable.
+ int disable_count_ = 0;
::frc971::control_loops::SingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsEncoderZeroingEstimator> profiled_subsystem_;