y2019/actors/autonomous_actor.cc

#include "y2019/actors/autonomous_actor.h"

#include <inttypes.h>

#include <chrono>
#include <cmath>

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

#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "y2019/actors/auto_splines.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"

namespace y2019 {
namespace actors {
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;

AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
    : frc971::autonomous::BaseAutonomousActor(
          event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
      localizer_control_sender_(
          event_loop->MakeSender<
              ::frc971::control_loops::drivetrain::LocalizerControl>(
              ".frc971.control_loops.drivetrain.localizer_control")),
      superstructure_goal_sender_(
          event_loop->MakeSender<::y2019::control_loops::superstructure::
                                     SuperstructureQueue::Goal>(
              ".y2019.control_loops.superstructure.superstructure_queue.goal")),
      superstructure_status_fetcher_(event_loop->MakeFetcher<
                                     ::y2019::control_loops::superstructure::
                                         SuperstructureQueue::Status>(
          ".y2019.control_loops.superstructure.superstructure_queue.status")) {}

bool AutonomousActor::WaitForDriveXGreater(double x) {
  LOG(INFO, "Waiting until x > %f\n", x);
  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (drivetrain_status_fetcher_->x > x) {
      LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
      return true;
    }
  }
}

bool AutonomousActor::WaitForDriveYCloseToZero(double y) {
  LOG(INFO, "Waiting until |y| < %f\n", y);
  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                      event_loop()->monotonic_now(),
                                      ::std::chrono::milliseconds(5) / 2);

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (::std::abs(drivetrain_status_fetcher_->y) < y) {
      LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
      return true;
    }
  }
}

void AutonomousActor::Reset(bool is_left) {
  const double turn_scalar = is_left ? 1.0 : -1.0;
  elevator_goal_ = 0.01;
  wrist_goal_ = -M_PI / 2.0;
  intake_goal_ = -1.2;

  suction_on_ = false;
  suction_gamepiece_ = 1;

  elevator_max_velocity_ = 0.0;
  elevator_max_acceleration_ = 0.0;
  wrist_max_velocity_ = 0.0;
  wrist_max_acceleration_ = 0.0;

  InitializeEncoders();
  SendSuperstructureGoal();

  {
    auto localizer_resetter = localizer_control_sender_.MakeMessage();
    // Start on the left l2.
    localizer_resetter->x = 1.0;
    localizer_resetter->y = 1.35 * turn_scalar;
    localizer_resetter->theta = M_PI;
    localizer_resetter->theta_uncertainty = 0.00001;
    if (!localizer_resetter.Send()) {
      LOG(ERROR, "Failed to reset localizer.\n");
    }
  }

  // Wait for the drivetrain to run so it has time to reset the heading.
  // Otherwise our drivetrain reset will do a 180 right at the start.
  WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
  LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);
  InitializeEncoders();
  ResetDrivetrain();
  WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
  LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);

  ResetDrivetrain();
  InitializeEncoders();
}

const ProfileParameters kJumpDrive = {2.0, 3.0};
const ProfileParameters kDrive = {4.0, 3.0};
const ProfileParameters kTurn = {5.0, 15.0};

const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};

const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};

const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};

const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};

