frc971/control_loops/claw/claw.cc

#include "frc971/control_loops/claw/claw.h"

#include <stdio.h>

#include <algorithm>

#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"

#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"

// Zeroing plan.
// There are 2 types of zeros.  Enabled and disabled ones.
// Disabled ones are only valid during auto mode, and can be used to speed up
// the enabled zero process.  We need to re-zero during teleop in case the auto
// zero was poor and causes us to miss all our shots.
//
// We need to be able to zero manually while disabled by moving the joint over
// the zeros.
// Zero on the down edge when disabled (gravity in the direction of motion)
//
// When enabled, zero on the up edge (gravity opposing the direction of motion)
// The enabled sequence needs to work as follows.  We can crash the claw if we
// bring them too close to each other or too far from each other.  The only safe
// thing to do is to move them in unison.
//
// Start by moving them both towards the front of the bot to either find either
// the middle hall effect on either jaw, or the front hall effect on the bottom
// jaw.  Any edge that isn't the desired edge will provide an approximate edge
// location that can be used for the fine tuning step.
// Once an edge is found on the front claw, move back the other way with both
// claws until an edge is found for the other claw.
// Now that we have an approximate zero, we can robustify the limits to keep
// both claws safe.  Then, we can move both claws to a position that is the
// correct side of the zero and go zero.

// Valid region plan.
// Difference between the arms has a range, and the values of each arm has a range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
// out of the condition.

namespace frc971 {
namespace control_loops {

static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;

ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
    : StateFeedbackLoop<4, 2, 2>(loop),
      uncapped_average_voltage_(0.0),
      is_zeroing_(true) {}

void ClawLimitedLoop::CapU() {
  uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
  if (is_zeroing_) {
    LOG(DEBUG, "zeroing\n");
    const frc971::constants::Values &values = constants::GetValues();
    if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
      const double difference =
          uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
      U(0, 0) -= difference;
    } else if (uncapped_average_voltage_ < -values.claw.max_zeroing_voltage) {
      const double difference =
          -uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
      U(0, 0) += difference;
    }
  }

  double max_value =
      ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));

  const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
  if (max_value > k_max_voltage) {
    LOG(DEBUG, "Capping U because max is %f\n", max_value);
    U = U * k_max_voltage / max_value;
    LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
  }
}

ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
                                                 ClawMotor *motor)
    : offset_(0.0),
      name_(name),
      motor_(motor),
      zeroing_state_(UNKNOWN_POSITION),
      posedge_value_(0.0),
      negedge_value_(0.0),
      encoder_(0.0),
      last_encoder_(0.0) {}

void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition &claw) {
  front_.Update(claw.front);
  calibration_.Update(claw.calibration);
  back_.Update(claw.back);

  bool any_sensor_triggered = any_triggered();
  if (any_sensor_triggered && any_triggered_last_) {
    // We are still on the hall effect and nothing has changed.
    min_hall_effect_on_angle_ =
        ::std::min(min_hall_effect_on_angle_, claw.position);
    max_hall_effect_on_angle_ =
        ::std::max(max_hall_effect_on_angle_, claw.position);
  } else if (!any_sensor_triggered && !any_triggered_last_) {
    // We are still off the hall effect and nothing has changed.
    min_hall_effect_off_angle_ =
        ::std::min(min_hall_effect_off_angle_, claw.position);
    max_hall_effect_off_angle_ =
        ::std::max(max_hall_effect_off_angle_, claw.position);
  } else if (any_sensor_triggered && !any_triggered_last_) {
    // Saw a posedge on the hall effect.  Reset the limits.
    min_hall_effect_on_angle_ = ::std::min(claw.posedge_value, claw.position);
    max_hall_effect_on_angle_ = ::std::max(claw.posedge_value, claw.position);
  } else if (!any_sensor_triggered && any_triggered_last_) {
    // Saw a negedge on the hall effect.  Reset the limits.
    min_hall_effect_off_angle_ = ::std::min(claw.negedge_value, claw.position);
    max_hall_effect_off_angle_ = ::std::max(claw.negedge_value, claw.position);
  }

  posedge_value_ = claw.posedge_value;
  negedge_value_ = claw.negedge_value;
  last_encoder_ = encoder_;
  if (front().value() || calibration().value() || back().value()) {
    last_on_encoder_ = encoder_;
  } else {
    last_off_encoder_ = encoder_;
  }
  encoder_ = claw.position;
  any_triggered_last_ = any_sensor_triggered;
}

