run control loops and write their output on new sensor values
This also includes sending solenoid values from their own thread at 50Hz
(formerly I613f95a6efb5efe428029e4825ba6caeb34ea326).
Change-Id: I3d3021cdbbf2ddf895e5ceebd4db299b4743e124
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 142da9f..4712bb9 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -12,29 +12,30 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, has_position, fail_no_position,
- fail_no_goal>::kStaleLogInterval;
+template <class T, bool fail_no_goal>
+constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+template <class T, bool fail_no_goal>
void
-ControlLoop<T, has_position, fail_no_position, fail_no_goal>::ZeroOutputs() {
+ControlLoop<T, fail_no_goal>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Iterate() {
+template <class T, bool fail_no_goal>
+void ControlLoop<T, fail_no_goal>::Iterate() {
no_prior_goal_.Print();
no_sensor_generation_.Print();
- very_stale_position_.Print();
- no_prior_position_.Print();
driver_station_old_.Print();
no_driver_station_.Print();
- // Fetch the latest control loop goal and position. If there is no new
+ control_loop_->position.FetchAnother();
+ const PositionType *const position = control_loop_->position.get();
+ LOG_STRUCT(DEBUG, "position", *position);
+
+ // Fetch the latest control loop goal. If there is no new
// goal, we will just reuse the old one.
// If there is no goal, we haven't started up fully. It isn't worth
// the added complexity for each loop implementation to handle that case.
@@ -72,39 +73,6 @@
LOG_STRUCT(DEBUG, "goal", *goal);
}
- // Only pass in a position if we got one this cycle.
- const PositionType *position = NULL;
-
- // Only fetch the latest position if we have one.
- if (has_position) {
- // If the position is stale, this is really bad. Try fetching a position
- // and check how fresh it is, and then take the appropriate action.
- if (control_loop_->position.FetchLatest()) {
- position = control_loop_->position.get();
- } else {
- if (control_loop_->position.get() && !reset_) {
- int msec_age = control_loop_->position.Age().ToMSec();
- if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
- LOG_INTERVAL(very_stale_position_);
- ZeroOutputs();
- return;
- } else {
- LOG(ERROR, "Stale position. %d ms (< %d ms)\n", msec_age,
- kPositionTimeoutMs);
- }
- } else {
- LOG_INTERVAL(no_prior_position_);
- if (fail_no_position) {
- ZeroOutputs();
- return;
- }
- }
- }
- if (position) {
- LOG_STRUCT(DEBUG, "position", *position);
- }
- }
-
bool outputs_enabled = false;
// Check to see if we got a driver station packet recently.
@@ -160,14 +128,9 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Run() {
- ::aos::time::Time::EnableMockTime();
+template <class T, bool fail_no_goal>
+void ControlLoop<T, fail_no_goal>::Run() {
while (true) {
- ::aos::time::Time::UpdateMockTime();
- const ::aos::time::Time next_loop = NextLoopTime();
- time::SleepUntil(next_loop);
- ::aos::time::Time::SetMockTime(next_loop);
Iterate();
}
}
diff --git a/aos/common/controls/control_loop.cc b/aos/common/controls/control_loop.cc
index 72201bf..c314254 100644
--- a/aos/common/controls/control_loop.cc
+++ b/aos/common/controls/control_loop.cc
@@ -3,11 +3,5 @@
namespace aos {
namespace controls {
-time::Time NextLoopTime(time::Time start) {
- return (start / static_cast<int32_t>(kLoopFrequency.ToNSec())) *
- static_cast<int32_t>(kLoopFrequency.ToNSec()) +
- kLoopFrequency;
-}
-
} // namespace controls
} // namespace aos
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index 76c06e7..e9454d1 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -39,19 +39,12 @@
// Control loops run this often, "starting" at time 0.
constexpr time::Time kLoopFrequency = time::Time::InSeconds(0.01);
-// Calculates the next time to run control loops after start.
-time::Time NextLoopTime(time::Time start = time::Time::Now());
-
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
// 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.
-// If has_position is false, the control loop will always use NULL as the
-// position and not check the queue. This is used for "loops" that control
-// motors open loop.
-template <class T, bool has_position = true, bool fail_no_position = true,
- bool fail_no_goal = true>
+template <class T, bool fail_no_goal = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
@@ -157,15 +150,9 @@
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
static constexpr ::aos::time::Time kStaleLogInterval =
::aos::time::Time::InSeconds(0.1);
- SimpleLogInterval very_stale_position_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "outputs disabled because position is very stale");
SimpleLogInterval no_prior_goal_ =
SimpleLogInterval(kStaleLogInterval, ERROR,
"no prior goal");
- SimpleLogInterval no_prior_position_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "no prior position");
SimpleLogInterval no_driver_station_ =
SimpleLogInterval(kStaleLogInterval, ERROR,
"no driver station packet");