y2018/control_loops/superstructure/superstructure.cc

#include "y2018/control_loops/superstructure/superstructure.h"

#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"

namespace y2018 {
namespace control_loops {
namespace superstructure {

namespace {
// The maximum voltage the intake roller will be allowed to use.
constexpr double kMaxIntakeRollerVoltage = 12.0;
}  // namespace

Superstructure::Superstructure(
    control_loops::SuperstructureQueue *superstructure_queue)
    : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
          superstructure_queue),
      intake_left_(constants::GetValues().left_intake.zeroing),
      intake_right_(constants::GetValues().right_intake.zeroing) {}

void Superstructure::RunIteration(
    const control_loops::SuperstructureQueue::Goal *unsafe_goal,
    const control_loops::SuperstructureQueue::Position *position,
    control_loops::SuperstructureQueue::Output *output,
    control_loops::SuperstructureQueue::Status *status) {
  if (WasReset()) {
    LOG(ERROR, "WPILib reset, restarting\n");
    intake_left_.Reset();
    intake_right_.Reset();
    arm_.Reset();
  }

  const double left_intake_goal = ::std::min(
      arm_.max_intake_override(),
      (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
  intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
                       &(position->left_intake),
                       output != nullptr ? &(output->left_intake) : nullptr,
                       &(status->left_intake));

  const double right_intake_goal = ::std::min(
      arm_.max_intake_override(),
      (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));

  intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
                        &(position->right_intake),
                        output != nullptr ? &(output->right_intake) : nullptr,
                        &(status->right_intake));

  const bool intake_clear_of_box =
      intake_left_.clear_of_box() && intake_right_.clear_of_box();
  arm_.Iterate(
      unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
      unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
      unsafe_goal != nullptr ? unsafe_goal->open_claw : false, &(position->arm),
      position->claw_beambreak_triggered,
      position->box_back_beambreak_triggered, intake_clear_of_box,
      output != nullptr ? &(output->voltage_proximal) : nullptr,
      output != nullptr ? &(output->voltage_distal) : nullptr,
      output != nullptr ? &(output->release_arm_brake) : nullptr,
      output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm),
      unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false);

  if (output) {
    if (unsafe_goal) {
      output->hook_release = unsafe_goal->hook_release;
      output->voltage_winch = unsafe_goal->voltage_winch;
      output->forks_release = unsafe_goal->deploy_fork;
    } else {
      output->voltage_winch = 0.0;
      output->hook_release = false;
      output->forks_release = false;
    }
  }

  status->estopped = status->left_intake.estopped ||
                     status->right_intake.estopped || status->arm.estopped;

  status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
                   status->arm.zeroed;

  if (output && unsafe_goal) {
    double roller_voltage = ::std::max(
        -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
                                             kMaxIntakeRollerVoltage));
    constexpr int kReverseTime = 15;
    if (unsafe_goal->intake.roller_voltage < 0.0) {
      output->left_intake.voltage_rollers = roller_voltage;
      output->right_intake.voltage_rollers = roller_voltage;
      rotation_state_ = RotationState::NOT_ROTATING;
      rotation_count_ = 0;
    } else {
      switch (rotation_state_) {
        case RotationState::NOT_ROTATING:
          if (position->left_intake.beam_break) {
            rotation_state_ = RotationState::ROTATING_RIGHT;
            rotation_count_ = kReverseTime;
            break;
          } else if (position->right_intake.beam_break) {
            rotation_state_ = RotationState::ROTATING_LEFT;
            rotation_count_ = kReverseTime;
            break;
          } else {
            break;
          }
        case RotationState::ROTATING_LEFT:
          if (position->right_intake.beam_break) {
            rotation_count_ = kReverseTime;
          } else {
            --rotation_count_;
          }
          if (rotation_count_ == 0) {
            rotation_state_ = RotationState::NOT_ROTATING;
          }
          break;
        case RotationState::ROTATING_RIGHT:
          if (position->left_intake.beam_break) {
            rotation_count_ = kReverseTime;
          } else {
            --rotation_count_;
          }
          if (rotation_count_ == 0) {
            rotation_state_ = RotationState::NOT_ROTATING;
          }
          break;
      }

      if (position->box_back_beambreak_triggered && roller_voltage > 0.0) {
        roller_voltage = 0;
      }
      switch (rotation_state_) {
        case RotationState::NOT_ROTATING:
          output->left_intake.voltage_rollers = roller_voltage;
          output->right_intake.voltage_rollers = roller_voltage;
          break;
        case RotationState::ROTATING_LEFT:
          output->left_intake.voltage_rollers = roller_voltage;
          output->right_intake.voltage_rollers = -roller_voltage;
          break;
        case RotationState::ROTATING_RIGHT:
          output->left_intake.voltage_rollers = -roller_voltage;
          output->right_intake.voltage_rollers = roller_voltage;
          break;
      }
    }
  }
}

}  // namespace superstructure
}  // namespace control_loops
}  // namespace y2018