blob: c2fddccd8542e179e7934c52660dde87a7815b7f [file] [log] [blame]
#include "y2020/actors/autonomous_actor.h"
#include <inttypes.h>
#include <chrono>
#include <cmath>
#include "aos/logging/logging.h"
#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "y2020/actors/auto_splines.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, true, "If true, define a spline autonomous mode");
DEFINE_bool(galactic_search, false,
"If true, do the galactic search autonomous");
namespace y2020 {
namespace actors {
using ::aos::monotonic_clock;
using ::frc971::ProfileParametersT;
using frc971::control_loops::drivetrain::LocalizerControl;
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>(
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
path_fetcher_(event_loop->MakeFetcher<y2020::vision::GalacticSearchPath>(
"/pi1/camera")),
auto_splines_() {
set_max_drivetrain_voltage(2.0);
}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
joystick_state_fetcher_.Fetch();
CHECK(joystick_state_fetcher_.get() != nullptr)
<< "Expect at least one JoystickState message before running auto...";
alliance_ = joystick_state_fetcher_->alliance();
}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
if (FLAGS_galactic_search) {
GalacticSearch();
} else if (FLAGS_spline_auto) {
SplineAuto();
} else {
return DriveFwd();
}
return true;
}
void AutonomousActor::SendStartingPosition(double x, double y, double theta) {
// Set up the starting position for the blue alliance.
double starting_heading = theta;
// TODO(james): Resetting the localizer breaks the left/right statespace
// controller. That is a bug, but we can fix that later by not resetting.
auto builder = localizer_control_sender_.MakeBuilder();
LocalizerControl::Builder localizer_control_builder =
builder.MakeBuilder<LocalizerControl>();
localizer_control_builder.add_x(x);
localizer_control_builder.add_y(y);
localizer_control_builder.add_theta(starting_heading);
localizer_control_builder.add_theta_uncertainty(0.00001);
if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
void AutonomousActor::GalacticSearch() {
path_fetcher_.Fetch();
CHECK(path_fetcher_.get() != nullptr)
<< "Expect at least one GalacticSearchPath message before running "
"auto...";
if (path_fetcher_->alliance() == y2020::vision::Alliance::kUnknown) {
AOS_LOG(ERROR, "The galactic search path is unknown, doing nothing.");
} else {
SplineHandle spline1 = PlanSpline(
[this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
if (path_fetcher_->alliance() == y2020::vision::Alliance::kRed) {
if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
target_spline = auto_splines_.SplineRedA(builder);
} else {
CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
target_spline = auto_splines_.SplineRedB(builder);
}
} else {
if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
target_spline = auto_splines_.SplineBlueA(builder);
} else {
CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
target_spline = auto_splines_.SplineBlueB(builder);
}
}
const frc971::MultiSpline *const spline =
flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
SendStartingPosition(CHECK_NOTNULL(spline));
return target_spline;
},
SplineDirection::kForward);
if (!spline1.WaitForPlan()) return;
spline1.Start();
if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
}
}
void AutonomousActor::SplineAuto() {
SplineHandle spline1 = PlanSpline(
[this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
target_spline = auto_splines_.TestSpline(builder);
const frc971::MultiSpline *const spline =
flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
SendStartingPosition(CHECK_NOTNULL(spline));
return target_spline;
},
SplineDirection::kForward);
if (!spline1.WaitForPlan()) return;
spline1.Start();
if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
}
void AutonomousActor::SendStartingPosition(
const frc971::MultiSpline *const spline) {
float x = spline->spline_x()->Get(0);
float y = spline->spline_y()->Get(0);
Eigen::Matrix<double, 2, 6> control_points;
for (size_t ii = 0; ii < 6; ++ii) {
control_points(0, ii) = spline->spline_x()->Get(ii);
control_points(1, ii) = spline->spline_y()->Get(ii);
}
frc971::control_loops::drivetrain::Spline spline_object(control_points);
SendStartingPosition(x, y, spline_object.Theta(0));
}
ProfileParametersT MakeProfileParametersT(const float max_velocity,
const float max_acceleration) {
ProfileParametersT params;
params.max_velocity = max_velocity;
params.max_acceleration = max_acceleration;
return params;
}
bool AutonomousActor::DriveFwd() {
SendStartingPosition(0, 0, 0);
const ProfileParametersT kDrive = MakeProfileParametersT(0.3f, 1.0f);
const ProfileParametersT kTurn = MakeProfileParametersT(5.0f, 15.0f);
StartDrive(1.0, 0.0, kDrive, kTurn);
return WaitForDriveDone();
}
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset;
{
StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder =
builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
frc971::ProfileParameters::Builder profile_params_builder =
builder.MakeBuilder<frc971::ProfileParameters>();
profile_params_builder.add_max_velocity(0.0);
profile_params_builder.add_max_acceleration(0.0);
flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
profile_params_builder.Finish();
intake_builder.add_unsafe_goal(intake_goal_);
intake_builder.add_profile_params(profile_params_offset);
intake_offset = intake_builder.Finish();
}
superstructure::Goal::Builder superstructure_builder =
builder.MakeBuilder<superstructure::Goal>();
superstructure_builder.add_intake(intake_offset);
superstructure_builder.add_roller_voltage(roller_voltage_);
superstructure_builder.add_roller_speed_compensation(kRollerSpeedCompensation);
if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
} // namespace actors
} // namespace y2020