frc971/control_loops/claw/claw_calibration.cc

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

#include "aos/linux_code/init.h"
#include "frc971/constants.h"

namespace frc971 {

typedef constants::Values::Claws Claws;

bool DoGetPositionOfEdge(
    const double start_position,
    const control_loops::HalfClawPosition &last_claw_position,
    const control_loops::HalfClawPosition &claw_position,
    const HallEffectStruct &last_hall_effect,
    const HallEffectStruct &hall_effect,
    Claws::AnglePair *limits) {

  if (hall_effect.posedge_count != last_hall_effect.posedge_count) {
    if (claw_position.posedge_value < last_claw_position.position) {
      limits->upper_angle = claw_position.posedge_value - start_position;
    } else {
      limits->lower_decreasing_angle =
          claw_position.posedge_value - start_position;
    }
    return true;
  }
  if (hall_effect.negedge_count != last_hall_effect.negedge_count) {
    if (claw_position.negedge_value > last_claw_position.position) {
      limits->upper_decreasing_angle =
          claw_position.negedge_value - start_position;
    } else {
      limits->lower_angle = claw_position.negedge_value - start_position;
    }
    return true;
  }

  return false;
}

bool GetPositionOfEdge(
    const double start_position,
    const control_loops::HalfClawPosition &last_claw_position,
    const control_loops::HalfClawPosition &claw_position, Claws::Claw *claw) {

  if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
                          last_claw_position.front, claw_position.front,
                          &claw->front)) {
    return true;
  }
  if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
                          last_claw_position.calibration,
                          claw_position.calibration, &claw->calibration)) {
    return true;
  }
  if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
                          last_claw_position.back, claw_position.back,
                          &claw->back)) {
    return true;
  }

  double position = claw_position.position - start_position;

  if (position > claw->upper_limit) {
    claw->upper_hard_limit = claw->upper_limit = position;
    return true;
  }
  if (position < claw->lower_limit) {
    claw->lower_hard_limit = claw->lower_limit = position;
    return true;
  }
  return false;
}

