force all loops to handle not having goals
Change-Id: I630f98d3b373f4c5e8fc6dec589dd5b077a9a391
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 42702c0..bf2eea4 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -11,22 +11,21 @@
// TODO(aschuh): Tests.
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kPwmDisableTime;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kStaleLogInterval;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kPwmDisableTime;
-template <class T, bool fail_no_goal>
-void
-ControlLoop<T, fail_no_goal>::ZeroOutputs() {
+template <class T>
+void ControlLoop<T>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Iterate() {
+template <class T>
+void ControlLoop<T>::Iterate() {
no_goal_.Print();
driver_station_old_.Print();
no_sensor_state_.Print();
@@ -49,10 +48,6 @@
LOG_STRUCT(DEBUG, "goal", *goal);
} else {
LOG_INTERVAL(no_goal_);
- if (fail_no_goal) {
- ZeroOutputs();
- return;
- }
}
::aos::robot_state.FetchLatest();
@@ -130,8 +125,8 @@
status.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Run() {
+template <class T>
+void ControlLoop<T>::Run() {
while (true) {
Iterate();
}
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index a176fb6..d48f017 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -44,7 +44,7 @@
// that has a goal, position, status, and output queue.
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
-template <class T, bool fail_no_goal = true>
+template <class T>
class ControlLoop : public SerializableControlLoop {
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
@@ -116,11 +116,12 @@
protected:
// Runs an iteration of the control loop.
- // goal is the last goal that was sent. It might be any number of cycles old.
- // position is the current position, or NULL if we didn't get a position this
- // cycle.
- // output is the values to be sent to the motors. This is NULL if the output
- // is going to be ignored and set to 0.
+ // goal is the last goal that was sent. It might be any number of cycles old
+ // or nullptr if we haven't ever received a goal.
+ // position is the current position, or nullptr if we didn't get a position
+ // this cycle.
+ // output is the values to be sent to the motors. This is nullptr if the
+ // output is going to be ignored and set to 0.
// status is the status of the control loop.
// Both output and status should be filled in by the implementation.
virtual void RunIteration(const GoalType *goal,
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);