y2016/actors/autonomous_actor.cc

#include "y2016/actors/autonomous_actor.h"

#include <inttypes.h>

#include <chrono>
#include <cmath>

#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"

#include "y2016/control_loops/drivetrain/drivetrain_base.h"
#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/shooter/shooter_status_generated.h"
#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
#include "y2016/queues/ball_detector_generated.h"
#include "y2016/vision/vision_generated.h"

namespace y2016 {
namespace actors {
using ::aos::monotonic_clock;
using ::frc971::ProfileParametersT;
namespace superstructure = y2016::control_loops::superstructure;
namespace shooter = y2016::control_loops::shooter;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;

namespace {
ProfileParametersT MakeProfileParameters(float max_velocity,
                                         float max_acceleration) {
  ProfileParametersT result;
  result.max_velocity = max_velocity;
  result.max_acceleration = max_acceleration;
  return result;
}

const ProfileParametersT kSlowDrive = MakeProfileParameters(0.8, 2.5);
const ProfileParametersT kLowBarDrive = MakeProfileParameters(1.3, 2.5);
const ProfileParametersT kMoatDrive = MakeProfileParameters(1.2, 3.5);
const ProfileParametersT kRealignDrive = MakeProfileParameters(2.0, 2.5);
const ProfileParametersT kRockWallDrive = MakeProfileParameters(0.8, 2.5);
const ProfileParametersT kFastDrive = MakeProfileParameters(3.0, 2.5);

const ProfileParametersT kSlowTurn = MakeProfileParameters(0.8, 3.0);
const ProfileParametersT kFastTurn = MakeProfileParameters(3.0, 10.0);
const ProfileParametersT kStealTurn = MakeProfileParameters(4.0, 15.0);
const ProfileParametersT kSwerveTurn = MakeProfileParameters(2.0, 7.0);
const ProfileParametersT kFinishTurn = MakeProfileParameters(2.0, 5.0);

const ProfileParametersT kTwoBallLowDrive = MakeProfileParameters(1.7, 3.5);
const ProfileParametersT kTwoBallFastDrive = MakeProfileParameters(3.0, 1.5);
const ProfileParametersT kTwoBallReturnDrive = MakeProfileParameters(3.0, 1.9);
const ProfileParametersT kTwoBallReturnSlow = MakeProfileParameters(3.0, 2.5);
const ProfileParametersT kTwoBallBallPickup = MakeProfileParameters(2.0, 1.75);
const ProfileParametersT kTwoBallBallPickupAccel =
    MakeProfileParameters(2.0, 2.5);

}  // namespace

AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
    : frc971::autonomous::BaseAutonomousActor(
          event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
      vision_align_actor_factory_(
          actors::VisionAlignActor::MakeFactory(event_loop)),
      vision_status_fetcher_(
          event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
              "/superstructure")),
      ball_detector_fetcher_(
          event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
              "/superstructure")),
      shooter_goal_sender_(
          event_loop->MakeSender<::y2016::control_loops::shooter::Goal>(
              "/shooter")),
      shooter_status_fetcher_(
          event_loop->MakeFetcher<::y2016::control_loops::shooter::Status>(
              "/shooter")),
      superstructure_status_fetcher_(
          event_loop
              ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
                  "/superstructure")),
      superstructure_goal_sender_(
          event_loop->MakeSender<::y2016::control_loops::superstructure::Goal>(
              "/superstructure")) {}

constexpr double kDoNotTurnCare = 2.0;

