Merge "Shrink the Connect message for message_bridge"
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 70ceb1a..198405f 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -563,6 +563,7 @@
::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
double dt_float = ::aos::time::DurationInSeconds(dt);
+ const double last_distance = (*state)(0);
// TODO(austin): This feels like something that should be pulled out into
// a library for re-use.
*state = RungeKutta(
@@ -571,6 +572,12 @@
return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
},
*state, dt_float);
+ // Force the distance to move forwards, to guarantee that we actually finish
+ // the planning.
+ constexpr double kMinDistanceIncrease = 1e-7;
+ if ((*state)(0) < last_distance + kMinDistanceIncrease) {
+ (*state)(0) = last_distance + kMinDistanceIncrease;
+ }
::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
(*state)(1) = result(1);
@@ -585,6 +592,13 @@
result.back()(1) = 0.0;
while (!is_at_end(state)) {
+ if (state_is_faulted(state)) {
+ LOG(WARNING)
+ << "Found invalid state in generating spline and aborting. This is "
+ "likely due to a spline with extremely high jerk/changes in "
+ "curvature with an insufficiently small step size.";
+ return {};
+ }
result.emplace_back(GetNextXVA(dt, &state));
}
return result;
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a5b9807..6be898a 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -138,6 +138,13 @@
return state(0) > length() - 1e-4;
}
+ // Returns true if the state is invalid or unreasonable in some way.
+ bool state_is_faulted(::Eigen::Matrix<double, 2, 1> state) const {
+ // Consider things faulted if the current velocity implies we are going
+ // backwards or if any infinities/NaNs have crept in.
+ return state(1) < 0 || !state.allFinite();
+ }
+
// Returns the length of the path in meters.
double length() const { return spline_->length(); }
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 7e98a38..60d1e65 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -41,14 +41,15 @@
traj = Trajectory(distanceSpline)
traj.Plan()
XVA = traj.GetPlanXVA(dT)
- self.draw_x_axis(cr, start, height, zero, XVA, end)
- self.drawVelocity(cr, XVA, start, height, skip, zero, end)
- self.drawAcceleration(cr, XVA, start, height, skip, zero,
- AXIS_MARGIN_SPACE, end)
- self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
- cr.set_source_rgb(0, 0, 0)
- cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
- cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ if len(XVA[0]) > 0:
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
cr.stroke()
def connectLines(self, cr, points, color):