Added a tests for dealing with bad output/position. Currently, they fail.
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 934ba9d..96b3b9f 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -144,6 +144,41 @@
VerifyNearGoal();
}
+// Tests that it survives disabling.
+TEST_F(DrivetrainTest, SurvivesZeroing) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ if (i > 20 && i < 200) {
+ SendDSPacket(false);
+ } else {
+ SendDSPacket(true);
+ }
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+// Tests surviving bad positions.
+TEST_F(DrivetrainTest, SurvivesBadPosition) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ if (i > 20 && i < 200) {
+ } else {
+ drivetrain_motor_plant_.SendPositionMessage();
+ }
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971