y2016/actors/autonomous_actor.cc

#include "y2016/actors/autonomous_actor.h"

#include <inttypes.h>

#include <cmath>

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

#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/actors/autonomous_action.q.h"
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"

namespace y2016 {
namespace actors {
using ::frc971::control_loops::drivetrain_queue;

namespace {
const ProfileParameters kSlowDrive = {0.8, 2.5};
const ProfileParameters kLowBarDrive = {1.3, 2.5};
const ProfileParameters kMoatDrive = {1.2, 3.5};
const ProfileParameters kRealignDrive = {2.0, 2.5};
const ProfileParameters kRockWallDrive = {0.8, 2.5};
const ProfileParameters kFastDrive = {3.0, 2.5};

const ProfileParameters kSlowTurn = {0.8, 3.0};
const ProfileParameters kFastTurn = {3.0, 10.0};
const ProfileParameters kSwerveTurn = {2.0, 7.0};
const ProfileParameters kFinishTurn = {2.0, 5.0};

const ProfileParameters kTwoBallLowDrive = {1.7, 3.5};
const ProfileParameters kTwoBallFastDrive = {3.0, 1.5};
const ProfileParameters kTwoBallReturnDrive = {3.0, 1.9};
const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};

const double kDistanceShort = 0.25;
}  // namespace

AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
    : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s),
      dt_config_(control_loops::drivetrain::GetDrivetrainConfig()),
      initial_drivetrain_({0.0, 0.0}) {}

void AutonomousActor::ResetDrivetrain() {
  LOG(INFO, "resetting the drivetrain\n");
  drivetrain_queue.goal.MakeWithBuilder()
      .control_loop_driving(false)
      .highgear(true)
      .steering(0.0)
      .throttle(0.0)
      .left_goal(initial_drivetrain_.left)
      .left_velocity_goal(0)
      .right_goal(initial_drivetrain_.right)
      .right_velocity_goal(0)
      .Send();
}

void AutonomousActor::StartDrive(double distance, double angle,
                                 ProfileParameters linear,
                                 ProfileParameters angular) {
  {
    LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
    {
      const double dangle = angle * dt_config_.robot_radius;
      initial_drivetrain_.left += distance - dangle;
      initial_drivetrain_.right += distance + dangle;
    }

    auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
    drivetrain_message->control_loop_driving = true;
    drivetrain_message->highgear = true;
    drivetrain_message->steering = 0.0;
    drivetrain_message->throttle = 0.0;
    drivetrain_message->left_goal = initial_drivetrain_.left;
    drivetrain_message->left_velocity_goal = 0;
    drivetrain_message->right_goal = initial_drivetrain_.right;
    drivetrain_message->right_velocity_goal = 0;
    drivetrain_message->linear = linear;
    drivetrain_message->angular = angular;

    LOG_STRUCT(DEBUG, "drivetrain_goal", *drivetrain_message);

    drivetrain_message.Send();
  }
}

void AutonomousActor::InitializeEncoders() {
  drivetrain_queue.status.FetchAnother();
  initial_drivetrain_.left = drivetrain_queue.status->estimated_left_position;
  initial_drivetrain_.right = drivetrain_queue.status->estimated_right_position;
}

void AutonomousActor::WaitUntilDoneOrCanceled(
    ::std::unique_ptr<aos::common::actions::Action> action) {
  if (!action) {
    LOG(ERROR, "No action, not waiting\n");
    return;
  }

  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  while (true) {
    // Poll the running bit and see if we should cancel.
    phased_loop.SleepUntilNext();
    if (!action->Running() || ShouldCancel()) {
      return;
    }
  }
}

