Add y2024 Autonomous Actor Functions
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I6f5392055e3cde1e86a53edca7ae8c3d0b2a7cc3
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
index 611f3cd..f69d9fc 100644
--- a/y2024/autonomous/BUILD
+++ b/y2024/autonomous/BUILD
@@ -49,6 +49,7 @@
"//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/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 6b1d9a5..3c7f2fa 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -34,74 +34,24 @@
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
- : frc971::autonomous::BaseAutonomousActor(
+ : frc971::autonomous::UserButtonLocalizedAutonomousActor(
event_loop,
control_loops::drivetrain::GetDrivetrainConfig(event_loop)),
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_(),
superstructure_goal_sender_(
- event_loop->MakeSender<::y2024::control_loops::superstructure::Goal>(
- "/superstructure")),
+ event_loop
+ ->MakeSender<::y2024::control_loops::superstructure::GoalStatic>(
+ "/superstructure")),
superstructure_status_fetcher_(
event_loop
->MakeFetcher<::y2024::control_loops::superstructure::Status>(
- "/superstructure")) {
- 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));
- }
- }
- }
- });
-}
+ "/superstructure")),
+ auto_splines_() {}
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_,
@@ -116,42 +66,8 @@
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();
- preloaded_ = false;
-}
-
-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.");
@@ -165,15 +81,6 @@
return true;
}
-void AutonomousActor::SplineAuto() {
- CHECK(test_spline_);
-
- if (!test_spline_->WaitForPlan()) return;
- test_spline_->Start();
-
- if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
-}
-
void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
// Set up the starting position for the blue alliance.
@@ -192,4 +99,84 @@
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
+
+void AutonomousActor::Reset() {
+ set_intake_goal(control_loops::superstructure::IntakeGoal::NONE);
+ set_note_goal(control_loops::superstructure::NoteGoal::NONE);
+ set_auto_aim(false);
+ set_fire(false);
+ set_preloaded(false);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendSuperstructureGoal() {
+ aos::Sender<control_loops::superstructure::GoalStatic>::StaticBuilder
+ goal_builder = superstructure_goal_sender_.MakeStaticBuilder();
+
+ goal_builder->set_intake_goal(intake_goal_);
+ goal_builder->set_note_goal(note_goal_);
+ goal_builder->set_fire(fire_);
+
+ control_loops::superstructure::ShooterGoalStatic *shooter_goal =
+ goal_builder->add_shooter_goal();
+
+ shooter_goal->set_auto_aim(auto_aim_);
+ shooter_goal->set_preloaded(preloaded_);
+
+ goal_builder.CheckOk(goal_builder.Send());
+}
+
+void AutonomousActor::Intake() {
+ set_intake_goal(control_loops::superstructure::IntakeGoal::INTAKE);
+ set_note_goal(control_loops::superstructure::NoteGoal::CATAPULT);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Aim() {
+ set_auto_aim(true);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Shoot() {
+ set_fire(true);
+ SendSuperstructureGoal();
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
+ set_preloaded(true);
+ SendSuperstructureGoal();
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->shooter()->catapult_state() ==
+ control_loops::superstructure::CatapultState::LOADED);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index c3d7d79..6796c94 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -3,62 +3,70 @@
#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"
#include "y2024/autonomous/auto_splines.h"
-#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_static.h"
+#include "y2024/control_loops/superstructure/superstructure_status_static.h"
namespace y2024::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_intake_goal(control_loops::superstructure::IntakeGoal intake_goal) {
+ intake_goal_ = intake_goal;
+ }
+ void set_note_goal(control_loops::superstructure::NoteGoal note_goal) {
+ note_goal_ = note_goal;
+ }
+ void set_auto_aim(bool auto_aim) { auto_aim_ = auto_aim; }
+ void set_fire(bool fire) { fire_ = fire; }
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
+
+ bool Run(const ::frc971::autonomous::AutonomousActionParams *params) override;
+ void Replan() override;
+ void SendStartingPosition(const Eigen::Vector3d &start) override;
+ void Reset() override;
+
+ void SplineAuto();
void SendSuperstructureGoal();
- void Reset();
+ void Intake();
+ void Aim();
+ void Shoot();
- void SendStartingPosition(const Eigen::Vector3d &start);
- void MaybeSendStartingPosition();
- void SplineAuto();
- void Replan();
+ [[nodiscard]] bool WaitForPreloaded();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- aos::Fetcher<aos::RobotState> robot_state_fetcher_;
- aos::TimerHandler *replan_timer_;
- aos::TimerHandler *button_poll_;
+ aos::Sender<control_loops::superstructure::GoalStatic>
+ superstructure_goal_sender_;
- 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_;
-
- bool preloaded_ = false;
-
- aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
aos::Fetcher<y2024::control_loops::superstructure::Status>
superstructure_status_fetcher_;
+ AutonomousSplines auto_splines_;
+
std::optional<SplineHandle> test_spline_;
- // List of arm angles from arm::PointsList
- const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+ control_loops::superstructure::IntakeGoal intake_goal_ =
+ control_loops::superstructure::IntakeGoal::NONE;
+
+ control_loops::superstructure::NoteGoal note_goal_ =
+ control_loops::superstructure::NoteGoal::NONE;
+
+ bool auto_aim_ = false;
+ bool fire_ = false;
+ bool preloaded_ = false;
};
} // namespace y2024::autonomous
-#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
\ No newline at end of file