y2016/control_loops/shooter/shooter.cc

#include "y2016/control_loops/shooter/shooter.h"

#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"

#include "y2016/control_loops/shooter/shooter_plant.h"

namespace y2016 {
namespace control_loops {

Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
    : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter),
      loop_(new StateFeedbackLoop<2, 1, 1>(
          ::y2016::control_loops::shooter::MakeShooterLoop())),
      history_position_(0),
      position_goal_(0.0),
      last_position_(0.0),
      last_velocity_goal_(0) {
  memset(history_, 0, sizeof(history_));
}

/*static*/ const double Shooter::dt = 0.005;
/*static*/ const double Shooter::kMaxSpeed =
    10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;

void Shooter::RunIteration(
    const control_loops::ShooterQueue::Goal *goal,
    const control_loops::ShooterQueue::Position *position,
    ::aos::control_loops::Output *output,
    control_loops::ShooterQueue::Status *status) {
  double velocity_goal = std::min(goal->velocity, kMaxSpeed);
  const double current_position =
      (position == NULL ? loop_->X_hat(0, 0) : position->position);
  double output_voltage = 0.0;

  // TODO(phil): Set a queue to trigger a shot. For now, disable the fetch from
  // the queue.
  if (false) {
  // if (index_loop.status.FetchLatest() || index_loop.status.get()) {
    // if (index_loop.status->is_shooting) {
    if (velocity_goal != last_velocity_goal_ && velocity_goal < 130) {
      velocity_goal = last_velocity_goal_;
    }
  } else {
    LOG(WARNING, "assuming index isn't shooting\n");
  }
  last_velocity_goal_ = velocity_goal;

  // Track the current position if the velocity goal is small.
  if (velocity_goal <= 1.0) {
    position_goal_ = current_position;
  }

  // TODO(phil): Does this change make sense?
  // loop_->Y << current_position;
  Eigen::Matrix<double, 1, 1> Y;
  Y << current_position;
  loop_->Correct(Y);

  // Add the position to the history.
  history_[history_position_] = current_position;
  history_position_ = (history_position_ + 1) % kHistoryLength;

  // Prevents integral windup by limiting the position error such that the
  // error can't produce much more than full power.
  const double kVelocityWeightScalar = 0.35;
  const double max_reference =
      (loop_->U_max(0, 0) -
       kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
           loop_->K(0, 1)) /
          loop_->K(0, 0) +
      loop_->X_hat(0, 0);
  const double min_reference =
      (loop_->U_min(0, 0) -
       kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
           loop_->K(0, 1)) /
          loop_->K(0, 0) +
      loop_->X_hat(0, 0);

  position_goal_ =
      ::std::max(::std::min(position_goal_, max_reference), min_reference);
  // TODO(phil): Does this change make sense?
  // loop_->R << position_goal_, velocity_goal;
  loop_->mutable_R(0, 0) = position_goal_;
  loop_->mutable_R(1, 0) = velocity_goal;
  position_goal_ += velocity_goal * dt;

  // TODO(phil): Does this change make sense?
  // loop_->Update(position, output == NULL);
  loop_->Update(output == NULL);

  // Kill power at low velocity goals.
  if (velocity_goal < 1.0) {
    loop_->mutable_U(0, 0) = 0.0;
  } else {
    output_voltage = loop_->U(0, 0);
  }

  LOG(DEBUG,
      "PWM: %f, raw_pos: %f rotations: %f "
      "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
      output_voltage, current_position, current_position / (2 * M_PI),
      (current_position - last_position_) / dt,
      // TODO(phil): Does this change make sense?
      // loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
      loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->R(0, 0), loop_->R(1, 0));

  // Calculates the velocity over the last kHistoryLength * .01 seconds
  // by taking the difference between the current and next history positions.
  int old_history_position =
      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
  average_velocity_ =
      (history_[old_history_position] - history_[history_position_]) / dt /
      (double)(kHistoryLength - 1);

  status->average_velocity = average_velocity_;

  // Determine if the velocity is close enough to the goal to be ready.
  if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
      velocity_goal != 0.0) {
    LOG(DEBUG, "Steady: ");
    status->ready = true;
  } else {
    LOG(DEBUG, "Not ready: ");
    status->ready = false;
  }
  LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);

  last_position_ = current_position;

  if (output) {
    output->voltage = output_voltage;
  }
}

}  // namespace control_loops
}  // namespace y2016