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.