void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
  set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);

  front_.Reset(claw.front);
  calibration_.Reset(claw.calibration);
  back_.Reset(claw.back);
  // close up the min and max edge positions as they are no longer valid and
  // will be expanded in future iterations
  min_hall_effect_on_angle_ = claw.position;
  max_hall_effect_on_angle_ = claw.position;
  min_hall_effect_off_angle_ = claw.position;
  max_hall_effect_off_angle_ = claw.position;
  any_triggered_last_ = any_triggered();
}

bool TopZeroedStateFeedbackLoop::SetCalibrationOnEdge(
    const constants::Values::Claws::Claw &claw_values,
    JointZeroingState zeroing_state) {
  double edge_encoder;
  double edge_angle;
  if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
    LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
    SetCalibration(edge_encoder, edge_angle);
    set_zeroing_state(zeroing_state);
    return true;
  }
  return false;
}

bool BottomZeroedStateFeedbackLoop::SetCalibrationOnEdge(
    const constants::Values::Claws::Claw &claw_values,
    JointZeroingState zeroing_state) {
  double edge_encoder;
  double edge_angle;
  if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
    LOG(INFO, "Calibration edge.\n");
    SetCalibration(edge_encoder, edge_angle);
    set_zeroing_state(zeroing_state);
    return true;
  }
  return false;
}

ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
    : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
      has_top_claw_goal_(false),
      top_claw_goal_(0.0),
      top_claw_(this),
      has_bottom_claw_goal_(false),
      bottom_claw_goal_(0.0),
      bottom_claw_(this),
      claw_(MakeClawLoop()),
      was_enabled_(false),
      doing_calibration_fine_tune_(false),
      capped_goal_(false),
      mode_(UNKNOWN_LOCATION) {}

const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;

bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
    const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
    const HallEffectTracker &sensorB) {
  if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
      !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
      this_sensor.value() && !this_sensor.last_value()) {
   posedge_filter_ = &this_sensor;
  } else if (posedge_filter_ == &this_sensor &&
             !this_sensor.posedge_count_changed() &&
             !sensorA.posedge_count_changed() &&
             !sensorB.posedge_count_changed() && this_sensor.value()) {
    posedge_filter_ = nullptr;
	return true;
  } else if (posedge_filter_ == &this_sensor) {
    posedge_filter_ = nullptr;
  }
  return false;
}

bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
    const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
    const HallEffectTracker &sensorB) {
  if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
      !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
      !this_sensor.value() && this_sensor.last_value()) {
    negedge_filter_ = &this_sensor;
  } else if (negedge_filter_ == &this_sensor &&
             !this_sensor.negedge_count_changed() &&
             !sensorA.negedge_count_changed() &&
             !sensorB.negedge_count_changed() && !this_sensor.value()) {
    negedge_filter_ = nullptr;
	return true;
  } else if (negedge_filter_ == &this_sensor) {
    negedge_filter_ = nullptr;
  }
  return false;
}

bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
    const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
    double *edge_angle, const HallEffectTracker &this_sensor,
    const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
    const char *hall_effect_name) {
  bool found_edge = false;

  if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
    if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
	  LOG(INFO, "%s: Uncertain which side, rejecting posedge\n", name_);
    } else {
      const double average_last_encoder =
          (min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
      if (posedge_value_ < average_last_encoder) {
        *edge_angle = angles.upper_decreasing_angle;
        LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
            name_, hall_effect_name, *edge_angle, posedge_value_,
            average_last_encoder);
      } else {
        *edge_angle = angles.lower_angle;
        LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
            name_, hall_effect_name, *edge_angle, posedge_value_,
            average_last_encoder);
      }
      *edge_encoder = posedge_value_;
      found_edge = true;
    }
  }

  if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
    if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
	  LOG(INFO, "%s: Uncertain which side, rejecting negedge\n", name_);
    } else {
      const double average_last_encoder =
          (min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
      if (negedge_value_ > average_last_encoder) {
        *edge_angle = angles.upper_angle;
        LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
            name_, hall_effect_name, *edge_angle, negedge_value_,
            average_last_encoder);
      } else {
        *edge_angle = angles.lower_decreasing_angle;
        LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
            name_, hall_effect_name, *edge_angle, negedge_value_,
            average_last_encoder);
      }
      *edge_encoder = negedge_value_;
      found_edge = true;
    }
  }

  return found_edge;
}

bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
    const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
    double *edge_angle) {
  if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
                          front_, calibration_, back_, "front")) {
    return true;
  }
  if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
                          calibration_, front_, back_, "calibration")) {
    return true;
  }
  if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
                          back_, calibration_, front_, "back")) {
    return true;
  }
  return false;
}

void TopZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
                                                double edge_angle) {
  double old_offset = offset_;
  offset_ = edge_angle - edge_encoder;
  const double doffset = offset_ - old_offset;
  motor_->ChangeTopOffset(doffset);
}

void BottomZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
                                                   double edge_angle) {
  double old_offset = offset_;
  offset_ = edge_angle - edge_encoder;
  const double doffset = offset_ - old_offset;
  motor_->ChangeBottomOffset(doffset);
}

void ClawMotor::ChangeTopOffset(double doffset) {
  claw_.ChangeTopOffset(doffset);
  if (has_top_claw_goal_) {
    top_claw_goal_ += doffset;
  }
}

void ClawMotor::ChangeBottomOffset(double doffset) {
  claw_.ChangeBottomOffset(doffset);
  if (has_bottom_claw_goal_) {
    bottom_claw_goal_ += doffset;
  }
}

void ClawLimitedLoop::ChangeTopOffset(double doffset) {
  Y_(1, 0) += doffset;
  X_hat(1, 0) += doffset;
  LOG(INFO, "Changing top offset by %f\n", doffset);
}
void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
  Y_(0, 0) += doffset;
  X_hat(0, 0) += doffset;
  X_hat(1, 0) -= doffset;
  LOG(INFO, "Changing bottom offset by %f\n", doffset);
}

void LimitClawGoal(double *bottom_goal, double *top_goal,
                   const frc971::constants::Values &values) {
  // first update position based on angle limit

  const double separation = *top_goal - *bottom_goal;
  if (separation > values.claw.claw_max_separation) {
    const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
    *bottom_goal += dsep;
    *top_goal -= dsep;
    LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
  }
  if (separation < values.claw.claw_min_separation) {
    const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
    *bottom_goal += dsep;
    *top_goal -= dsep;
    LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
  }

  // now move both goals in unison
  if (*bottom_goal < values.claw.lower_claw.lower_limit) {
    *top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
    *bottom_goal = values.claw.lower_claw.lower_limit;
  }
  if (*bottom_goal > values.claw.lower_claw.upper_limit) {
    *top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
    *bottom_goal = values.claw.lower_claw.upper_limit;
  }

  if (*top_goal < values.claw.upper_claw.lower_limit) {
    *bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
    *top_goal = values.claw.upper_claw.lower_limit;
  }
  if (*top_goal > values.claw.upper_claw.upper_limit) {
    *bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
    *top_goal = values.claw.upper_claw.upper_limit;
  }
}

bool ClawMotor::is_ready() const {
  return (
      (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
       bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
      (::aos::robot_state->autonomous &&
       ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         top_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
        (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         bottom_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))));
}

bool ClawMotor::is_zeroing() const { return !is_ready(); }

