Add goforward ctrl

diff --git a/ctrl/goforward.cc b/ctrl/goforward.cc
new file mode 100644 (file)
index 0000000..16d0a5d
--- /dev/null
+++ b/ctrl/goforward.cc
@@ -0,0 +1,20 @@
+#include "stage.hh"
+using namespace Stg;
+
+class ControlledRobot {
+public:
+       ModelPosition *pos = nullptr;
+};
+
+extern "C" int Init(Model *model, CtrlArgs *)
+{
+       auto r = new ControlledRobot();
+       r->pos = dynamic_cast<ModelPosition *>(model);
+       if (r->pos == nullptr) {
+               PRINT_ERR("No position model.");
+               exit(1);
+       }
+       r->pos->Subscribe();
+       r->pos->SetXSpeed(3.0);
+       return 0;
+}