Put reused auto code in a different class
To keep old years using the same API I opted to make a new class which
new autonomous_actors can use.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I991db4f65491994a718c5c8714f051fa071a128e
diff --git a/y2023/autonomous/BUILD b/y2023/autonomous/BUILD
index cfcc19c..3f2066f 100644
--- a/y2023/autonomous/BUILD
+++ b/y2023/autonomous/BUILD
@@ -48,7 +48,7 @@
"//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
- "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/autonomous:user_button_localized_autonomous_actor",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 8891223..e452d21 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -39,15 +39,12 @@
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
- : frc971::autonomous::BaseAutonomousActor(
+ : frc971::autonomous::UserButtonLocalizedAutonomousActor(
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")),
- robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
auto_splines_(),
arm_goal_position_(control_loops::superstructure::arm::StartingIndex()),
superstructure_goal_sender_(
@@ -57,57 +54,9 @@
event_loop
->MakeFetcher<::y2023::control_loops::superstructure::Status>(
"/superstructure")),
- points_(control_loops::superstructure::arm::PointList()) {
- drivetrain_status_fetcher_.Fetch();
- replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
-
- event_loop->OnRun([this, event_loop]() {
- replan_timer_->Schedule(event_loop->monotonic_now());
- button_poll_->Schedule(event_loop->monotonic_now(),
- chrono::milliseconds(50));
- });
-
- // TODO(james): Really need to refactor this code since we keep using it.
- button_poll_ = event_loop->AddTimer([this]() {
- const aos::monotonic_clock::time_point now =
- this->event_loop()->context().monotonic_event_time;
- if (robot_state_fetcher_.Fetch()) {
- if (robot_state_fetcher_->user_button()) {
- user_indicated_safe_to_reset_ = true;
- MaybeSendStartingPosition();
- }
- }
- if (joystick_state_fetcher_.Fetch()) {
- if (joystick_state_fetcher_->has_alliance() &&
- (joystick_state_fetcher_->alliance() != alliance_)) {
- alliance_ = joystick_state_fetcher_->alliance();
- is_planned_ = false;
- // Only kick the planning out by 2 seconds. If we end up enabled in
- // that second, then we will kick it out further based on the code
- // below.
- replan_timer_->Schedule(now + std::chrono::seconds(2));
- }
- if (joystick_state_fetcher_->enabled()) {
- if (!is_planned_) {
- // Only replan once we've been disabled for 5 seconds.
- replan_timer_->Schedule(now + std::chrono::seconds(5));
- }
- }
- }
- });
-}
+ points_(control_loops::superstructure::arm::PointList()) {}
void AutonomousActor::Replan() {
- if (!drivetrain_status_fetcher_.Fetch()) {
- replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
- AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
- return;
- }
-
- if (alliance_ == aos::Alliance::kInvalid) {
- return;
- }
- sent_starting_position_ = false;
if (FLAGS_spline_auto) {
test_spline_ =
PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
@@ -158,23 +107,7 @@
MaybeSendStartingPosition();
}
-void AutonomousActor::MaybeSendStartingPosition() {
- if (is_planned_ && user_indicated_safe_to_reset_ &&
- !sent_starting_position_) {
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
-}
-
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();
-
wrist_goal_ = 0.0;
roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
arm_goal_position_ = control_loops::superstructure::arm::StartingIndex();
@@ -182,23 +115,8 @@
SendSuperstructureGoal();
}
-bool AutonomousActor::RunAction(
+bool AutonomousActor::Run(
const ::frc971::autonomous::AutonomousActionParams *params) {
- Reset();
-
- AOS_LOG(INFO, "Params are %d\n", params->mode());
-
- if (!user_indicated_safe_to_reset_) {
- AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
- // Clear this so that we don't accidentally resend things as soon as we
- // replan later.
- user_indicated_safe_to_reset_ = false;
- is_planned_ = false;
- starting_position_.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.");
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 31ae4f5..5a5cd06 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -3,7 +3,7 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -14,13 +14,11 @@
namespace y2023 {
namespace autonomous {
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor
+ : public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
- bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) override;
-
private:
void set_arm_goal_position(uint32_t requested_arm_goal_position) {
arm_goal_position_ = requested_arm_goal_position;
@@ -48,39 +46,27 @@
void IntakeCube();
void Balance();
+ bool Run(const ::frc971::autonomous::AutonomousActionParams *params) override;
+ void Replan() override;
+ void SendStartingPosition(const Eigen::Vector3d &start) override;
+ void Reset() override;
+
[[nodiscard]] bool WaitForArmGoal(double distance_to_go = 0.01);
[[nodiscard]] bool WaitForPreloaded();
- void Reset();
-
- void SendStartingPosition(const Eigen::Vector3d &start);
- void MaybeSendStartingPosition();
void SplineAuto();
void ChargedUp();
void ChargedUpCableSide();
- void Replan();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- aos::Fetcher<aos::RobotState> robot_state_fetcher_;
double wrist_goal_;
control_loops::superstructure::RollerGoal roller_goal_ =
control_loops::superstructure::RollerGoal::IDLE;
- aos::TimerHandler *replan_timer_;
- aos::TimerHandler *button_poll_;
-
- aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
- bool user_indicated_safe_to_reset_ = false;
- bool sent_starting_position_ = false;
-
- bool is_planned_ = false;
-
- std::optional<Eigen::Vector3d> starting_position_;
uint32_t arm_goal_position_;
bool preloaded_ = false;