cleaned up a bunch of build + formatting
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.cc b/aos/crio/motor_server/CRIOControlLoopRunner.cc
new file mode 100644
index 0000000..a4cf3ce
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.cc
@@ -0,0 +1,50 @@
+#include "CRIOControlLoopRunner.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+#include "aos/crio/motor_server/MotorOutput.h"
+#include "aos/crio/motor_server/MotorServer.h"
+
+using ::aos::control_loops::SerializableControlLoop;
+
+namespace aos {
+namespace crio {
+
+bool CRIOControlLoopRunner::started_ = false;
+std::vector<SerializableControlLoop *> CRIOControlLoopRunner::loops_;
+Mutex CRIOControlLoopRunner::loops_lock;
+
+void CRIOControlLoopRunner::Start() {
+ if (started_) {
+ LOG(WARNING, "not going to Start twice!!\n");
+ return;
+ }
+ started_ = true;
+
+ // TODO(aschuh): Hold on to a handle to this...
+ (new WDInterruptNotifier<void>(Notify))->StartPeriodic(0.01);
+}
+
+void CRIOControlLoopRunner::AddControlLoop(SerializableControlLoop *loop) {
+ MutexLocker control_loop_goals_locker(&loops_lock);
+ loops_.push_back(loop);
+ MotorServer::RegisterControlLoopGoal(loop);
+}
+
+void CRIOControlLoopRunner::Notify(void *) {
+ // TODO(aschuh): Too many singletons/static classes!
+ SensorOutputs::UpdateAll();
+ // sensors get read first so it doesn't really matter if this takes a little bit
+ {
+ MutexLocker control_loop_goals_locker(
+ &MotorServer::control_loop_goals_lock);
+ for (auto it = loops_.begin(); it != loops_.end(); ++it) {
+ (*it)->Iterate();
+ }
+ }
+ MotorOutput::RunIterationAll();
+ MotorServer::WriteOutputs();
+}
+
+} // namespace crio
+} // namespace aos