copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.cpp b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
new file mode 100644
index 0000000..9d6cd52
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
@@ -0,0 +1,49 @@
+#include "CRIOControlLoopRunner.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+#include "aos/crio/motor_server/MotorOutput.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