bool AutonomousActor::WaitForDriveNear(double distance, double angle) {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  constexpr double kPositionTolerance = 0.02;
  constexpr double kProfileTolerance = 0.001;

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (drivetrain_queue.status.get()) {
      const double left_profile_error =
          (initial_drivetrain_.left -
           drivetrain_queue.status->profiled_left_position_goal);
      const double right_profile_error =
          (initial_drivetrain_.right -
           drivetrain_queue.status->profiled_right_position_goal);

      const double left_error =
          (initial_drivetrain_.left -
           drivetrain_queue.status->estimated_left_position);
      const double right_error =
          (initial_drivetrain_.right -
           drivetrain_queue.status->estimated_right_position);

      const double profile_distance_to_go =
          (left_profile_error + right_profile_error) / 2.0;
      const double profile_angle_to_go =
          (right_profile_error - left_profile_error) /
          (dt_config_.robot_radius * 2.0);

      const double distance_to_go = (left_error + right_error) / 2.0;
      const double angle_to_go =
          (right_error - left_error) / (dt_config_.robot_radius * 2.0);

      if (::std::abs(profile_distance_to_go) < distance + kProfileTolerance &&
          ::std::abs(profile_angle_to_go) < angle + kProfileTolerance &&
          ::std::abs(distance_to_go) < distance + kPositionTolerance &&
          ::std::abs(angle_to_go) < angle + kPositionTolerance) {
        LOG(INFO, "Closer than %f distance, %f angle\n", distance, angle);
        return true;
      }
    }
  }
}

bool AutonomousActor::WaitForDriveProfileDone() {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  constexpr double kProfileTolerance = 0.001;

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (drivetrain_queue.status.get()) {
      if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
                     initial_drivetrain_.left) < kProfileTolerance &&
          ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
                     initial_drivetrain_.right) < kProfileTolerance) {
        LOG(INFO, "Finished drive\n");
        return true;
      }
    }
  }
}

bool AutonomousActor::WaitForMaxBy(double angle) {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  double max_angle = -M_PI;
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_queue.status.get()) {
      if (drivetrain_queue.status->ground_angle > max_angle) {
        max_angle = drivetrain_queue.status->ground_angle;
      }
      if (drivetrain_queue.status->ground_angle < max_angle - angle) {
        return true;
      }
    }
  }
}

bool AutonomousActor::WaitForAboveAngle(double angle) {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_queue.status.get()) {
      if (drivetrain_queue.status->ground_angle > angle) {
        return true;
      }
    }
  }
}

bool AutonomousActor::WaitForBelowAngle(double angle) {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_queue.status.get()) {
      if (drivetrain_queue.status->ground_angle < angle) {
        return true;
      }
    }
  }
}

bool AutonomousActor::IsDriveDone() {
  constexpr double kPositionTolerance = 0.02;
  constexpr double kVelocityTolerance = 0.10;
  constexpr double kProfileTolerance = 0.001;

  if (drivetrain_queue.status.get()) {
    if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
                   initial_drivetrain_.left) < kProfileTolerance &&
        ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
                   initial_drivetrain_.right) < kProfileTolerance &&
        ::std::abs(drivetrain_queue.status->estimated_left_position -
                   initial_drivetrain_.left) < kPositionTolerance &&
        ::std::abs(drivetrain_queue.status->estimated_right_position -
                   initial_drivetrain_.right) < kPositionTolerance &&
        ::std::abs(drivetrain_queue.status->estimated_left_velocity) <
            kVelocityTolerance &&
        ::std::abs(drivetrain_queue.status->estimated_right_velocity) <
            kVelocityTolerance) {
      LOG(INFO, "Finished drive\n");
      return true;
    }
  }
  return false;
}

