From bbe2102ac88871d2c5d85d235ca1c52caef1b32e Mon Sep 17 00:00:00 2001
From: Simon Rasmussen <zetlon@gmail.com>
Date: Thu, 13 Apr 2017 10:43:23 +0200
Subject: [PATCH] Initial implementation of commander

---
 include/ur_modern_driver/ur/commander.h | 27 +++++++++++
 src/ur/commander.cpp                    | 62 +++++++++++++++++++++++++
 2 files changed, 89 insertions(+)
 create mode 100644 include/ur_modern_driver/ur/commander.h
 create mode 100644 src/ur/commander.cpp

diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h
new file mode 100644
index 0000000..6325f8c
--- /dev/null
+++ b/include/ur_modern_driver/ur/commander.h
@@ -0,0 +1,27 @@
+#pragma once
+#include <array>
+#include <sstream>
+#include <iomanip>
+#include "ur_modern_driver/ur/stream.h"
+
+class URCommander
+{
+private:
+  URStream& stream_;
+
+protected:
+  bool write(std::string& s);
+
+public:
+  URCommander(URStream& stream) : stream_(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+  virtual bool stopj(double a = 10.0);
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+  virtual bool setToolVoltage(uint8_t voltage);
+  virtual bool setFlag(bool value);
+  virtual bool setPayload(double value);
+};
\ No newline at end of file
diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp
new file mode 100644
index 0000000..d9e16d2
--- /dev/null
+++ b/src/ur/commander.cpp
@@ -0,0 +1,62 @@
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/log.h"
+
+bool URCommander::write(std::string& s)
+{
+    size_t len = s.size();
+    const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
+    ssize_t res = stream_.send(data, len);
+    return res > 0 && static_cast<size_t>(res) == len;
+}
+
+
+bool URCommander::speedj(std::array<double, 6> &speeds, double acceleration)
+{
+    std::ostringstream out;
+    out << std::fixed << std::setprecision(4);
+    out << "speedj([";
+    std::string mod;
+    for(auto const& val : speeds)
+    {
+        out << mod << val;
+        mod = ",";
+    }
+    out << "]," << acceleration << ")\n";
+    std::string s(out.str());
+    return write(s);
+}
+bool URCommander::stopj(double a)
+{
+
+}
+
+bool URCommander::setAnalogOut(uint8_t pin, double value)
+{
+    std::ostringstream out;
+    out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n";
+    std::string s(out.str());
+    return write(s);
+}
+
+bool URCommander::setDigitalOut(uint8_t pin, bool value)
+{
+    std::ostringstream out;
+    out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+    std::string s(out.str());
+    return write(s);
+}
+
+bool URCommander::setToolVoltage(uint8_t voltage)
+{
+    
+}
+
+
+bool URCommander::setFlag(bool value)
+{
+
+}
+bool URCommander::setPayload(double value)
+{
+
+}
\ No newline at end of file