void AutonomousActor::MoveSuperstructure(
    double intake, double shoulder, double wrist,
    const ProfileParametersT intake_params,
    const ProfileParametersT shoulder_params,
    const ProfileParametersT wrist_params, bool traverse_up,
    double roller_power) {
  superstructure_goal_ = {intake, shoulder, wrist};

  auto builder = superstructure_goal_sender_.MakeBuilder();

  superstructure::Goal::Builder superstructure_goal_builder =
      builder.MakeBuilder<superstructure::Goal>();

  superstructure_goal_builder.add_angle_intake(intake);
  superstructure_goal_builder.add_angle_shoulder(shoulder);
  superstructure_goal_builder.add_angle_wrist(wrist);

  superstructure_goal_builder.add_max_angular_velocity_intake(
      intake_params.max_velocity);
  superstructure_goal_builder.add_max_angular_velocity_shoulder(
      shoulder_params.max_velocity);
  superstructure_goal_builder.add_max_angular_velocity_wrist(
      wrist_params.max_velocity);

  superstructure_goal_builder.add_max_angular_acceleration_intake(
      intake_params.max_acceleration);
  superstructure_goal_builder.add_max_angular_acceleration_shoulder(
      shoulder_params.max_acceleration);
  superstructure_goal_builder.add_max_angular_acceleration_wrist(
      wrist_params.max_acceleration);

  superstructure_goal_builder.add_voltage_top_rollers(roller_power);
  superstructure_goal_builder.add_voltage_bottom_rollers(roller_power);

  superstructure_goal_builder.add_traverse_unlatched(true);
  superstructure_goal_builder.add_traverse_down(!traverse_up);
  superstructure_goal_builder.add_voltage_climber(0.0);
  superstructure_goal_builder.add_unfold_climber(false);

  if (!builder.Send(superstructure_goal_builder.Finish())) {
    AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
  }
}

void AutonomousActor::OpenShooter() {
  shooter_speed_ = 0.0;

  auto builder = shooter_goal_sender_.MakeBuilder();
  shooter::Goal::Builder shooter_goal_builder =
      builder.MakeBuilder<shooter::Goal>();
  shooter_goal_builder.add_angular_velocity(shooter_speed_);
  shooter_goal_builder.add_clamp_open(true);
  shooter_goal_builder.add_push_to_shooter(false);
  shooter_goal_builder.add_force_lights_on(false);
  if (!builder.Send(shooter_goal_builder.Finish())) {
    AOS_LOG(ERROR, "Sending shooter goal failed.\n");
  }
}

void AutonomousActor::CloseShooter() {
  shooter_speed_ = 0.0;

  auto builder = shooter_goal_sender_.MakeBuilder();
  shooter::Goal::Builder shooter_goal_builder =
      builder.MakeBuilder<shooter::Goal>();
  shooter_goal_builder.add_angular_velocity(shooter_speed_);
  shooter_goal_builder.add_clamp_open(false);
  shooter_goal_builder.add_push_to_shooter(false);
  shooter_goal_builder.add_force_lights_on(false);

  if (!builder.Send(shooter_goal_builder.Finish())) {
    AOS_LOG(ERROR, "Sending shooter goal failed.\n");
  }
}

void AutonomousActor::SetShooterSpeed(double speed) {
  shooter_speed_ = speed;

  // In auto, we want to have the lights on whenever possible since we have no
  // hope of a human aligning the robot.
  bool force_lights_on = shooter_speed_ > 1.0;

  auto builder = shooter_goal_sender_.MakeBuilder();
  shooter::Goal::Builder shooter_goal_builder =
      builder.MakeBuilder<shooter::Goal>();
  shooter_goal_builder.add_angular_velocity(shooter_speed_);
  shooter_goal_builder.add_clamp_open(false);
  shooter_goal_builder.add_push_to_shooter(false);
  shooter_goal_builder.add_force_lights_on(force_lights_on);

  if (!builder.Send(shooter_goal_builder.Finish())) {
    AOS_LOG(ERROR, "Sending shooter goal failed.\n");
  }
}