bool AutonomousActor::WaitForDriveDone() {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_queue.status.FetchLatest();
    if (IsDriveDone()) {
      return true;
    }
  }
}

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

  auto new_superstructure_goal =
      ::y2016::control_loops::superstructure_queue.goal.MakeMessage();

  new_superstructure_goal->angle_intake = intake;
  new_superstructure_goal->angle_shoulder = shoulder;
  new_superstructure_goal->angle_wrist = wrist;

  new_superstructure_goal->max_angular_velocity_intake =
      intake_params.max_velocity;
  new_superstructure_goal->max_angular_velocity_shoulder =
      shoulder_params.max_velocity;
  new_superstructure_goal->max_angular_velocity_wrist =
      wrist_params.max_velocity;

  new_superstructure_goal->max_angular_acceleration_intake =
      intake_params.max_acceleration;
  new_superstructure_goal->max_angular_acceleration_shoulder =
      shoulder_params.max_acceleration;
  new_superstructure_goal->max_angular_acceleration_wrist =
      wrist_params.max_acceleration;

  new_superstructure_goal->voltage_top_rollers = roller_power;
  new_superstructure_goal->voltage_bottom_rollers = roller_power;

  new_superstructure_goal->traverse_unlatched = true;
  new_superstructure_goal->traverse_down = !traverse_up;
  new_superstructure_goal->voltage_climber = 0.0;
  new_superstructure_goal->unfold_climber = false;

  if (!new_superstructure_goal.Send()) {
    LOG(ERROR, "Sending superstructure goal failed.\n");
  }
}

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

  if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
           .angular_velocity(shooter_speed_)
           .clamp_open(true)
           .push_to_shooter(false)
           .force_lights_on(false)
           .Send()) {
    LOG(ERROR, "Sending shooter goal failed.\n");
  }
}

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

  if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
           .angular_velocity(shooter_speed_)
           .clamp_open(false)
           .push_to_shooter(false)
           .force_lights_on(false)
           .Send()) {
    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;

  if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
           .angular_velocity(shooter_speed_)
           .clamp_open(false)
           .push_to_shooter(false)
           .force_lights_on(force_lights_on)
           .Send()) {
    LOG(ERROR, "Sending shooter goal failed.\n");
  }
}

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

  control_loops::shooter::shooter_queue.status.FetchLatest();
  if (control_loops::shooter::shooter_queue.status.get()) {
    initial_shots = control_loops::shooter::shooter_queue.status->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;

  if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
           .angular_velocity(shooter_speed_)
           .clamp_open(false)
           .push_to_shooter(true)
           .force_lights_on(force_lights_on)
           .Send()) {
    LOG(ERROR, "Sending shooter goal failed.\n");
  }

  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  while (true) {
    if (ShouldCancel()) return;

    // Wait for the shot count to change so we know when the shot is complete.
    control_loops::shooter::shooter_queue.status.FetchLatest();
    if (control_loops::shooter::shooter_queue.status.get()) {
      if (initial_shots < control_loops::shooter::shooter_queue.status->shots) {
        return;
      }
    }
    phased_loop.SleepUntilNext();
  }
}

void AutonomousActor::WaitForShooterSpeed() {
  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  while (true) {
    if (ShouldCancel()) return;

    control_loops::shooter::shooter_queue.status.FetchLatest();
    if (control_loops::shooter::shooter_queue.status.get()) {
      if (control_loops::shooter::shooter_queue.status->left.ready &&
          control_loops::shooter::shooter_queue.status->right.ready) {
        return;
      }
    }
    phased_loop.SleepUntilNext();
  }
}

void AutonomousActor::AlignWithVisionGoal() {
  actors::VisionAlignActionParams params;
  vision_action_ = ::std::move(actors::MakeVisionAlignAction(params));
  vision_action_->Start();
}

void AutonomousActor::WaitForAlignedWithVision(
    ::aos::time::Time align_duration) {
  bool vision_valid = false;
  double last_angle = 0.0;
  int ready_to_fire = 0;

  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);
  ::aos::time::Time end_time =
      ::aos::time::Time::Now() + align_duration;
  while (end_time > ::aos::time::Time::Now()) {
    if (ShouldCancel()) break;

    ::y2016::vision::vision_status.FetchLatest();
    if (::y2016::vision::vision_status.get()) {
      vision_valid = (::y2016::vision::vision_status->left_image_valid &&
                       ::y2016::vision::vision_status->right_image_valid);
      last_angle = ::y2016::vision::vision_status->horizontal_angle;
    }

    drivetrain_queue.status.FetchLatest();
    drivetrain_queue.goal.FetchLatest();

    if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
      const double left_goal = drivetrain_queue.goal->left_goal;
      const double right_goal = drivetrain_queue.goal->right_goal;
      const double left_current =
          drivetrain_queue.status->estimated_left_position;
      const double right_current =
          drivetrain_queue.status->estimated_right_position;
      const double left_velocity =
          drivetrain_queue.status->estimated_left_velocity;
      const double right_velocity =
          drivetrain_queue.status->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;
        LOG(INFO, "Vision align success!\n");
      }
    }
    phased_loop.SleepUntilNext();
  }

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

