Prevent infinite loops in Trajectory planning
There were some cases where splines with really extreme jerks and too
high of timesteps would end up in infiite loops because the planning
would end up with a backwards velocity. Handle those cases more
explicitly.
Change-Id: Ib091d8cbebc1874282a65b4b7b8a390dff9c9054
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):