bot3/autonomous/auto.cc

#include <stdio.h>

#include <memory>

#include "aos/common/util/phased_loop.h"
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"

#include "bot3/autonomous/auto.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.h"
#include "bot3/control_loops/elevator/elevator.q.h"
#include "bot3/control_loops/intake/intake.q.h"

using ::aos::time::Time;
using ::bot3::control_loops::drivetrain_queue;
using ::bot3::control_loops::intake_queue;
using ::bot3::control_loops::elevator_queue;

namespace bot3 {
namespace autonomous {

struct ProfileParams {
  double velocity;
  double acceleration;
};

namespace time = ::aos::time;

static double left_initial_position, right_initial_position;

bool ShouldExitAuto() {
  ::bot3::autonomous::autonomous.FetchLatest();
  bool ans = !::bot3::autonomous::autonomous->run_auto;
  if (ans) {
    LOG(INFO, "Time to exit auto mode\n");
  }
  return ans;
}

void ResetDrivetrain() {
  LOG(INFO, "resetting the drivetrain\n");
  control_loops::drivetrain_queue.goal.MakeWithBuilder()
      .control_loop_driving(false)
      .steering(0.0)
      .throttle(0.0)
      .left_goal(left_initial_position)
      .left_velocity_goal(0)
      .right_goal(right_initial_position)
      .right_velocity_goal(0)
      .Send();
}

void InitializeEncoders() {
  control_loops::drivetrain_queue.status.FetchAnother();
  left_initial_position =
      control_loops::drivetrain_queue.status->filtered_left_position;
  right_initial_position =
      control_loops::drivetrain_queue.status->filtered_right_position;
}

void GrabberForTime(double voltage, double wait_time) {
  ::aos::time::Time now = ::aos::time::Time::Now();
  ::aos::time::Time end_time = now + time::Time::InSeconds(wait_time);
  LOG(INFO, "Starting to grab at %f for %f seconds\n", voltage, wait_time);

  while (true) {
    autonomous::can_grabber_control.MakeWithBuilder()
        .can_grabber_voltage(voltage).can_grabbers(false).Send();

    // Poll the running bit and auto done bits.
    if (ShouldExitAuto()) {
      return;
    }
    if (::aos::time::Time::Now() > end_time) {
      LOG(INFO, "Done grabbing\n");
      return;
    }
    ::aos::time::PhasedLoopXMS(5, 2500);
  }
}

// Auto methods.
void CanGrabberAuto() {
  ResetDrivetrain();

  // Launch can grabbers.
  //GrabberForTime(12.0, 0.26);
  GrabberForTime(6.0, 0.40);
  if (ShouldExitAuto()) return;
  InitializeEncoders();
  ResetDrivetrain();
  if (ShouldExitAuto()) return;
  // Send our intake goals.
  if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(true)
          .Send()) {
    LOG(ERROR, "Sending intake goal failed.\n");
  }
  {
    auto new_elevator_goal = elevator_queue.goal.MakeMessage();
    new_elevator_goal->max_velocity = 0.0;
    new_elevator_goal->max_acceleration = 0.0;
    new_elevator_goal->height = 0.030;
    new_elevator_goal->velocity = 0.0;
    new_elevator_goal->passive_support = true;
    new_elevator_goal->can_support = false;

    if (new_elevator_goal.Send()) {
      LOG(DEBUG, "sending goals: elevator: %f\n", 0.03);
    } else {
      LOG(ERROR, "Sending elevator goal failed.\n");
    }
  }

  const double kMoveBackDistance = 1.75;
  //const double kMoveBackDistance = 0.0;

  // Drive backwards, and pulse the can grabbers again to tip the cans.
  control_loops::drivetrain_queue.goal.MakeWithBuilder()
      .control_loop_driving(true)
      .steering(0.0)
      .throttle(0.0)
      .left_goal(left_initial_position + kMoveBackDistance)
      .left_velocity_goal(0)
      .right_goal(right_initial_position + kMoveBackDistance)
      .right_velocity_goal(0)
      .Send();
  GrabberForTime(12.0, 0.02);
  if (ShouldExitAuto()) return;

  // We shouldn't need as much power at this point, so lower the can grabber
  // voltages to avoid damaging the motors due to stalling.
  GrabberForTime(4.0, 0.75);
  if (ShouldExitAuto()) return;
  GrabberForTime(-3.0, 0.25);
  if (ShouldExitAuto()) return;
  GrabberForTime(-12.0, 1.0);
  if (ShouldExitAuto()) return;
  GrabberForTime(-3.0, 12.0);
}

void HandleAuto() {
  ::aos::time::Time start_time = ::aos::time::Time::Now();
  LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());

  // TODO(comran): Add various options for different autos down below.
  CanGrabberAuto();
}

}  // namespace autonomous
}  // namespace bot3