bool AutonomousActor::IntakeDone() {
  control_loops::superstructure_queue.status.FetchAnother();

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

  if (control_loops::superstructure_queue.status->state < 12 ||
      control_loops::superstructure_queue.status->state == 16) {
    LOG(ERROR, "Superstructure no longer running, aborting action\n");
    return true;
  }

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

bool AutonomousActor::SuperstructureProfileDone() {
  constexpr double kProfileError = 1e-5;
  return ::std::abs(
             control_loops::superstructure_queue.status->intake.goal_angle -
             superstructure_goal_.intake) < kProfileError &&
         ::std::abs(
             control_loops::superstructure_queue.status->shoulder.goal_angle -
             superstructure_goal_.shoulder) < kProfileError &&
         ::std::abs(
             control_loops::superstructure_queue.status->wrist.goal_angle -
             superstructure_goal_.wrist) < kProfileError &&
         ::std::abs(control_loops::superstructure_queue.status->intake
                        .goal_angular_velocity) < kProfileError &&
         ::std::abs(control_loops::superstructure_queue.status->shoulder
                        .goal_angular_velocity) < kProfileError &&
         ::std::abs(control_loops::superstructure_queue.status->wrist
                        .goal_angular_velocity) < kProfileError;
}

bool AutonomousActor::SuperstructureDone() {
  control_loops::superstructure_queue.status.FetchAnother();

  constexpr double kEpsilon = 0.03;

  if (control_loops::superstructure_queue.status->state < 12 ||
      control_loops::superstructure_queue.status->state == 16) {
    LOG(ERROR, "Superstructure no longer running, aborting action\n");
    return true;
  }

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

void AutonomousActor::WaitForIntake() {
  while (true) {
    if (ShouldCancel()) return;
    if (IntakeDone()) return;
  }
}

void AutonomousActor::WaitForSuperstructure() {
  while (true) {
    if (ShouldCancel()) return;
    if (SuperstructureDone()) return;
  }
}

void AutonomousActor::WaitForSuperstructureLow() {
  while (true) {
    if (ShouldCancel()) return;
    control_loops::superstructure_queue.status.FetchAnother();

    if (control_loops::superstructure_queue.status->state < 12 ||
        control_loops::superstructure_queue.status->state == 16) {
      LOG(ERROR, "Superstructure no longer running, aborting action\n");
      return;
    }
    if (SuperstructureProfileDone()) return;
    if (control_loops::superstructure_queue.status->shoulder.angle < 0.1) {
      return;
    }
  }
}
void AutonomousActor::BackLongShotLowBarTwoBall() {
  LOG(INFO, "Expanding for back long shot\n");
  MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625, {7.0, 40.0}, {4.0, 6.0},
                     {10.0, 25.0}, false, 0.0);
}

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

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

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

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

void AutonomousActor::FrontMiddleShot() {
  LOG(INFO, "Expanding for front middle shot\n");
  MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.42, {7.0, 40.0},
                     {4.0, 10.0}, {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, {7.0, 40.0},
                     {4.0, 10.0}, {10.0, 25.0}, !traverse_down, 0.0);
}

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

  ::aos::time::SleepFor(::aos::time::Time::InSeconds(1.5));

  if (ShouldCancel()) return;
  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.
  LOG(INFO, "Waiting for vision to be aligned\n");
  WaitForAlignedWithVision(aos::time::Time::InSeconds(3));
  if (ShouldCancel()) return;
  LOG(INFO, "Waiting for shooter to be up to speed\n");
  WaitForShooterSpeed();
  if (ShouldCancel()) return;

  ::aos::time::SleepFor(::aos::time::Time::InSeconds(1.0));
  LOG(INFO, "Shoot!\n");
  Shoot();

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

  // Wait for everything to be folded up.
  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_queue.status.FetchLatest();
  if (drivetrain_queue.status.get()) {
    const double left_error =
        (initial_drivetrain_.left -
         drivetrain_queue.status->estimated_left_position);
    const double right_error =
        (initial_drivetrain_.right -
         drivetrain_queue.status->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;
    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() {
  ::y2016::sensors::ball_detector.FetchLatest();
  if (::y2016::sensors::ball_detector.get()) {
    const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
    if (ball_detected) {
      CloseShooter();
    }
  }
}

void AutonomousActor::WaitForBall() {
  while (true) {
    ::y2016::sensors::ball_detector.FetchAnother();
    if (::y2016::sensors::ball_detector.get()) {
      const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
      if (ball_detected) {
        return;
      }
      if (ShouldCancel()) return;
    }
  }
}

void AutonomousActor::TwoBallAuto() {
  aos::time::Time start_time = aos::time::Time::Now();
  OpenShooter();
  MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
                     false, 12.0);
  if (ShouldCancel()) return;
  LOG(INFO, "Waiting for the intake to come down.\n");

  WaitForIntake();
  LOG(INFO, "Intake done at %f seconds, starting to drive\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  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, 0.0)) return;

  MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
                     false, 0.0);
  LOG(INFO, "Shutting off rollers at %f seconds, starting to straighten out\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
  MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
                     false, 0.0);
  CloseShooter();
  if (!WaitForDriveNear(kDriveDistance - 2.4, 0.0)) return;

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

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

  if (!WaitForDriveDone()) return;
  LOG(INFO, "First shot done driving at %f seconds\n",
      (aos::time::Time::Now() - start_time).ToSeconds());

  WaitForSuperstructure();

  if (ShouldCancel()) return;
  ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
  AlignWithVisionGoal();

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

  WaitForAlignedWithVision(aos::time::Time::InSeconds(0.2));
  LOG(INFO, "Shoot!\n");
  Shoot();

  LOG(INFO, "First shot at %f seconds\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  if (ShouldCancel()) return;

  SetShooterSpeed(0.0);
  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, 0.0)) return;
  StartDrive(0, -kShootTurnAngle, kTwoBallReturnDrive, kSwerveTurn);

  if (!WaitForDriveNear(0.06, 0.0)) return;
  LOG(INFO, "At Low Bar %f\n",
      (aos::time::Time::Now() - start_time).ToSeconds());

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

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

  if (!WaitForDriveProfileDone()) return;

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

  LOG(INFO, "Done backing up %f\n",
      (aos::time::Time::Now() - start_time).ToSeconds());

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

  StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
  LOG(INFO, "Straightening up at %f\n",
      (aos::time::Time::Now() - start_time).ToSeconds());

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

  ::y2016::sensors::ball_detector.FetchLatest();
  if (::y2016::sensors::ball_detector.get()) {
    const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
    if (!ball_detected) {
      if (!WaitForDriveDone()) return;
      LOG(INFO, "Aborting, no ball %f\n",
          (aos::time::Time::Now() - start_time).ToSeconds());
      return;
    }
  }
  CloseShooter();

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

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

  if (!WaitForDriveDone()) return;
  LOG(INFO, "Second shot done driving at %f seconds\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  WaitForSuperstructure();
  ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
  AlignWithVisionGoal();
  if (ShouldCancel()) return;

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

  // 2.2 with 0.4 of vision.
  // 1.8 without any vision.
  LOG(INFO, "Going to vision align at %f\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  WaitForAlignedWithVision(start_time + aos::time::Time::InSeconds(13.1) -
                           aos::time::Time::Now());
  LOG(INFO, "Shoot at %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
  Shoot();

  LOG(INFO, "Second shot at %f seconds\n",
      (aos::time::Time::Now() - start_time).ToSeconds());
  if (ShouldCancel()) return;

  SetShooterSpeed(0.0);
  LOG(INFO, "Folding superstructure back down\n");
  TuckArm(true, false);
  LOG(INFO, "Shot %f\n", (aos::time::Time::Now() - start_time).ToSeconds());

  WaitForSuperstructureLow();

  LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
}

bool AutonomousActor::RunAction(const actors::AutonomousActionParams &params) {
  aos::time::Time start_time = aos::time::Time::Now();
  LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);

  InitializeEncoders();
  ResetDrivetrain();

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

      // Spin up the shooter wheels.
      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.
      LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      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.
      LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      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.
      LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

      // Spin up the shooter wheels.
      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.
      LOG(INFO, "Unfolding superstructure\n");
      FrontMiddleShot();

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

      break;
    case 5:
      TwoBallAuto();
      return true;
      break;
    default:
      LOG(ERROR, "Invalid auto mode %d\n", params.mode);
      return true;
  }

  DoFullShot();

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

  LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());

  ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
                                      ::aos::time::Time::InMS(5) / 2);

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

  return true;
}

::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
    const ::y2016::actors::AutonomousActionParams &params) {
  return ::std::unique_ptr<AutonomousAction>(
      new AutonomousAction(&::y2016::actors::autonomous_action, params));
}

}  // namespace actors
}  // namespace y2016