Added turret and intake code and tests.
Tests are from Adam.
Change-Id: I5a89700cfe2e9983771b4523facc302243b5dc50
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
new file mode 100644
index 0000000..c66e493
--- /dev/null
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -0,0 +1,125 @@
+#include "y2017/control_loops/superstructure/intake/intake.h"
+
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/intake/intake_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+constexpr double Intake::kZeroingVoltage;
+constexpr double Intake::kOperatingVoltage;
+
+Intake::Intake()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(MakeIntegralIntakeLoop())),
+ constants::GetValues().intake.zeroing,
+ constants::Values::kIntakeRange, 0.3, 5.0) {}
+
+void Intake::Reset() {
+ state_ = State::UNINITIALIZED;
+ profiled_subsystem_.Reset();
+}
+
+void Intake::Iterate(
+ const control_loops::IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disable = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disable = true;
+ break;
+
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disable) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(0.10, 1);
+
+ // We are not ready to start doing anything yet.
+ disable = true;
+ break;
+
+ case State::ZEROING:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disable) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disable) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ if (unsafe_goal) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance);
+ }
+
+ // ESTOP if we hit the hard limits.
+ if (profiled_subsystem_.CheckHardLimits()) {
+ state_ = State::ESTOP;
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage =
+ (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disable);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ // Save debug/internal state.
+ // TODO(austin): Save more.
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace intake
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017