int Main() {
  while (!control_loops::claw_queue_group.position.FetchNextBlocking());

  const double top_start_position =
      control_loops::claw_queue_group.position->top.position;
  const double bottom_start_position =
      control_loops::claw_queue_group.position->bottom.position;

  Claws limits;

  limits.claw_zeroing_off_speed = 0.5;
  limits.claw_zeroing_speed = 0.1;
  limits.claw_zeroing_separation = 0.1;

  // claw separation that would be considered a collision
  limits.claw_min_separation = 0.0;
  limits.claw_max_separation = 0.0;

  // We should never get closer/farther than these.
  limits.soft_min_separation = 0.0;
  limits.soft_max_separation = 0.0;

  limits.upper_claw.lower_hard_limit = 0.0;
  limits.upper_claw.upper_hard_limit = 0.0;
  limits.upper_claw.lower_limit = 0.0;
  limits.upper_claw.upper_limit = 0.0;
  limits.upper_claw.front.lower_angle = 0.0;
  limits.upper_claw.front.upper_angle = 0.0;
  limits.upper_claw.front.lower_decreasing_angle = 0.0;
  limits.upper_claw.front.upper_decreasing_angle = 0.0;
  limits.upper_claw.calibration.lower_angle = 0.0;
  limits.upper_claw.calibration.upper_angle = 0.0;
  limits.upper_claw.calibration.lower_decreasing_angle = 0.0;
  limits.upper_claw.calibration.upper_decreasing_angle = 0.0;
  limits.upper_claw.back.lower_angle = 0.0;
  limits.upper_claw.back.upper_angle = 0.0;
  limits.upper_claw.back.lower_decreasing_angle = 0.0;
  limits.upper_claw.back.upper_decreasing_angle = 0.0;

  limits.lower_claw.lower_hard_limit = 0.0;
  limits.lower_claw.upper_hard_limit = 0.0;
  limits.lower_claw.lower_limit = 0.0;
  limits.lower_claw.upper_limit = 0.0;
  limits.lower_claw.front.lower_angle = 0.0;
  limits.lower_claw.front.upper_angle = 0.0;
  limits.lower_claw.front.lower_decreasing_angle = 0.0;
  limits.lower_claw.front.upper_decreasing_angle = 0.0;
  limits.lower_claw.calibration.lower_angle = 0.0;
  limits.lower_claw.calibration.upper_angle = 0.0;
  limits.lower_claw.calibration.lower_decreasing_angle = 0.0;
  limits.lower_claw.calibration.upper_decreasing_angle = 0.0;
  limits.lower_claw.back.lower_angle = 0.0;
  limits.lower_claw.back.upper_angle = 0.0;
  limits.lower_claw.back.lower_decreasing_angle = 0.0;
  limits.lower_claw.back.upper_decreasing_angle = 0.0;

  limits.claw_unimportant_epsilon = 0.01;
  limits.start_fine_tune_pos = -0.2;
  limits.max_zeroing_voltage = 4.0;

  control_loops::ClawGroup::Position last_position =
      *control_loops::claw_queue_group.position;

  while (true) {
    if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
      bool print = false;
      if (GetPositionOfEdge(top_start_position, last_position.top,
                            control_loops::claw_queue_group.position->top,
                            &limits.upper_claw)) {
        print = true;
        LOG(DEBUG, "Got an edge on the upper claw\n");
      }
      if (GetPositionOfEdge(bottom_start_position, last_position.bottom,
                            control_loops::claw_queue_group.position->bottom,
                            &limits.lower_claw)) {
        print = true;
        LOG(DEBUG, "Got an edge on the lower claw\n");
      }
      const double top_position =
          control_loops::claw_queue_group.position->top.position -
          top_start_position;
      const double bottom_position =
          control_loops::claw_queue_group.position->bottom.position -
          bottom_start_position;
      const double separation = top_position - bottom_position;
      if (separation > limits.claw_max_separation) {
        limits.soft_max_separation = limits.claw_max_separation = separation;
        print = true;
      }
      if (separation < limits.claw_min_separation) {
        limits.soft_min_separation = limits.claw_min_separation = separation;
        print = true;
      }

      if (print) {
        printf("{%f,\n", limits.claw_zeroing_off_speed);
        printf("%f,\n", limits.claw_zeroing_speed);
        printf("%f,\n", limits.claw_zeroing_separation);
        printf("%f,\n", limits.claw_min_separation);
        printf("%f,\n", limits.claw_max_separation);
        printf("%f,\n", limits.soft_min_separation);
        printf("%f,\n", limits.soft_max_separation);
        printf(
            "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
            "%f}},\n",
            limits.upper_claw.lower_hard_limit,
            limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
            limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
            limits.upper_claw.front.upper_angle,
            limits.upper_claw.front.lower_decreasing_angle,
            limits.upper_claw.front.upper_decreasing_angle,
            limits.upper_claw.calibration.lower_angle,
            limits.upper_claw.calibration.upper_angle,
            limits.upper_claw.calibration.lower_decreasing_angle,
            limits.upper_claw.calibration.upper_decreasing_angle,
            limits.upper_claw.back.lower_angle,
            limits.upper_claw.back.upper_angle,
            limits.upper_claw.back.lower_decreasing_angle,
            limits.upper_claw.back.upper_decreasing_angle);

        printf(
            "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
            "%f}},\n",
            limits.lower_claw.lower_hard_limit,
            limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
            limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
            limits.lower_claw.front.upper_angle,
            limits.lower_claw.front.lower_decreasing_angle,
            limits.lower_claw.front.upper_decreasing_angle,
            limits.lower_claw.calibration.lower_angle,
            limits.lower_claw.calibration.upper_angle,
            limits.lower_claw.calibration.lower_decreasing_angle,
            limits.lower_claw.calibration.upper_decreasing_angle,
            limits.lower_claw.back.lower_angle,
            limits.lower_claw.back.upper_angle,
            limits.lower_claw.back.lower_decreasing_angle,
            limits.lower_claw.back.upper_decreasing_angle);
        printf("%f,  // claw_unimportant_epsilon\n",
               limits.claw_unimportant_epsilon);
        printf("%f,   // start_fine_tune_pos\n", limits.start_fine_tune_pos);
        printf("%f,\n", limits.max_zeroing_voltage);
        printf("}\n");
      }

      last_position = *control_loops::claw_queue_group.position;
    }
  }
  return 0;
}

}  // namespace frc971

int main() {
  ::aos::Init();
  int returnvalue = ::frc971::Main();
  ::aos::Cleanup();
  return returnvalue;
}