void AutonomousActor::Shoot() {
  uint32_t initial_shots = 0;

  shooter_status_fetcher_.Fetch();
  if (shooter_status_fetcher_.get()) {
    initial_shots = shooter_status_fetcher_->shots();
  }

  // In auto, we want to have the lights on whenever possible since we have no
  // hope of a human aligning the robot.
  bool force_lights_on = shooter_speed_ > 1.0;

  auto builder = shooter_goal_sender_.MakeBuilder();
  shooter::Goal::Builder shooter_goal_builder =
      builder.MakeBuilder<shooter::Goal>();
  shooter_goal_builder.add_angular_velocity(shooter_speed_);
  shooter_goal_builder.add_clamp_open(false);
  shooter_goal_builder.add_push_to_shooter(true);
  shooter_goal_builder.add_force_lights_on(force_lights_on);

  if (!builder.Send(shooter_goal_builder.Finish())) {
    AOS_LOG(ERROR, "Sending shooter goal failed.\n");
  }

  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);
  while (true) {
    if (ShouldCancel()) return;

    // Wait for the shot count to change so we know when the shot is complete.
    shooter_status_fetcher_.Fetch();
    if (shooter_status_fetcher_.get()) {
      if (initial_shots < shooter_status_fetcher_->shots()) {
        return;
      }
    }
    phased_loop.SleepUntilNext();
  }
}

void AutonomousActor::WaitForShooterSpeed() {
  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);
  while (true) {
    if (ShouldCancel()) return;

    shooter_status_fetcher_.Fetch();
    if (shooter_status_fetcher_.get()) {
      if (shooter_status_fetcher_->left()->ready() &&
          shooter_status_fetcher_->right()->ready()) {
        return;
      }
    }
    phased_loop.SleepUntilNext();
  }
}

void AutonomousActor::AlignWithVisionGoal() {
  vision_align_action::VisionAlignActionParamsT params;
  vision_action_ = vision_align_actor_factory_.Make(params);
  vision_action_->Start();
}

void AutonomousActor::WaitForAlignedWithVision(
    chrono::nanoseconds align_duration) {
  bool vision_valid = false;
  double last_angle = 0.0;
  int ready_to_fire = 0;

  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);
  const monotonic_clock::time_point end_time = monotonic_now() + align_duration;
  while (end_time > monotonic_now()) {
    if (ShouldCancel()) break;

    vision_status_fetcher_.Fetch();
    if (vision_status_fetcher_.get()) {
      vision_valid = (vision_status_fetcher_->left_image_valid() &&
                      vision_status_fetcher_->right_image_valid());
      last_angle = vision_status_fetcher_->horizontal_angle();
    }

    drivetrain_status_fetcher_.Fetch();
    drivetrain_goal_fetcher_.Fetch();

    if (drivetrain_status_fetcher_.get() && drivetrain_goal_fetcher_.get()) {
      const double left_goal = drivetrain_goal_fetcher_->left_goal();
      const double right_goal = drivetrain_goal_fetcher_->right_goal();
      const double left_current =
          drivetrain_status_fetcher_->estimated_left_position();
      const double right_current =
          drivetrain_status_fetcher_->estimated_right_position();
      const double left_velocity =
          drivetrain_status_fetcher_->estimated_left_velocity();
      const double right_velocity =
          drivetrain_status_fetcher_->estimated_right_velocity();

      if (vision_valid && ::std::abs(last_angle) < 0.02 &&
          ::std::abs((left_goal - right_goal) -
                     (left_current - right_current)) /
                  dt_config_.robot_radius / 2.0 <
              0.02 &&
          ::std::abs(left_velocity - right_velocity) < 0.01) {
        ++ready_to_fire;
      } else {
        ready_to_fire = 0;
      }
      if (ready_to_fire > 15) {
        break;
        AOS_LOG(INFO, "Vision align success!\n");
      }
    }
    phased_loop.SleepUntilNext();
  }

  vision_action_->Cancel();
  WaitUntilDoneOrCanceled(::std::move(vision_action_));
  AOS_LOG(INFO, "Done waiting for vision\n");
}