// Positive angle is up, and positive power is up.
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
                             const control_loops::ClawGroup::Position *position,
                             control_loops::ClawGroup::Output *output,
                             control_loops::ClawGroup::Status *status) {
  constexpr double dt = 0.01;

  // Disable the motors now so that all early returns will return with the
  // motors disabled.
  if (output) {
    output->top_claw_voltage = 0;
    output->bottom_claw_voltage = 0;
    output->intake_voltage = 0;
  }

  if (reset()) {
    top_claw_.Reset(position->top);
    bottom_claw_.Reset(position->bottom);
  }

  if (::aos::robot_state.get() == nullptr) {
    return;
  }

  const frc971::constants::Values &values = constants::GetValues();

  if (position) {
    Eigen::Matrix<double, 2, 1> Y;
    Y << position->bottom.position + bottom_claw_.offset(),
        position->top.position + top_claw_.offset();
    claw_.Correct(Y);

    top_claw_.SetPositionValues(position->top);
    bottom_claw_.SetPositionValues(position->bottom);

    if (!has_top_claw_goal_) {
      has_top_claw_goal_ = true;
      top_claw_goal_ = top_claw_.absolute_position();
      initial_separation_ =
          top_claw_.absolute_position() - bottom_claw_.absolute_position();
    }
    if (!has_bottom_claw_goal_) {
      has_bottom_claw_goal_ = true;
      bottom_claw_goal_ = bottom_claw_.absolute_position();
      initial_separation_ =
          top_claw_.absolute_position() - bottom_claw_.absolute_position();
    }
    LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
        top_claw_.absolute_position(), bottom_claw_.absolute_position());
  }

  const bool autonomous = ::aos::robot_state->autonomous;
  const bool enabled = ::aos::robot_state->enabled;

  double bottom_claw_velocity_ = 0.0;
  double top_claw_velocity_ = 0.0;

  if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
       bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
      (autonomous &&
       ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         top_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
        (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         bottom_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
    // Ready to use the claw.
    // Limit the goals here.
    bottom_claw_goal_ = goal->bottom_angle;
    top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
    has_bottom_claw_goal_ = true;
    has_top_claw_goal_ = true;
    doing_calibration_fine_tune_ = false;

    mode_ = READY;
  } else if (top_claw_.zeroing_state() !=
                 ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
             bottom_claw_.zeroing_state() !=
                 ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
    // Time to fine tune the zero.
    // Limit the goals here.
    if (!enabled) {
      // If we are disabled, start the fine tune process over again.
      doing_calibration_fine_tune_ = false;
    }
    if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
      // always get the bottom claw to calibrated first
      LOG(DEBUG, "Calibrating the bottom of the claw\n");
      if (!doing_calibration_fine_tune_) {
        if (::std::abs(bottom_absolute_position() -
                       values.claw.start_fine_tune_pos) <
            values.claw.claw_unimportant_epsilon) {
          doing_calibration_fine_tune_ = true;
          bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
          top_claw_velocity_ = bottom_claw_velocity_ =
              values.claw.claw_zeroing_speed;
          LOG(DEBUG, "Ready to fine tune the bottom\n");
          mode_ = FINE_TUNE_BOTTOM;
        } else {
          // send bottom to zeroing start
          bottom_claw_goal_ = values.claw.start_fine_tune_pos;
          LOG(DEBUG, "Going to the start position for the bottom\n");
          mode_ = PREP_FINE_TUNE_BOTTOM;
        }
      } else {
        mode_ = FINE_TUNE_BOTTOM;
        bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
        top_claw_velocity_ = bottom_claw_velocity_ =
            values.claw.claw_zeroing_speed;
        if (top_claw_.front_or_back_triggered() ||
            bottom_claw_.front_or_back_triggered()) {
          // We shouldn't hit a limit, but if we do, go back to the zeroing
          // point and try again.
          doing_calibration_fine_tune_ = false;
          bottom_claw_goal_ = values.claw.start_fine_tune_pos;
          top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
          LOG(DEBUG, "Found a limit, starting over.\n");
          mode_ = PREP_FINE_TUNE_BOTTOM;
        }

        if (position &&
            bottom_claw_.SawFilteredPosedge(
                bottom_claw_.calibration(),
                bottom_claw_.front(), bottom_claw_.back())) {
          // do calibration
          bottom_claw_.SetCalibration(
              position->bottom.posedge_value,
              values.claw.lower_claw.calibration.lower_angle);
          bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
          // calibrated so we are done fine tuning bottom
          doing_calibration_fine_tune_ = false;
          LOG(DEBUG, "Calibrated the bottom correctly!\n");
        } else if (bottom_claw_.calibration().last_value()) {
          doing_calibration_fine_tune_ = false;
          bottom_claw_goal_ = values.claw.start_fine_tune_pos;
          top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
          mode_ = PREP_FINE_TUNE_BOTTOM;
        } else {
          LOG(DEBUG, "Fine tuning\n");
        }
      }
      // now set the top claw to track

      top_claw_goal_ = bottom_claw_goal_ + values.claw.claw_zeroing_separation;
    } else {
      // bottom claw must be calibrated, start on the top
      if (!doing_calibration_fine_tune_) {
        if (::std::abs(top_absolute_position() -
                       values.claw.start_fine_tune_pos) <
            values.claw.claw_unimportant_epsilon) {
          doing_calibration_fine_tune_ = true;
          top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
          top_claw_velocity_ = bottom_claw_velocity_ =
              values.claw.claw_zeroing_speed;
          LOG(DEBUG, "Ready to fine tune the top\n");
          mode_ = FINE_TUNE_TOP;
        } else {
          // send top to zeroing start
          top_claw_goal_ = values.claw.start_fine_tune_pos;
          LOG(DEBUG, "Going to the start position for the top\n");
          mode_ = PREP_FINE_TUNE_TOP;
        }
      } else {
        mode_ = FINE_TUNE_TOP;
        top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
        top_claw_velocity_ = bottom_claw_velocity_ =
            values.claw.claw_zeroing_speed;
        if (top_claw_.front_or_back_triggered() ||
            bottom_claw_.front_or_back_triggered()) {
          // this should not happen, but now we know it won't
          doing_calibration_fine_tune_ = false;
          top_claw_goal_ = values.claw.start_fine_tune_pos;
          top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
          LOG(DEBUG, "Found a limit, starting over.\n");
          mode_ = PREP_FINE_TUNE_TOP;
        }

        if (position &&
            top_claw_.SawFilteredPosedge(top_claw_.calibration(),
                                         top_claw_.front(), top_claw_.back())) {
          // do calibration
          top_claw_.SetCalibration(
              position->top.posedge_value,
              values.claw.upper_claw.calibration.lower_angle);
          top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
          // calibrated so we are done fine tuning top
          doing_calibration_fine_tune_ = false;
          LOG(DEBUG, "Calibrated the top correctly!\n");
        } else if (top_claw_.calibration().last_value()) {
          doing_calibration_fine_tune_ = false;
          top_claw_goal_ = values.claw.start_fine_tune_pos;
          top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
          mode_ = PREP_FINE_TUNE_TOP;
		}
      }
      // now set the bottom claw to track
      bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
    }
  } else {
    doing_calibration_fine_tune_ = false;
    if (!was_enabled_ && enabled) {
      if (position) {
        top_claw_goal_ = position->top.position;
        bottom_claw_goal_ = position->bottom.position;
        initial_separation_ =
            position->top.position - position->bottom.position;
      } else {
        has_top_claw_goal_ = false;
        has_bottom_claw_goal_ = false;
      }
    }

    if ((bottom_claw_.zeroing_state() !=
             ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
         bottom_claw_.front().value() || top_claw_.front().value()) &&
        !top_claw_.back().value() && !bottom_claw_.back().value()) {
      if (enabled) {
        // Time to slowly move back up to find any position to narrow down the
        // zero.
        top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
        bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
        top_claw_velocity_ = bottom_claw_velocity_ =
            values.claw.claw_zeroing_off_speed;
        LOG(DEBUG, "Bottom is known.\n");
      }
    } else {
      // We don't know where either claw is.  Slowly start moving down to find
      // any hall effect.
      if (enabled) {
        top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
        bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
        top_claw_velocity_ = bottom_claw_velocity_ =
            -values.claw.claw_zeroing_off_speed;
        LOG(DEBUG, "Both are unknown.\n");
      }
    }

    if (position) {
      if (enabled) {
        top_claw_.SetCalibrationOnEdge(
            values.claw.upper_claw,
            ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
        bottom_claw_.SetCalibrationOnEdge(
            values.claw.lower_claw,
            ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
      } else {
        // TODO(austin): Only calibrate on the predetermined edge.
        // We might be able to just ignore this since the backlash is soooo
        // low.
        // :)
        top_claw_.SetCalibrationOnEdge(
            values.claw.upper_claw,
            ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
        bottom_claw_.SetCalibrationOnEdge(
            values.claw.lower_claw,
            ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
      }
    }
    mode_ = UNKNOWN_LOCATION;
  }

  // Limit the goals if both claws have been (mostly) found.
  if (mode_ != UNKNOWN_LOCATION) {
    LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
  }

  if (has_top_claw_goal_ && has_bottom_claw_goal_) {
    claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
        bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
    double separation = -971;
    if (position != nullptr) {
      separation = position->top.position - position->bottom.position;
    }
    LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
        claw_.R(1, 0), separation);

    // Only cap power when one of the halves of the claw is moving slowly and
    // could wind up.
    claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
                         mode_ == FINE_TUNE_BOTTOM);
    claw_.Update(output == nullptr);
  } else {
    claw_.Update(true);
  }

  capped_goal_ = false;
  switch (mode_) {
    case READY:
    case PREP_FINE_TUNE_TOP:
    case PREP_FINE_TUNE_BOTTOM:
      break;
    case FINE_TUNE_BOTTOM:
    case FINE_TUNE_TOP:
    case UNKNOWN_LOCATION: {
      if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
        double dx = (claw_.uncapped_average_voltage() -
                     values.claw.max_zeroing_voltage) /
                    claw_.K(0, 0);
        bottom_claw_goal_ -= dx;
        top_claw_goal_ -= dx;
        capped_goal_ = true;
        LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
        LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
            claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
            (claw_.uncapped_average_voltage() -
             values.claw.max_zeroing_voltage));
      } else if (claw_.uncapped_average_voltage() <
                 -values.claw.max_zeroing_voltage) {
        double dx = (claw_.uncapped_average_voltage() +
                     values.claw.max_zeroing_voltage) /
                    claw_.K(0, 0);
        bottom_claw_goal_ -= dx;
        top_claw_goal_ -= dx;
        capped_goal_ = true;
        LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
      }
    } break;
  }

  if (output) {
    if (goal) {
      //setup the intake
      output->intake_voltage = (goal->intake > 12.0) ? 12 :
	  	  (goal->intake < -12.0) ? -12.0 : goal->intake;
      output->tusk_voltage = goal->centering;
      output->tusk_voltage = (goal->centering > 12.0) ? 12 :
	  	  (goal->centering < -12.0) ? -12.0 : goal->centering;
    }
    output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
    output->bottom_claw_voltage =  claw_.U(0, 0);

    if (output->top_claw_voltage > kMaxVoltage) {
      output->top_claw_voltage = kMaxVoltage;
    } else if (output->top_claw_voltage < -kMaxVoltage) {
      output->top_claw_voltage = -kMaxVoltage;
    }

    if (output->bottom_claw_voltage > kMaxVoltage) {
      output->bottom_claw_voltage = kMaxVoltage;
    } else if (output->bottom_claw_voltage < -kMaxVoltage) {
      output->bottom_claw_voltage = -kMaxVoltage;
    }
  }

  bool bottom_done =
      ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.005;
  bool separation_done =
      ::std::abs((top_absolute_position() - bottom_absolute_position()) -
                 goal->separation_angle) <
      0.005;
  status->done = is_ready() && separation_done && bottom_done;

  status->bottom = bottom_absolute_position();
  status->separation = top_absolute_position() - bottom_absolute_position();
  status->bottom_velocity = claw_.X_hat(2, 0);
  status->separation_velocity = claw_.X_hat(3, 0);

  was_enabled_ = ::aos::robot_state->enabled;
}

}  // namespace control_loops
}  // namespace frc971