Fix edge case in coerce and add tests
This adds a baseline test case, as well as two tests (horizontal &
vertical) which failed prior to this change. I would encourage someone
to write more tests for the coerce goal functionality, but I am not
going to do it in this change :P
Change-Id: I76ee32cec46b9557fc5f5a09fa172be64b82cc70
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 6e927b0..f9a52cc 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -50,7 +50,7 @@
if (projectedh(i, 0) > 0) {
max_boundary =
::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
- } else {
+ } else if (projectedh(i, 0) != 0) {
min_boundary =
::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
}