| #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 |