bool AutonomousActor::IntakeDone() {
  superstructure_status_fetcher_.Fetch();

  constexpr double kProfileError = 1e-5;
  constexpr double kEpsilon = 0.15;

  if (superstructure_status_fetcher_->state() < 12 ||
      superstructure_status_fetcher_->state() == 16) {
    AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
    return true;
  }

  if (::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
                 superstructure_goal_.intake) < kProfileError &&
      ::std::abs(
          superstructure_status_fetcher_->intake()->goal_angular_velocity()) <
          kProfileError) {
    AOS_LOG(DEBUG, "Profile done.\n");
    if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
                   superstructure_goal_.intake) < kEpsilon &&
        ::std::abs(
            superstructure_status_fetcher_->intake()->angular_velocity()) <
            kEpsilon) {
      AOS_LOG(INFO, "Near goal, done.\n");
      return true;
    }
  }
  return false;
}

bool AutonomousActor::SuperstructureProfileDone() {
  if (superstructure_status_fetcher_->state() < 12 ||
      superstructure_status_fetcher_->state() == 16) {
    AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
    return true;
  }

  constexpr double kProfileError = 1e-5;
  return ::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
                    superstructure_goal_.intake) < kProfileError &&
         ::std::abs(superstructure_status_fetcher_->shoulder()->goal_angle() -
                    superstructure_goal_.shoulder) < kProfileError &&
         ::std::abs(superstructure_status_fetcher_->wrist()->goal_angle() -
                    superstructure_goal_.wrist) < kProfileError &&
         ::std::abs(superstructure_status_fetcher_->intake()
                        ->goal_angular_velocity()) < kProfileError &&
         ::std::abs(superstructure_status_fetcher_->shoulder()
                        ->goal_angular_velocity()) < kProfileError &&
         ::std::abs(
             superstructure_status_fetcher_->wrist()->goal_angular_velocity()) <
             kProfileError;
}

bool AutonomousActor::SuperstructureDone() {
  superstructure_status_fetcher_.Fetch();

  constexpr double kEpsilon = 0.03;
  if (SuperstructureProfileDone()) {
    AOS_LOG(DEBUG, "Profile done.\n");
    if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
                   superstructure_goal_.intake) < (kEpsilon + 0.1) &&
        ::std::abs(superstructure_status_fetcher_->shoulder()->angle() -
                   superstructure_goal_.shoulder) < (kEpsilon + 0.05) &&
        ::std::abs(superstructure_status_fetcher_->wrist()->angle() -
                   superstructure_goal_.wrist) < (kEpsilon + 0.01) &&
        ::std::abs(
            superstructure_status_fetcher_->intake()->angular_velocity()) <
            (kEpsilon + 0.1) &&
        ::std::abs(
            superstructure_status_fetcher_->shoulder()->angular_velocity()) <
            (kEpsilon + 0.10) &&
        ::std::abs(
            superstructure_status_fetcher_->wrist()->angular_velocity()) <
            (kEpsilon + 0.05)) {
      AOS_LOG(INFO, "Near goal, done.\n");
      return true;
    }
  }
  return false;
}

void AutonomousActor::WaitForIntake() {
  WaitUntil(::std::bind(&AutonomousActor::IntakeDone, this));
}

void AutonomousActor::WaitForSuperstructure() {
  WaitUntil(::std::bind(&AutonomousActor::SuperstructureDone, this));
}

void AutonomousActor::WaitForSuperstructureProfile() {
  WaitUntil([this]() {
    superstructure_status_fetcher_.Fetch();
    return SuperstructureProfileDone();
  });
}

void AutonomousActor::WaitForSuperstructureLow() {
  WaitUntil([this]() {
    superstructure_status_fetcher_.Fetch();

    return SuperstructureProfileDone() ||
           superstructure_status_fetcher_->shoulder()->angle() < 0.1;
  });
}

