From d7d84caee79513fbd1da44eb2b3c8a8ce9f13f8d Mon Sep 17 00:00:00 2001
From: Miguel Prada <miguel.prada.sarasola@gmail.com>
Date: Mon, 12 Feb 2018 23:04:00 +0100
Subject: [PATCH] Add time parameter to speedj for UR software >= 3.3

Fixes #15.

Related to https://github.com/ThomasTimm/ur_modern_driver/issues/92
---
 include/ur_modern_driver/ur/commander.h | 22 ++++++++++++++++-
 include/ur_modern_driver/ur/factory.h   |  4 +++-
 src/ur/commander.cpp                    | 32 +++++++++++++++++--------
 3 files changed, 46 insertions(+), 12 deletions(-)

diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h
index 6a2b486..0a4cf6f 100644
--- a/include/ur_modern_driver/ur/commander.h
+++ b/include/ur_modern_driver/ur/commander.h
@@ -49,7 +49,27 @@ public:
   {
   }
 
-  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0;
   virtual bool setDigitalOut(uint8_t pin, bool value);
   virtual bool setAnalogOut(uint8_t pin, double value);
 };
+
+class URCommander_V3_1__2 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_1__2(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};
+
+class URCommander_V3_3 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_3(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};
diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h
index 54c6cdf..1f1c1ef 100644
--- a/include/ur_modern_driver/ur/factory.h
+++ b/include/ur_modern_driver/ur/factory.h
@@ -80,8 +80,10 @@ public:
   {
     if (major_version_ == 1)
       return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
+    else if (minor_version_ < 3)
+      return std::unique_ptr<URCommander>(new URCommander_V3_1__2(stream));
     else
-      return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));
+      return std::unique_ptr<URCommander>(new URCommander_V3_3(stream));
   }
 
   std::unique_ptr<URParser<StatePacket>> getStateParser()
diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp
index 5c752f0..9b85865 100644
--- a/src/ur/commander.cpp
+++ b/src/ur/commander.cpp
@@ -86,16 +86,6 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
   return write(s);
 }
 
-bool URCommander_V3_X::speedj(std::array<double, 6> &speeds, double acceleration)
-{
-  std::ostringstream out;
-  out << std::fixed << std::setprecision(5);
-  out << "speedj(";
-  formatArray(out, speeds);
-  out << "," << acceleration << ")\n";
-  std::string s(out.str());
-  return write(s);
-}
 bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
 {
   std::ostringstream out;
@@ -130,3 +120,25 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
   std::string s(out.str());
   return write(s);
 }
+
+bool URCommander_V3_1__2::speedj(std::array<double, 6> &speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_3::speedj(std::array<double, 6> &speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << "," << 0.008 << ")\n";
+  std::string s(out.str());
+  return write(s);
+}