Splines can run in 2023 autonomous.
Mostly copying boilerplate from 2022, with 2023-specific additions like
handling field mirroring correctly.
Change-Id: Ic8cdfde4982a3637ce7c0655800954e13ad60cf9
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index 07250e4..0cc98a2 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -1,26 +1,37 @@
#include "y2023/autonomous/auto_splines.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "aos/flatbuffer_merge.h"
namespace y2023 {
namespace actors {
-void MaybeFlipSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
- bool is_left) {
- flatbuffers::Vector<float> *spline_y =
- GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+namespace {
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
- if (!is_left) {
- for (size_t i = 0; i < spline_y->size(); i++) {
- spline_y->Mutate(i, -spline_y->Get(i));
+ // For 2023: The field is mirrored across the center line, and is not
+ // rotationally symmetric. As such, we only flip the X coordinates when
+ // changing side of the field.
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
}
}
+ return spline_offset;
}
+} // namespace
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -78,11 +89,13 @@
multispline_builder.add_spline_x(spline_x_offset);
multispline_builder.add_spline_y(spline_y_offset);
- return multispline_builder.Finish();
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder->fbb()->CreateVector<float>(
{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
@@ -96,7 +109,17 @@
multispline_builder.add_spline_x(spline_x_offset);
multispline_builder.add_spline_y(spline_y_offset);
- return multispline_builder.Finish();
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
}
} // namespace actors
diff --git a/y2023/autonomous/auto_splines.h b/y2023/autonomous/auto_splines.h
index 68795e6..1280693 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
/*
@@ -16,10 +17,24 @@
class AutonomousSplines {
public:
+ AutonomousSplines()
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ private:
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
};
} // namespace actors
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index a815b25..8e99af6 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -8,6 +8,8 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+DEFINE_bool(spline_auto, true, "Run simple test S-spline auto mode.");
+
namespace y2023 {
namespace actors {
@@ -18,11 +20,86 @@
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ 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_() {
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Setup(event_loop->monotonic_now());
+ button_poll_->Setup(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_->Setup(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_->Setup(now + std::chrono::seconds(5));
+ }
+ }
+ }
+ });
+}
+
+void AutonomousActor::Replan() {
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+ sent_starting_position_ = false;
+ if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
+
+ starting_position_ = test_spline_->starting_position();
+ }
+
+ is_planned_ = true;
+
+ 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();
}
bool AutonomousActor::RunAction(
@@ -30,8 +107,58 @@
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.");
+ return false;
+ }
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ } else {
+ AOS_LOG(WARNING, "No auto mode selected.");
+ }
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.
+
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(start(0));
+ localizer_control_builder.add_y(start(1));
+ localizer_control_builder.add_theta(start(2));
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ AOS_LOG(INFO, "User button pressed, x: %f y: %f theta: %f", start(0),
+ start(1), start(2));
+ if (builder.Send(localizer_control_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+
} // namespace actors
} // namespace y2023
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 6eb8f90..cf0b458 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -6,6 +6,8 @@
#include "frc971/autonomous/base_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 "y2023/autonomous/auto_splines.h"
namespace y2023 {
namespace actors {
@@ -19,6 +21,29 @@
private:
void Reset();
+
+ void SendStartingPosition(const Eigen::Vector3d &start);
+ void MaybeSendStartingPosition();
+ void SplineAuto();
+ 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_;
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ std::optional<SplineHandle> test_spline_;
+ 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_;
};
} // namespace actors
diff --git a/y2023/autonomous/splines/test_spline.json b/y2023/autonomous/splines/test_spline.json
new file mode 100644
index 0000000..7672596
--- /dev/null
+++ b/y2023/autonomous/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [6.22420997455908, 6.1347950111487386, 6.080329974810555, 6.023577036950107, 5.9617203084135255, 5.81469341092744], "spline_y": [-2.63127733767268, -2.63127733767268, -2.656484781970896, -2.656484781970896, -2.6668098529078925, -2.6448802602350456], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 2}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 4}]}