y2017/control_loops/superstructure/intake/intake.cc

#include "y2017/control_loops/superstructure/intake/intake.h"

#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/intake/intake_integral_plant.h"

namespace y2017::control_loops::superstructure::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) {
  clear_min_position();
}

void Intake::Reset() {
  state_ = State::UNINITIALIZED;
  clear_min_position();
  profiled_subsystem_.Reset();
  disable_count_ = 0;
}

flatbuffers::Offset<
    frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Intake::Iterate(const IntakeGoal *unsafe_goal,
                const ::frc971::PotAndAbsolutePosition *position,
                double *output, flatbuffers::FlatBufferBuilder *fbb) {
  bool disable = output == nullptr;
  profiled_subsystem_.Correct(*position);

  switch (state_) {
    case State::UNINITIALIZED:
      // Wait in the uninitialized state until the intake is initialized.
      AOS_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());
        if (unsafe_goal->disable_intake()) {
          if (disable_count_ > 100) {
            disable = true;
          }
          ++disable_count_;
          if (disable_count_ > 200) {
            state_ = State::ESTOP;
          }
        } else {
          disable_count_ = 0;
        }
      }

      // Force the intake to be at least at min_position_ out.
      if (profiled_subsystem_.unprofiled_goal(0, 0) < min_position_) {
        AOS_LOG(DEBUG, "Limiting intake to %f from %f\n", min_position_,
                profiled_subsystem_.unprofiled_goal(0, 0));
        profiled_subsystem_.set_unprofiled_goal(min_position_);
      }

      // ESTOP if we hit the hard limits.
      if (profiled_subsystem_.CheckHardLimits() ||
          profiled_subsystem_.error()) {
        state_ = State::ESTOP;
      }
    } break;

    case State::ESTOP:
      AOS_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();
  }

  frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::Builder
      status_builder = profiled_subsystem_.BuildStatus<
          frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::
              Builder>(fbb);

  // Save debug/internal state.
  status_builder.add_estopped((state_ == State::ESTOP));
  status_builder.add_state(static_cast<int32_t>(state_));

  return status_builder.Finish();
}

}  // namespace y2017::control_loops::superstructure::intake