y2020/control_loops/superstructure/superstructure.cc

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

#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
#include "aos/network/team_number.h"

namespace y2020::control_loops::superstructure {

using frc971::control_loops::AbsoluteAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;

Superstructure::Superstructure(::aos::EventLoop *event_loop,
                               const ::std::string &name)
    : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                    name),
      hood_(constants::GetValues().hood),
      intake_joint_(constants::GetValues().intake),
      turret_(constants::GetValues().turret.subsystem_params),
      drivetrain_status_fetcher_(
          event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
              "/drivetrain")),
      joystick_state_fetcher_(
          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
      has_turret_(::aos::network::GetTeamNumber() != 9971) {
  event_loop->SetRuntimeRealtimePriority(30);
}

double Superstructure::robot_speed() const {
  return (drivetrain_status_fetcher_.get() != nullptr
              ? drivetrain_status_fetcher_->robot_speed()
              : 0.0);
}

void Superstructure::RunIteration(const Goal *unsafe_goal,
                                  const Position *position,
                                  aos::Sender<Output>::Builder *output,
                                  aos::Sender<Status>::Builder *status) {
  if (WasReset()) {
    AOS_LOG(ERROR, "WPILib reset, restarting\n");
    hood_.Reset();
    intake_joint_.Reset();
    turret_.Reset();
  }

  const aos::monotonic_clock::time_point position_timestamp =
      event_loop()->context().monotonic_event_time;

  if (drivetrain_status_fetcher_.Fetch()) {
    aos::Alliance alliance = aos::Alliance::kInvalid;
    joystick_state_fetcher_.Fetch();
    if (joystick_state_fetcher_.get() != nullptr) {
      alliance = joystick_state_fetcher_->alliance();
    }
    const turret::Aimer::WrapMode mode =
        (unsafe_goal != nullptr && unsafe_goal->shooting())
            ? turret::Aimer::WrapMode::kAvoidWrapping
            : turret::Aimer::WrapMode::kAvoidEdges;
    aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
                  turret::Aimer::ShotMode::kShootOnTheFly);
  }

  const float velocity = robot_speed();

  const flatbuffers::Offset<AimerStatus> aimer_status_offset =
      aimer_.PopulateStatus(status->fbb());

  const double distance_to_goal = aimer_.DistanceToGoal();

  aos::FlatbufferFixedAllocatorArray<
      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
      hood_goal;
  aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;

  constants::Values::ShotParams shot_params;
  if (constants::GetValues().shot_interpolation_table.GetInRange(
          distance_to_goal, &shot_params)) {
    hood_goal.Finish(frc971::control_loops::
                         CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
                             *hood_goal.fbb(), shot_params.hood_angle));

    shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
                                          shot_params.velocity_accelerator,
                                          shot_params.velocity_finisher));
  } else {
    hood_goal.Finish(
        frc971::control_loops::
            CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
                *hood_goal.fbb(), constants::GetValues().hood.range.upper));

    shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(), 0.0, 0.0));
  }

  OutputT output_struct;

  flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
      hood_status_offset = hood_.Iterate(
          unsafe_goal != nullptr
              ? (unsafe_goal->hood_tracking() ? &hood_goal.message()
                                              : unsafe_goal->hood())
              : nullptr,
          position->hood(),
          output != nullptr ? &(output_struct.hood_voltage) : nullptr,
          status->fbb());

  bool intake_out_jostle = false;

  if (unsafe_goal != nullptr) {
    if (unsafe_goal->shooting() &&
        shooting_start_time_ == aos::monotonic_clock::min_time) {
      shooting_start_time_ = position_timestamp;
    }

    if (unsafe_goal->shooting()) {
      intake_joint_.set_max_acceleration(30.0);
      constexpr std::chrono::milliseconds kPeriod =
          std::chrono::milliseconds(250);
      if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
          kPeriod) {
        intake_joint_.set_min_position(-0.25);
        intake_out_jostle = false;
      } else {
        intake_out_jostle = true;
        intake_joint_.set_min_position(-0.75);
      }
    } else {
      intake_joint_.clear_max_acceleration();
      intake_joint_.clear_min_position();
    }

    if (!unsafe_goal->shooting()) {
      shooting_start_time_ = aos::monotonic_clock::min_time;
    }
  }

  flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
      intake_joint_.Iterate(
          unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
          position->intake_joint(),
          output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
          status->fbb());

  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
      *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
                                                   ? aimer_.TurretGoal()
                                                   : unsafe_goal->turret())
                                            : nullptr;

  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
      turret_status_offset;
  if (has_turret_) {
    turret_status_offset = turret_.Iterate(
        turret_goal, position->turret(),
        output != nullptr ? &(output_struct.turret_voltage) : nullptr,
        status->fbb());
  } else {
    PotAndAbsoluteEncoderProfiledJointStatus::Builder turret_builder(
        *status->fbb());
    turret_builder.add_position(M_PI);
    turret_builder.add_velocity(0.0);
    turret_status_offset = turret_builder.Finish();
  }

  flatbuffers::Offset<ShooterStatus> shooter_status_offset =
      shooter_.RunIteration(
          unsafe_goal != nullptr
              ? (unsafe_goal->shooter_tracking() ? &shooter_goal.message()
                                                 : unsafe_goal->shooter())
              : nullptr,
          position->shooter(), status->fbb(),
          output != nullptr ? &(output_struct) : nullptr, position_timestamp);

  const AbsoluteAndAbsoluteEncoderProfiledJointStatus *const hood_status =
      GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);

  const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
      GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);

  if (output != nullptr) {
    // Friction is a pain and putting a really high burden on the integrator.
    // TODO(james): I'm not sure how helpful this gain is.
    const double turret_velocity_sign =
        turret_status->velocity() * kTurretFrictionGain;
    output_struct.turret_voltage +=
        std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
                   kTurretFrictionVoltageLimit);
    const double time_sec =
        aos::time::DurationInSeconds(position_timestamp.time_since_epoch());
    output_struct.turret_voltage +=
        kTurretDitherGain * std::sin(2.0 * M_PI * time_sec * 30.0);
    output_struct.turret_voltage =
        std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
                   turret_.operating_voltage());
  }

  bool zeroed;
  bool estopped;

  {
    const AbsoluteEncoderProfiledJointStatus *const intake_status =
        GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);

    zeroed = hood_status->zeroed() && intake_status->zeroed() &&
             turret_status->zeroed();
    estopped = hood_status->estopped() || intake_status->estopped() ||
               turret_status->estopped();
  }

  flatbuffers::Offset<flatbuffers::Vector<Subsystem>>
      subsystems_not_ready_offset;
  const bool turret_ready =
      (std::abs(turret_.goal(0) - turret_.position()) < 0.025) || !has_turret_;
  if (unsafe_goal && unsafe_goal->shooting() &&
      (!shooter_.ready() || !turret_ready)) {
    aos::SizedArray<Subsystem, 3> subsystems_not_ready;
    if (!shooter_.finisher_ready()) {
      subsystems_not_ready.push_back(Subsystem::FINISHER);
    }
    if (!shooter_.accelerator_ready()) {
      subsystems_not_ready.push_back(Subsystem::ACCELERATOR);
    }
    if (!turret_ready) {
      subsystems_not_ready.push_back(Subsystem::TURRET);
    }

    subsystems_not_ready_offset = status->fbb()->CreateVector(
        subsystems_not_ready.data(), subsystems_not_ready.size());
  }

  Status::Builder status_builder = status->MakeBuilder<Status>();

  status_builder.add_zeroed(zeroed);
  status_builder.add_estopped(estopped);

  status_builder.add_hood(hood_status_offset);
  status_builder.add_intake(intake_status_offset);
  status_builder.add_turret(turret_status_offset);
  status_builder.add_shooter(shooter_status_offset);
  status_builder.add_aimer(aimer_status_offset);
  status_builder.add_subsystems_not_ready(subsystems_not_ready_offset);

  status_builder.add_send_failures(status_failure_counter_.failures());

  status_failure_counter_.Count(status->Send(status_builder.Finish()));

  if (output != nullptr) {
    output_struct.washing_machine_spinner_voltage = 0.0;
    output_struct.feeder_voltage = 0.0;
    output_struct.intake_roller_voltage = 0.0;
    output_struct.climber_voltage = 0.0;
    if (unsafe_goal) {
      if (unsafe_goal->has_turret()) {
        output_struct.climber_voltage =
            std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);

        // Make sure the turret is relatively close to the goal before turning
        // the climber on.
        CHECK(unsafe_goal->has_turret());
        if (std::abs(unsafe_goal->turret()->unsafe_goal() -
                     turret_.position()) > 0.1 &&
            has_turret_) {
          output_struct.climber_voltage = 0;
        }
      }

      if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
        preloading_timeout_ = position_timestamp + kPreloadingTimeout;
      }

      if (position_timestamp <= preloading_timeout_ &&
          !position->intake_beambreak_triggered()) {
        output_struct.washing_machine_spinner_voltage = 5.0;
        output_struct.feeder_voltage = 12.0;

        preloading_backpower_timeout_ =
            position_timestamp + kPreloadingBackpowerDuration;
      }

      if (position->intake_beambreak_triggered() &&
          position_timestamp <= preloading_backpower_timeout_) {
        output_struct.feeder_voltage = -12.0;
      }

      if (unsafe_goal->has_feed_voltage_override()) {
        output_struct.feeder_voltage = unsafe_goal->feed_voltage_override();
        output_struct.washing_machine_spinner_voltage = -5.0;
        preloading_timeout_ = position_timestamp;
      }

      if (unsafe_goal->shooting()) {
        if ((shooter_.ready() ||
             (!has_turret_ && shooter_.accelerator_ready())) &&
            turret_ready) {
          output_struct.feeder_voltage = 12.0;
        }

        if (!intake_out_jostle) {
          output_struct.washing_machine_spinner_voltage = 5.0;
        } else {
          output_struct.washing_machine_spinner_voltage = -5.0;
        }
        output_struct.intake_roller_voltage = 3.0;
      } else {
        output_struct.intake_roller_voltage =
            unsafe_goal->roller_voltage() +
            std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0f);
      }
    }

    output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
  }
}

}  // namespace y2020::control_loops::superstructure