void AutonomousActor::BackLongShotLowBarTwoBall() {
  AOS_LOG(INFO, "Expanding for back long shot\n");
  MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 6.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::BackLongShotTwoBall() {
  AOS_LOG(INFO, "Expanding for back long shot\n");
  MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 6.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::BackLongShotTwoBallFinish() {
  AOS_LOG(INFO, "Expanding for back long shot\n");
  MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 6.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::BackLongShot() {
  AOS_LOG(INFO, "Expanding for back long shot\n");
  MoveSuperstructure(0.80, M_PI / 2.0 - 0.2, -0.62,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 6.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::BackMiddleShot() {
  AOS_LOG(INFO, "Expanding for back middle shot\n");
  MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::FrontLongShot() {
  AOS_LOG(INFO, "Expanding for front long shot\n");
  MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 6.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
}

void AutonomousActor::FrontMiddleShot() {
  AOS_LOG(INFO, "Expanding for front middle shot\n");
  MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.44,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), true, 0.0);
}

void AutonomousActor::TuckArm(bool low_bar, bool traverse_down) {
  MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0,
                     MakeProfileParameters(7.0, 40.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), !traverse_down, 0.0);
}

void AutonomousActor::DoFullShot() {
  if (ShouldCancel()) return;
  // Make sure that the base is aligned with the base.
  AOS_LOG(INFO, "Waiting for the superstructure\n");
  WaitForSuperstructure();

  this_thread::sleep_for(chrono::milliseconds(500));

  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Triggering the vision actor\n");
  AlignWithVisionGoal();

  // Wait for the drive base to be aligned with the target and make sure that
  // the shooter is up to speed.
  AOS_LOG(INFO, "Waiting for vision to be aligned\n");
  WaitForAlignedWithVision(chrono::milliseconds(2000));
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Waiting for shooter to be up to speed\n");
  WaitForShooterSpeed();
  if (ShouldCancel()) return;

  this_thread::sleep_for(chrono::milliseconds(300));
  AOS_LOG(INFO, "Shoot!\n");
  Shoot();

  // Turn off the shooter and fold up the superstructure.
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Stopping shooter\n");
  SetShooterSpeed(0.0);
  AOS_LOG(INFO, "Folding superstructure back down\n");
  TuckArm(false, false);

  // Wait for everything to be folded up.
  AOS_LOG(INFO, "Waiting for superstructure to be folded back down\n");
  WaitForSuperstructureLow();
}

void AutonomousActor::LowBarDrive() {
  TuckArm(false, true);
  StartDrive(-5.5, 0.0, kLowBarDrive, kSlowTurn);

  if (!WaitForDriveNear(5.3, 0.0)) return;
  TuckArm(true, true);

  if (!WaitForDriveNear(5.0, 0.0)) return;

  StartDrive(0.0, 0.0, kLowBarDrive, kSlowTurn);

  if (!WaitForDriveNear(3.0, 0.0)) return;

  StartDrive(0.0, 0.0, kLowBarDrive, kSlowTurn);

  if (!WaitForDriveNear(1.0, 0.0)) return;

  StartDrive(0, -M_PI / 4.0 - 0.2, kLowBarDrive, kSlowTurn);
}

void AutonomousActor::TippyDrive(double goal_distance, double tip_distance,
                                 double below, double above) {
  StartDrive(goal_distance, 0.0, kMoatDrive, kSlowTurn);
  if (!WaitForBelowAngle(below)) return;
  if (!WaitForAboveAngle(above)) return;
  // Ok, we are good now.  Compensate by moving the goal by the error.
  // Should be here at 2.7
  drivetrain_status_fetcher_.Fetch();
  if (drivetrain_status_fetcher_.get()) {
    const double left_error =
        (initial_drivetrain_.left -
         drivetrain_status_fetcher_->estimated_left_position());
    const double right_error =
        (initial_drivetrain_.right -
         drivetrain_status_fetcher_->estimated_right_position());
    const double distance_to_go = (left_error + right_error) / 2.0;
    const double distance_compensation =
        goal_distance - tip_distance - distance_to_go;
    AOS_LOG(INFO, "Going %f further at the bump\n", distance_compensation);
    StartDrive(distance_compensation, 0.0, kMoatDrive, kSlowTurn);
  }
}

void AutonomousActor::MiddleDrive() {
  TuckArm(false, false);
  TippyDrive(3.65, 2.7, -0.2, 0.0);
  if (!WaitForDriveDone()) return;
}

void AutonomousActor::OneFromMiddleDrive(bool left) {
  const double kTurnAngle = left ? -0.41 : 0.41;
  TuckArm(false, false);
  TippyDrive(4.05, 2.7, -0.2, 0.0);

  if (!WaitForDriveDone()) return;
  StartDrive(0.0, kTurnAngle, kRealignDrive, kFastTurn);
}

void AutonomousActor::TwoFromMiddleDrive() {
  TuckArm(false, false);
  constexpr double kDriveDistance = 5.10;
  TippyDrive(kDriveDistance, 2.7, -0.2, 0.0);

  if (!WaitForDriveNear(kDriveDistance - 3.0, 2.0)) return;
  StartDrive(0, -M_PI / 2 - 0.10, kMoatDrive, kFastTurn);

  if (!WaitForDriveDone()) return;
  StartDrive(0, M_PI / 3 + 0.35, kMoatDrive, kFastTurn);
}

void AutonomousActor::CloseIfBall() {
  ball_detector_fetcher_.Fetch();
  if (ball_detector_fetcher_.get()) {
    const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
    if (ball_detected) {
      CloseShooter();
    }
  }
}

void AutonomousActor::WaitForBallOrDriveDone() {
  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);
  while (true) {
    if (ShouldCancel()) {
      return;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (IsDriveDone()) {
      return;
    }

    ball_detector_fetcher_.Fetch();
    if (ball_detector_fetcher_.get()) {
      const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
      if (ball_detected) {
        return;
      }
    }
  }
}

void AutonomousActor::TwoBallAuto() {
  const monotonic_clock::time_point start_time = monotonic_now();
  OpenShooter();
  MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 12.0);
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Waiting for the intake to come down.\n");

  WaitForIntake();
  AOS_LOG(INFO, "Intake done at %f seconds, starting to drive\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  if (ShouldCancel()) return;
  const double kDriveDistance = 5.05;
  StartDrive(-kDriveDistance, 0.0, kTwoBallLowDrive, kSlowTurn);

  StartDrive(0.0, 0.4, kTwoBallLowDrive, kSwerveTurn);
  if (!WaitForDriveNear(kDriveDistance - 0.5, kDoNotTurnCare)) return;

  // Check if the ball is there.
  bool first_ball_there = true;
  ball_detector_fetcher_.Fetch();
  if (ball_detector_fetcher_.get()) {
    const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
    first_ball_there = ball_detected;
    AOS_LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
            ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  }
  MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
  AOS_LOG(INFO,
          "Shutting off rollers at %f seconds, starting to straighten out\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
  MoveSuperstructure(-0.05, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 0.0);
  CloseShooter();
  if (!WaitForDriveNear(kDriveDistance - 2.4, kDoNotTurnCare)) return;

  // We are now under the low bar.  Start lifting.
  BackLongShotLowBarTwoBall();
  AOS_LOG(INFO, "Spinning up the shooter wheels\n");
  SetShooterSpeed(640.0);
  StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);

  if (!WaitForDriveNear(1.50, kDoNotTurnCare)) return;
  constexpr double kShootTurnAngle = -M_PI / 4.0 - 0.05;
  StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
  BackLongShotTwoBall();

  if (!WaitForDriveDone()) return;
  AOS_LOG(INFO, "First shot done driving at %f seconds\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  WaitForSuperstructureProfile();

  if (ShouldCancel()) return;
  AlignWithVisionGoal();

  WaitForShooterSpeed();
  if (ShouldCancel()) return;

  constexpr chrono::milliseconds kVisionExtra{0};
  WaitForAlignedWithVision(chrono::milliseconds(500) + kVisionExtra);
  BackLongShotTwoBallFinish();
  WaitForSuperstructureProfile();
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Shoot!\n");
  if (first_ball_there) {
    Shoot();
  } else {
    AOS_LOG(INFO, "Nah, not shooting\n");
  }

  AOS_LOG(INFO, "First shot at %f seconds\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  if (ShouldCancel()) return;

  SetShooterSpeed(0.0);
  AOS_LOG(INFO, "Folding superstructure back down\n");
  TuckArm(true, true);

  // Undo vision move.
  StartDrive(0.0, 0.0, kTwoBallFastDrive, kFinishTurn);
  if (!WaitForDriveDone()) return;

  constexpr double kBackDrive = 3.09 - 0.4;
  StartDrive(kBackDrive, 0.0, kTwoBallReturnDrive, kSlowTurn);
  if (!WaitForDriveNear(kBackDrive - 0.19, kDoNotTurnCare)) return;
  StartDrive(0, -kShootTurnAngle, kTwoBallReturnDrive, kSwerveTurn);
  if (!WaitForDriveNear(1.0, kDoNotTurnCare)) return;
  StartDrive(0, 0, kTwoBallReturnSlow, kSwerveTurn);

  if (!WaitForDriveNear(0.06, kDoNotTurnCare)) return;
  AOS_LOG(INFO, "At Low Bar %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  OpenShooter();
  constexpr double kSecondBallAfterBarDrive = 2.10;
  StartDrive(kSecondBallAfterBarDrive, 0.0, kTwoBallBallPickupAccel, kSlowTurn);
  if (!WaitForDriveNear(kSecondBallAfterBarDrive - 0.5, kDoNotTurnCare)) return;
  constexpr double kBallSmallWallTurn = -0.11;
  StartDrive(0, kBallSmallWallTurn, kTwoBallBallPickup, kFinishTurn);

  MoveSuperstructure(0.03, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 12.0);

  if (!WaitForDriveProfileDone()) return;

  MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), false, 12.0);

  AOS_LOG(INFO, "Done backing up %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  constexpr double kDriveBackDistance = 5.15 - 0.4;
  StartDrive(-kDriveBackDistance, 0.0, kTwoBallLowDrive, kFinishTurn);
  CloseIfBall();
  if (!WaitForDriveNear(kDriveBackDistance - 0.75, kDoNotTurnCare)) return;

  StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
  AOS_LOG(INFO, "Straightening up at %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  CloseIfBall();
  if (!WaitForDriveNear(kDriveBackDistance - 2.3, kDoNotTurnCare)) return;

  ball_detector_fetcher_.Fetch();
  if (ball_detector_fetcher_.get()) {
    const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
    if (!ball_detected) {
      if (!WaitForDriveDone()) return;
      AOS_LOG(INFO, "Aborting, no ball %f\n",
              ::aos::time::DurationInSeconds(monotonic_now() - start_time));
      return;
    }
  }
  CloseShooter();

  BackLongShotLowBarTwoBall();
  AOS_LOG(INFO, "Spinning up the shooter wheels\n");
  SetShooterSpeed(640.0);
  StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);

  if (!WaitForDriveNear(1.80, kDoNotTurnCare)) return;
  StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
  BackLongShotTwoBall();

  if (!WaitForDriveDone()) return;
  AOS_LOG(INFO, "Second shot done driving at %f seconds\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  WaitForSuperstructure();
  AlignWithVisionGoal();
  if (ShouldCancel()) return;

  WaitForShooterSpeed();
  if (ShouldCancel()) return;

  // 2.2 with 0.4 of vision.
  // 1.8 without any vision.
  AOS_LOG(INFO, "Going to vision align at %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  WaitForAlignedWithVision(
      (start_time + chrono::milliseconds(13500) + kVisionExtra * 2) -
      monotonic_now());
  BackLongShotTwoBallFinish();
  WaitForSuperstructureProfile();
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Shoot at %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  Shoot();

  AOS_LOG(INFO, "Second shot at %f seconds\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
  if (ShouldCancel()) return;

  SetShooterSpeed(0.0);
  AOS_LOG(INFO, "Folding superstructure back down\n");
  TuckArm(true, false);
  AOS_LOG(INFO, "Shot %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  WaitForSuperstructureLow();

  AOS_LOG(INFO, "Done %f\n",
          ::aos::time::DurationInSeconds(monotonic_now() - start_time));
}

void AutonomousActor::StealAndMoveOverBy(double distance) {
  OpenShooter();
  MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), true, 12.0);
  if (ShouldCancel()) return;
  AOS_LOG(INFO, "Waiting for the intake to come down.\n");

  WaitForIntake();
  if (ShouldCancel()) return;
  StartDrive(-distance, M_PI / 2.0, kFastDrive, kStealTurn);
  WaitForBallOrDriveDone();
  if (ShouldCancel()) return;
  MoveSuperstructure(1.0, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
                     MakeProfileParameters(4.0, 10.0),
                     MakeProfileParameters(10.0, 25.0), true, 12.0);

  if (!WaitForDriveDone()) return;
  StartDrive(0.0, M_PI / 2.0, kFastDrive, kStealTurn);
  if (!WaitForDriveDone()) return;
}

bool AutonomousActor::RunAction(
    const ::frc971::autonomous::AutonomousActionParams *params) {
  monotonic_clock::time_point start_time = monotonic_now();
  AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
          params->mode());

  InitializeEncoders();
  ResetDrivetrain();

  switch (params->mode()) {
    case 0:
      LowBarDrive();
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontLongShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(640.0);

      break;
    case 1:
      TwoFromMiddleDrive();
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 2:
      OneFromMiddleDrive(true);
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 3:
      MiddleDrive();
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 4:
      OneFromMiddleDrive(false);
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 5:
    case 15:
      TwoBallAuto();
      return true;
      break;
    case 6:
      StealAndMoveOverBy(3.10 + 2 * 52 * 2.54 / 100.0);
      if (ShouldCancel()) return true;

      TwoFromMiddleDrive();
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 7:
      StealAndMoveOverBy(2.95 + 52 * 2.54 / 100.0);
      if (ShouldCancel()) return true;

      OneFromMiddleDrive(true);
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

      break;
    case 8: {
      StealAndMoveOverBy(2.95);
      if (ShouldCancel()) return true;

      MiddleDrive();
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

    } break;
    case 9: {
      StealAndMoveOverBy(1.70);
      if (ShouldCancel()) return true;

      OneFromMiddleDrive(false);
      if (!WaitForDriveDone()) return true;
      // Get the superstructure to unfold and get ready for shooting.
      AOS_LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      AOS_LOG(INFO, "Spinning up the shooter wheels\n");
      SetShooterSpeed(600.0);

    } break;
    default:
      AOS_LOG(ERROR, "Invalid auto mode %d\n", params->mode());
      return true;
  }

  DoFullShot();

  StartDrive(0.5, 0.0, kMoatDrive, kFastTurn);
  if (!WaitForDriveDone()) return true;

  AOS_LOG(INFO, "Done %f\n",
      ::aos::time::DurationInSeconds(monotonic_now() - start_time));

  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);

  while (!ShouldCancel()) {
    phased_loop.SleepUntilNext();
  }
  AOS_LOG(DEBUG, "Done running\n");

  return true;
}

}  // namespace actors
}  // namespace y2016