force all loops to handle not having goals
Change-Id: I630f98d3b373f4c5e8fc6dec589dd5b077a9a391
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 4f86697..49205d9 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -174,6 +174,17 @@
2.0 * M_PI / 180.0);*/
}
+// Tests that not having a goal doesn't break anything.
+TEST_F(ClawTest, NoGoal) {
+ for (int i = 0; i < 10; ++i) {
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+
+ TickTime();
+ }
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 395f8b9..bcfc6a1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -689,19 +689,28 @@
}
no_position_.Print();
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
+ bool control_loop_driving = false;
+ if (goal) {
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
#if HAVE_SHIFTERS
- bool highgear = goal->highgear;
+ bool highgear = goal->highgear;
#endif
- bool control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
+ control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
+#if HAVE_SHIFTERS
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+#else
+ dt_openloop.SetGoal(wheel, throttle, quickturn, false);
+#endif
+ }
+
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
@@ -714,11 +723,6 @@
}
}
dt_openloop.SetPosition(position);
-#if HAVE_SHIFTERS
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
-#else
- dt_openloop.SetGoal(wheel, throttle, quickturn, false);
-#endif
dt_openloop.Update();
if (control_loop_driving) {
@@ -732,7 +736,7 @@
}
dt_closedloop.Update(output == NULL, false);
}
-
+
// set the output status of the control loop state
if (status) {
bool done = false;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 96583da..7e14cf0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -170,6 +170,26 @@
VerifyNearGoal();
}
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+}
+
::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 9533ddf..65ee72e 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -56,7 +56,7 @@
}
Fridge::Fridge(control_loops::FridgeQueue *fridge)
- : aos::controls::ControlLoop<control_loops::FridgeQueue, false>(fridge),
+ : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
arm_loop_(new CappedStateFeedbackLoop(
StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop(
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index eecfe1b..87dbfa4 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -40,7 +40,7 @@
};
class Fridge
- : public aos::controls::ControlLoop<control_loops::FridgeQueue, false> {
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
public:
explicit Fridge(
control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);