Store the strategy used for each spline segment
We were running into an issue where the ff voltage would chatter. The
core problem was that we planned a segment using one acceleration
function, and executed it with another. That's not consistent with the
velocity plan, whether or not it's right. We need the acceleration that
we use to execute the plan to be consistent with the velocity, which
only works if we use the same acceleration function used to integrate up
the velocity during the segment. It's more important to be internally
consistent than to track constraints perfectly.
Change-Id: Id28c7036c2d3e585a99577cb4e03967f2bba3e26
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a42476a..049dd33 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -150,9 +150,26 @@
::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
::std::chrono::nanoseconds dt);
+ enum SegmentType : uint8_t {
+ VELOCITY_LIMITED,
+ CURVATURE_LIMITED,
+ ACCELERATION_LIMITED,
+ DECELERATION_LIMITED
+ };
+
+ const ::std::vector<SegmentType> &plan_segment_type() const {
+ return plan_segment_type_;
+ }
+
private:
// Computes alpha for a distance.
- double DistanceToAlpha(double distance) const;
+ size_t DistanceToSegment(double distance) const {
+ return ::std::max(
+ static_cast<size_t>(0),
+ ::std::min(plan_segment_type_.size() - 1,
+ static_cast<size_t>(::std::floor(distance / length() *
+ (plan_.size() - 1)))));
+ }
// Returns K1 and K2.
// K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
@@ -211,6 +228,7 @@
const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
// Velocities in the plan (distance for each index is defined by distance())
::std::vector<double> plan_;
+ ::std::vector<SegmentType> plan_segment_type_;
// Plan voltage limit.
double voltage_limit_ = 12.0;
};