bool AutonomousActor::RunAction(
    const ::frc971::autonomous::AutonomousActionParams &params) {
  const monotonic_clock::time_point start_time = monotonic_now();
  const bool is_left = params.mode == 0;

  {
    LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
        params.mode, is_left ? "left" : "right");
  }

  const double turn_scalar = is_left ? 1.0 : -1.0;

  Reset(is_left);
  enum class Mode { kTesting, kRocket, kCargoship };
  Mode mode = Mode::kCargoship;
  if (mode == Mode::kRocket) {
    SplineHandle spline1 =
        PlanSpline(AutonomousSplines::HabToFarRocketTest(is_left),
                   SplineDirection::kBackward);

    // Grab the disk, jump, wait until we have vacuum, then raise the elevator
    set_elevator_goal(0.010);
    set_wrist_goal(-M_PI / 2.0);
    set_intake_goal(-1.2);
    set_suction_goal(true, 1);
    SendSuperstructureGoal();

    // if planned start the spline and plan the next
    if (!spline1.WaitForPlan()) return true;
    LOG(INFO, "Planned\n");
    spline1.Start();

    // If suction, move the superstructure to score
    if (!WaitForGamePiece()) return true;
    LOG(INFO, "Has game piece\n");
    if (!spline1.WaitForSplineDistanceRemaining(3.5)) return true;
    set_elevator_wrist_goal(kPanelBackwardMiddlePos);
    SendSuperstructureGoal();

    if (!spline1.WaitForSplineDistanceRemaining(2.0)) return true;
    set_elevator_wrist_goal(kPanelForwardMiddlePos);
    SendSuperstructureGoal();

    // END SPLINE 1

    if (!spline1.WaitForSplineDistanceRemaining(0.2)) return true;
    LineFollowAtVelocity(1.3, 4);
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1200))) return true;

    set_suction_goal(false, 1);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
    LineFollowAtVelocity(-1.0, 4);
    SplineHandle spline2 = PlanSpline(AutonomousSplines::FarRocketToHP(is_left),
                                      SplineDirection::kBackward);

    if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
    if (!spline2.WaitForPlan()) return true;
    LOG(INFO, "Planned\n");
    // Drive back to hp and set the superstructure accordingly
    spline2.Start();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
    set_elevator_wrist_goal(kPanelHPIntakeBackwardPos);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
    set_suction_goal(true, 1);
    SendSuperstructureGoal();

    if (!spline2.WaitForSplineDistanceRemaining(1.6)) return true;
    LineFollowAtVelocity(-1.6);

    // As soon as we pick up Panel 2 go score on the back rocket
    if (!WaitForGamePiece()) return true;
    LineFollowAtVelocity(1.5);
    SplineHandle spline3 = PlanSpline(AutonomousSplines::HPToFarRocket(is_left),
                                      SplineDirection::kForward);
    if (!WaitForDriveXGreater(0.50)) return true;
    if (!spline3.WaitForPlan()) return true;
    spline3.Start();
    LOG(INFO, "Has game piece\n");
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
    set_elevator_wrist_goal(kPanelBackwardMiddlePos);
    SendSuperstructureGoal();
    if (!WaitForDriveXGreater(7.1)) return true;
    LineFollowAtVelocity(-1.5, 4);
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
    set_elevator_wrist_goal(kPanelBackwardUpperPos);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1500))) return true;
    set_suction_goal(false, 1);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
    LineFollowAtVelocity(1.0, 4);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
  } else if (mode == Mode::kCargoship) {
    SplineHandle spline1 =
        PlanSpline(AutonomousSplines::HABToSecondCargoShipBay(is_left),
                   SplineDirection::kBackward);
    set_elevator_goal(0.01);
    set_wrist_goal(-M_PI / 2.0);
    set_intake_goal(-1.2);
    set_suction_goal(true, 1);
    SendSuperstructureGoal();

    // if planned start the spline and plan the next
    if (!spline1.WaitForPlan()) return true;
    LOG(INFO, "Planned\n");
    spline1.Start();

    // If suction, move the superstructure to score
    if (!WaitForGamePiece()) return true;
    LOG(INFO, "Has game piece\n");
    // unstick the hatch panel
    if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
    set_elevator_goal(0.5);
    set_wrist_goal(-M_PI / 2.0);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
    set_elevator_wrist_goal(kPanelCargoBackwardPos);
    SendSuperstructureGoal();

    if (!spline1.WaitForSplineDistanceRemaining(0.8)) return true;
    // Line follow in to the first disc.
    LineFollowAtVelocity(-0.9, 2);
    if (!WaitForDriveYCloseToZero(1.2)) return true;

    set_suction_goal(false, 1);
    SendSuperstructureGoal();
    LOG(INFO, "Dropping disc 1 %f\n",
        ::aos::time::DurationInSeconds(monotonic_now() - start_time));

    if (!WaitForDriveYCloseToZero(1.13)) return true;
    if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;

    LineFollowAtVelocity(0.9, 2);
    SplineHandle spline2 =
        PlanSpline(AutonomousSplines::SecondCargoShipBayToHP(is_left),
                   SplineDirection::kForward);
    if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
    if (!spline2.WaitForPlan()) return true;
    LOG(INFO, "Planned\n");
    // Drive back to hp and set the superstructure accordingly
    spline2.Start();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
    set_elevator_wrist_goal(kPanelHPIntakeForwrdPos);
    SendSuperstructureGoal();
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
    set_suction_goal(true, 1);
    SendSuperstructureGoal();

    if (!spline2.WaitForSplineDistanceRemaining(1.75)) return true;
    LineFollowAtVelocity(1.5);
    // As soon as we pick up Panel 2 go score on the rocket
    if (!WaitForGamePiece()) return true;
    LOG(INFO, "Got gamepiece %f\n",
        ::aos::time::DurationInSeconds(monotonic_now() - start_time));
    LineFollowAtVelocity(-4.0);
    SplineHandle spline3 =
        PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
                   SplineDirection::kBackward);
    if (!WaitForDriveXGreater(0.55)) return true;
    if (!spline3.WaitForPlan()) return true;
    spline3.Start();
    // Wait until we are a bit out to lift.
    if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
    set_elevator_wrist_goal(kPanelCargoBackwardPos);
    SendSuperstructureGoal();

    if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
    // Line follow in to the second disc.
    LineFollowAtVelocity(-0.7, 3);
    LOG(INFO, "Drawing in disc 2 %f\n",
        ::aos::time::DurationInSeconds(monotonic_now() - start_time));
    if (!WaitForDriveYCloseToZero(1.2)) return true;

    set_suction_goal(false, 1);
    SendSuperstructureGoal();
    LOG(INFO, "Dropping disc 2 %f\n",
        ::aos::time::DurationInSeconds(monotonic_now() - start_time));

    if (!WaitForDriveYCloseToZero(1.13)) return true;
    if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
    LOG(INFO, "Backing up %f\n",
        ::aos::time::DurationInSeconds(monotonic_now() - start_time));
    LineFollowAtVelocity(0.9, 3);
    if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
  } else {
    // Grab the disk, wait until we have vacuum, then jump
    set_elevator_goal(0.01);
    set_wrist_goal(-M_PI / 2.0);
    set_intake_goal(-1.2);
    set_suction_goal(true, 1);
    SendSuperstructureGoal();

    if (!WaitForGamePiece()) return true;
    LOG(INFO, "Has game piece\n");

    StartDrive(-4.0, 0.0, kJumpDrive, kTurn);
    if (!WaitForDriveNear(3.3, 10.0)) return true;
    LOG(INFO, "Lifting\n");
    set_elevator_goal(0.30);
    SendSuperstructureGoal();

    if (!WaitForDriveNear(2.8, 10.0)) return true;
    LOG(INFO, "Off the platform\n");

    StartDrive(0.0, 1.00 * turn_scalar, kDrive, kTurn);
    LOG(INFO, "Turn started\n");
    if (!WaitForSuperstructureDone()) return true;
    LOG(INFO, "Superstructure done\n");

    if (!WaitForDriveNear(0.7, 10.0)) return true;
    StartDrive(0.0, -0.35 * turn_scalar, kDrive, kTurn);

    LOG(INFO, "Elevator up\n");
    set_elevator_goal(0.78);
    SendSuperstructureGoal();

    if (!WaitForDriveDone()) return true;
    LOG(INFO, "Done driving\n");

    if (!WaitForSuperstructureDone()) return true;
  }

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

  return true;
}

}  // namespace actors
}  // namespace y2019