Pre-plan auto splines
(a) Make it so that the drivetrain automatically evicts old splines
(b) Set up auto to preplan splines at construction and after every auto.
Change-Id: I96ddb3a38947da02ad9ddc6fe933b7e85727dc18
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index c9eb49e..c7f2d20 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -32,12 +32,14 @@
":auto_fbs",
"//aos/actions:action_lib",
"//aos/logging",
+ "//aos/util:math",
"//aos/util:phased_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain:spline",
"//frc971/control_loops/drivetrain:spline_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
],
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 1b1ce36..e691d03 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -5,12 +5,14 @@
#include <chrono>
#include <cmath>
+#include "aos/util/math.h"
#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/drivetrain/spline.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"
using ::aos::monotonic_clock;
@@ -474,9 +476,31 @@
SplineDirection::kBackward);
spline_builder.add_spline(multispline_offset);
+ // Calculate the starting position and yaw.
+ Eigen::Vector3d start;
+ {
+ const frc971::MultiSpline *const spline =
+ flatbuffers::GetTemporaryPointer(*builder.fbb(), multispline_offset);
+
+ start(0) = spline->spline_x()->Get(0);
+ start(1) = 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);
+ start(2) = spline_object.Theta(0);
+ if (direction == SplineDirection::kBackward) {
+ start(2) = aos::math::NormalizeAngle(start(2) + M_PI);
+ }
+ }
+
builder.Send(spline_builder.Finish());
- return BaseAutonomousActor::SplineHandle(spline_handle, this);
+ return BaseAutonomousActor::SplineHandle(spline_handle, this, start);
}
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 2478fda..92fc46c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -43,15 +43,23 @@
bool SplineDistanceRemaining(double distance);
bool WaitForSplineDistanceRemaining(double distance);
+ // Returns [x, y, theta] position of the start.
+ const Eigen::Vector3d &starting_position() const { return spline_start_; }
+
private:
friend BaseAutonomousActor;
SplineHandle(int32_t spline_handle,
- BaseAutonomousActor *base_autonomous_actor)
+ BaseAutonomousActor *base_autonomous_actor,
+ const Eigen::Vector3d &start)
: spline_handle_(spline_handle),
- base_autonomous_actor_(base_autonomous_actor) {}
+ base_autonomous_actor_(base_autonomous_actor),
+ spline_start_(start) {}
int32_t spline_handle_;
BaseAutonomousActor *base_autonomous_actor_;
+
+ // Starting [x, y, theta] position of the spline.
+ Eigen::Vector3d spline_start_;
};
// Represents the direction that we will drive along a spline.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1395e5d..91f1513 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -316,6 +316,27 @@
}
void DrivetrainLoop::UpdateTrajectoryFetchers() {
+ if (dt_spline_.trajectory_count() >= trajectory_fetchers_.size()) {
+ aos::monotonic_clock::time_point min_time = aos::monotonic_clock::max_time;
+ size_t min_fetcher_index = 0;
+ size_t fetcher_index = 0;
+ // Find the oldest spline to forget.
+ for (auto &fetcher : trajectory_fetchers_) {
+ CHECK_NE(fetcher.fetcher.context().monotonic_event_time,
+ monotonic_clock::min_time);
+ if (fetcher.fetcher.context().monotonic_event_time < min_time &&
+ !dt_spline_.IsCurrentTrajectory(fetcher.fetcher.get())) {
+ min_time = fetcher.fetcher.context().monotonic_event_time;
+ min_fetcher_index = fetcher_index;
+ }
+ ++fetcher_index;
+ }
+
+ dt_spline_.DeleteTrajectory(
+ trajectory_fetchers_[min_fetcher_index].fetcher.get());
+ trajectory_fetchers_[min_fetcher_index].in_use = false;
+ }
+
for (auto &fetcher : trajectory_fetchers_) {
const fb::Trajectory *trajectory = fetcher.fetcher.get();
// If the current fetcher is already being used by the SplineDrivetrain,
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 070cd03..325715f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -144,7 +144,9 @@
class DrivetrainLoop
: public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- static constexpr size_t kNumSplineFetchers = 4;
+ // Note that we only actually store N - 1 splines, since we need to keep one
+ // fetcher free to check whether there are any new splines.
+ static constexpr size_t kNumSplineFetchers = 5;
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index 86318ca..3438fe5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -11,8 +11,9 @@
{
"name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.fb.Trajectory",
- "max_size": 200000,
- "frequency": 10,
+ "max_size": 400000,
+ "frequency": 2,
+ "num_senders": 2,
"read_method": "PIN",
"num_readers": 6
},
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7dd5a52..26428ba 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -1399,14 +1399,25 @@
VerifyNearPosition(1.0, 1.0);
}
-// Tests that when we send a bunch of splines we can fill up the internal
-// buffers and that we handle that case sanely.
+// Tests that when we send a bunch of splines we properly evict old splines from
+// the internal buffers.
TEST_F(DrivetrainTest, FillSplineBuffer) {
SetEnabled(true);
- std::vector<int> sent_spline_indices;
- constexpr size_t kExtraSplines = 4;
- for (size_t spline_index = 1;
- spline_index < DrivetrainLoop::kNumSplineFetchers + 1 + kExtraSplines;
+ std::vector<int> expected_splines;
+ constexpr size_t kExtraSplines = 10;
+ constexpr size_t kNumStoredSplines = DrivetrainLoop::kNumSplineFetchers - 1;
+ constexpr int kRunSpline = 1;
+ {
+ // Tell the drivetrain to execute spline 1; we then will check that that
+ // spline never gets evicted.
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(kRunSpline);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+ for (size_t spline_index = 0;
+ spline_index < DrivetrainLoop::kNumSplineFetchers + kExtraSplines;
++spline_index) {
auto builder = trajectory_goal_sender_.MakeBuilder();
@@ -1430,65 +1441,34 @@
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
- RunFor(dt());
+ // Run for at least 2 iterations. Because of how the logic works, there will
+ // actually typically be a single iteration where we store kNumStoredSplines
+ // + 1.
+ RunFor(2 * dt());
- sent_spline_indices.push_back(spline_index);
- }
-
- drivetrain_status_fetcher_.Fetch();
-
- ASSERT_EQ(DrivetrainLoop::kNumSplineFetchers,
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->size());
- for (size_t ii = 0; ii < DrivetrainLoop::kNumSplineFetchers; ++ii) {
- EXPECT_EQ(sent_spline_indices[ii],
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->Get(ii));
- }
-
- // Next, start going through and executing all the splines; we should
- // gradually work through the splines.
- for (size_t ii = 0; ii < sent_spline_indices.size(); ++ii) {
- const int current_spline = sent_spline_indices[ii];
- {
- auto builder = drivetrain_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline_handle(current_spline);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ expected_splines.push_back(spline_index);
+ if (expected_splines.size() > kNumStoredSplines) {
+ if (expected_splines.front() != kRunSpline) {
+ expected_splines.erase(expected_splines.begin());
+ } else {
+ expected_splines.erase(expected_splines.begin() + 1);
+ }
}
- // Run for two iterations to give the drivetrain time to resolve all of its
- // internal state for handling splines (coordinating the fetchers and goal
- // message to get this two all happen in 1 message is entirely possible, but
- // has no practical need).
- RunFor(2 * dt());
+ // We should always just have the past kNumStoredSplines available.
drivetrain_status_fetcher_.Fetch();
- ASSERT_EQ(current_spline, drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->current_spline_idx())
- << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
-
- const int num_available_splines = std::min(
- sent_spline_indices.size() - ii, DrivetrainLoop::kNumSplineFetchers);
-
- ASSERT_EQ(num_available_splines,
+ ASSERT_EQ(expected_splines.size(),
CHECK_NOTNULL(drivetrain_status_fetcher_.get()
->trajectory_logging()
->available_splines())
- ->size())
- << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
- for (int jj = 0; jj < num_available_splines; ++jj) {
- EXPECT_EQ(sent_spline_indices[ii + jj],
+ ->size());
+ for (size_t ii = 0; ii < expected_splines.size(); ++ii) {
+ EXPECT_EQ(expected_splines[ii],
CHECK_NOTNULL(drivetrain_status_fetcher_.get()
->trajectory_logging()
->available_splines())
- ->Get(jj));
+ ->Get(ii));
}
}
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..1ebda6c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -41,6 +41,12 @@
UpdateSplineHandles();
}
+bool SplineDrivetrain::IsCurrentTrajectory(
+ const fb::Trajectory *trajectory) const {
+ return (current_trajectory_ != nullptr &&
+ current_trajectory().spline_handle() == trajectory->handle());
+}
+
bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
if (trajectory == nullptr) {
return false;
@@ -53,6 +59,19 @@
return false;
}
+void SplineDrivetrain::DeleteTrajectory(const fb::Trajectory *trajectory) {
+ CHECK(trajectory != nullptr);
+
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ trajectories_.erase(trajectories_.begin() + ii);
+ return;
+ }
+ }
+
+ LOG(FATAL) << "Trying to remove unknown trajectory " << trajectory->handle();
+}
+
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
trajectories_.emplace_back(
std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
@@ -60,11 +79,9 @@
}
void SplineDrivetrain::DeleteCurrentSpline() {
- CHECK(current_trajectory_index_);
- CHECK_LT(*current_trajectory_index_, trajectories_.size());
- trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+ DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
executing_spline_ = false;
- current_trajectory_index_.reset();
+ current_trajectory_ = nullptr;
current_xva_.setZero();
}
@@ -92,7 +109,7 @@
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
executing_spline_ = true;
- current_trajectory_index_ = ii;
+ current_trajectory_ = trajectories_[ii].get();
}
}
// If we didn't find the commanded spline in the list of available splines,
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 0170151..303701f 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -32,6 +32,8 @@
// returns false.
bool HasTrajectory(const fb::Trajectory *trajectory) const;
void AddTrajectory(const fb::Trajectory *trajectory);
+ bool IsCurrentTrajectory(const fb::Trajectory *trajectory) const;
+ void DeleteTrajectory(const fb::Trajectory *trajectory);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
const ::Eigen::Matrix<double, 2, 1> &voltage_error);
@@ -58,6 +60,8 @@
: true;
}
+ size_t trajectory_count() const { return trajectories_.size(); }
+
// Returns true if the splinedrivetrain is enabled.
bool enable() const { return enable_; }
@@ -79,10 +83,7 @@
void DeleteCurrentSpline();
const FinishedTrajectory ¤t_trajectory() const {
- CHECK(current_trajectory_index_);
- CHECK_LE(0u, *current_trajectory_index_);
- CHECK_LT(*current_trajectory_index_, trajectories_.size());
- return *trajectories_[*current_trajectory_index_];
+ return *CHECK_NOTNULL(current_trajectory_);
}
const DrivetrainConfig<double> dt_config_;
@@ -92,7 +93,7 @@
// TODO(james): Sort out construction to avoid so much dynamic memory
// allocation...
std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
- std::optional<size_t> current_trajectory_index_;
+ const FinishedTrajectory *current_trajectory_ = nullptr;
std::optional<int> commanded_spline_;
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index c2e2999..2e0bebc 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -8,7 +8,6 @@
#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"
@@ -43,6 +42,57 @@
"/pi2/camera")),
auto_splines_() {
set_max_drivetrain_voltage(2.0);
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Setup(event_loop->monotonic_now());
+ });
+}
+
+void AutonomousActor::Replan() {
+ if (FLAGS_galactic_search) {
+ galactic_search_splines_ = {
+ .red_a = PlanSpline(std::bind(&AutonomousSplines::SplineRedA,
+ &auto_splines_, std::placeholders::_1),
+ SplineDirection::kForward),
+ .red_b = PlanSpline(std::bind(&AutonomousSplines::SplineRedB,
+ &auto_splines_, std::placeholders::_1),
+ SplineDirection::kForward),
+ .blue_a = PlanSpline(std::bind(&AutonomousSplines::SplineBlueA,
+ &auto_splines_, std::placeholders::_1),
+ SplineDirection::kForward),
+ .blue_b = PlanSpline(std::bind(&AutonomousSplines::SplineBlueB,
+ &auto_splines_, std::placeholders::_1),
+ SplineDirection::kForward)};
+ } else if (FLAGS_bounce) {
+ bounce_splines_ = {
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavBounce1, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavBounce2, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavBounce3, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavBounce4, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kBackward)};
+ } else if (FLAGS_barrel) {
+ barrel_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavBarrel, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kForward);
+ } else if (FLAGS_slalom) {
+ slalom_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::AutoNavSlalom, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kForward);
+ } else if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1),
+ SplineDirection::kForward);
+ }
}
void AutonomousActor::Reset() {
@@ -59,6 +109,11 @@
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+
+ // Queue up a replan to occur as soon as this action completes.
+ // TODO(james): Modify this so we don't replan during teleop.
+ replan_timer_->Setup(monotonic_now());
+
AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
@@ -80,9 +135,8 @@
return true;
}
-void AutonomousActor::SendStartingPosition(double x, double y, double theta) {
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
// 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.
@@ -90,9 +144,9 @@
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_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);
if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
@@ -100,6 +154,8 @@
}
void AutonomousActor::GalacticSearch() {
+ CHECK(galactic_search_splines_);
+
path_fetcher_.Fetch();
CHECK(path_fetcher_.get() != nullptr)
<< "Expect at least one GalacticSearchPath message before running "
@@ -107,171 +163,101 @@
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::SplineGoal>::Builder
- *builder) {
- flatbuffers::Offset<frc971::MultiSpline> target_spline;
- if (path_fetcher_->alliance() == y2020::vision::Alliance::kRed) {
- if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
- LOG(INFO) << "Red A";
- target_spline = auto_splines_.SplineRedA(builder);
- } else {
- LOG(INFO) << "Red B";
- CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
- target_spline = auto_splines_.SplineRedB(builder);
- }
- } else {
- if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
- LOG(INFO) << "Blue A";
- target_spline = auto_splines_.SplineBlueA(builder);
- } else {
- LOG(INFO) << "Blue B";
- 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);
+ SplineHandle *spline = nullptr;
+ if (path_fetcher_->alliance() == y2020::vision::Alliance::kRed) {
+ if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
+ LOG(INFO) << "Red A";
+ spline = &galactic_search_splines_->red_a;
+ } else {
+ LOG(INFO) << "Red B";
+ CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
+ spline = &galactic_search_splines_->red_b;
+ }
+ } else {
+ if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
+ LOG(INFO) << "Blue A";
+ spline = &galactic_search_splines_->blue_a;
+ } else {
+ LOG(INFO) << "Blue B";
+ CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
+ spline = &galactic_search_splines_->red_b;
+ }
+ }
+ CHECK_NOTNULL(spline);
- SendStartingPosition(CHECK_NOTNULL(spline));
-
- return target_spline;
- },
- SplineDirection::kForward);
+ SendStartingPosition(spline->starting_position());
set_intake_goal(1.2);
set_roller_voltage(7.0);
SendSuperstructureGoal();
- if (!spline1.WaitForPlan()) return;
- spline1.Start();
+ if (!spline->WaitForPlan()) return;
+ spline->Start();
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!spline->WaitForSplineDistanceRemaining(0.02)) return;
RetractIntake();
}
}
void AutonomousActor::AutoNavBounce() {
- SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) {
- flatbuffers::Offset<frc971::MultiSpline> target_spline =
- auto_splines_.AutoNavBounce1(builder);
- const frc971::MultiSpline *const spline =
- flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
- SendStartingPosition(CHECK_NOTNULL(spline));
- return target_spline;
- },
- SplineDirection::kForward);
+ CHECK(bounce_splines_);
- if (!spline1.WaitForPlan()) return;
- spline1.Start();
+ auto &splines = *bounce_splines_;
- SplineHandle spline2 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) { return auto_splines_.AutoNavBounce2(builder); },
- SplineDirection::kBackward);
+ SendStartingPosition(splines[0].starting_position());
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
- if (!spline2.WaitForPlan()) return;
- spline2.Start();
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
- SplineHandle spline3 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) { return auto_splines_.AutoNavBounce3(builder); },
- SplineDirection::kForward);
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
- if (!spline2.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
- if (!spline3.WaitForPlan()) return;
- spline3.Start();
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
- SplineHandle spline4 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) { return auto_splines_.AutoNavBounce4(builder); },
- SplineDirection::kBackward);
+ if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
- if (!spline3.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
- if (!spline4.WaitForPlan()) return;
- spline4.Start();
-
- if (!spline4.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
}
void AutonomousActor::AutoNavBarrel() {
- SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) {
- flatbuffers::Offset<frc971::MultiSpline> target_spline;
- target_spline = auto_splines_.AutoNavBarrel(builder);
- const frc971::MultiSpline *const spline =
- flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
- SendStartingPosition(CHECK_NOTNULL(spline));
- return target_spline;
- },
- SplineDirection::kForward);
+ CHECK(barrel_spline_);
- if (!spline1.WaitForPlan()) return;
- spline1.Start();
+ SendStartingPosition(barrel_spline_->starting_position());
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!barrel_spline_->WaitForPlan()) return;
+ barrel_spline_->Start();
+
+ if (!barrel_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
void AutonomousActor::AutoNavSlalom() {
- SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) {
- flatbuffers::Offset<frc971::MultiSpline> target_spline;
- target_spline = auto_splines_.AutoNavSlalom(builder);
- const frc971::MultiSpline *const spline =
- flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
- SendStartingPosition(CHECK_NOTNULL(spline));
- return target_spline;
- },
- SplineDirection::kForward);
+ CHECK(slalom_spline_);
- if (!spline1.WaitForPlan()) return;
- spline1.Start();
+ SendStartingPosition(slalom_spline_->starting_position());
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ if (!slalom_spline_->WaitForPlan()) return;
+ slalom_spline_->Start();
+
+ if (!slalom_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
void AutonomousActor::SplineAuto() {
- SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::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);
+ CHECK(test_spline_);
- if (!spline1.WaitForPlan()) return;
- spline1.Start();
+ SendStartingPosition(test_spline_->starting_position());
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
-}
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
-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));
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
ProfileParametersT MakeProfileParametersT(const float max_velocity,
@@ -283,7 +269,7 @@
}
bool AutonomousActor::DriveFwd() {
- SendStartingPosition(0, 0, 0);
+ 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);
@@ -291,7 +277,6 @@
}
void AutonomousActor::SendSuperstructureGoal() {
-
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index b22ab36..7bdb44b 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -15,16 +15,16 @@
namespace y2020 {
namespace actors {
-using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
namespace superstructure = y2020::control_loops::superstructure;
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor : public frc971::autonomous::BaseAutonomousActor {
public:
- explicit AutonomousActor(::aos::EventLoop *event_loop);
+ explicit AutonomousActor(aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) override;
+ const frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset();
@@ -37,24 +37,42 @@
void SendSuperstructureGoal();
void RetractIntake();
void SplineAuto();
- void SendStartingPosition(double x, double y, double theta);
- void SendStartingPosition(const frc971::MultiSpline *const spline);
+ void SendStartingPosition(const Eigen::Vector3d &start);
void GalacticSearch();
void AutoNavBounce();
void AutoNavBarrel();
void AutoNavSlalom();
bool DriveFwd();
+ void Replan();
+
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
const float kRollerSpeedCompensation = 2.0;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- ::aos::Sender<::y2020::control_loops::superstructure::Goal>
+ aos::Sender<y2020::control_loops::superstructure::Goal>
superstructure_goal_sender_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<y2020::vision::GalacticSearchPath> path_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+
+ std::optional<std::array<SplineHandle, 4>> bounce_splines_;
+
+ struct GalacticSearchSplines {
+ SplineHandle red_a;
+ SplineHandle red_b;
+ SplineHandle blue_a;
+ SplineHandle blue_b;
+ };
+ std::optional<GalacticSearchSplines> galactic_search_splines_;
+
+ std::optional<SplineHandle> barrel_spline_;
+ std::optional<SplineHandle> slalom_spline_;
+ std::optional<SplineHandle> test_spline_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
};
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 17666de..652efbd 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -202,8 +202,9 @@
"name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.fb.Trajectory",
"source_node": "roborio",
- "max_size": 200000,
- "frequency": 10,
+ "max_size": 400000,
+ "frequency": 2,
+ "num_senders": 2,
"read_method": "PIN",
"num_readers": 6
},