From 3402db3348216307a07057dd4ac7fea21b1b4b3c Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Thu, 3 Dec 2015 10:54:36 +0100
Subject: [PATCH] Fixed set_io service to work with 3.x

---
 include/ur_modern_driver/ur_driver.h |  1 +
 src/ur_driver.cpp                    | 32 ++++++++++++++++++++++------
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h
index f2d9b7b..4781f4f 100644
--- a/include/ur_modern_driver/ur_driver.h
+++ b/include/ur_modern_driver/ur_driver.h
@@ -49,6 +49,7 @@ private:
 	bool reverse_connected_;
 	double servoj_time_;
 	bool executing_traj_;
+	double firmware_version_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index fe30c2c..274da91 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -181,7 +181,8 @@ bool UrDriver::uploadProg() {
 	cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
 
 	if (sec_interface_->robot_state_->getVersion() >= 3.1)
-		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", servoj_time_);
+		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n",
+				servoj_time_);
 	else
 		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
 	cmd_str += buf;
@@ -246,8 +247,8 @@ void UrDriver::closeServo(std::vector<double> positions) {
 bool UrDriver::start() {
 	if (!sec_interface_->start())
 		return false;
-	rt_interface_->robot_state_->setVersion(
-			sec_interface_->robot_state_->getVersion());
+	firmware_version_ = sec_interface_->robot_state_->getVersion();
+	rt_interface_->robot_state_->setVersion(firmware_version_);
 	if (!rt_interface_->start())
 		return false;
 	ip_addr_ = rt_interface_->getLocalIp();
@@ -295,15 +296,34 @@ void UrDriver::setFlag(unsigned int n, bool b) {
 }
 void UrDriver::setDigitalOut(unsigned int n, bool b) {
 	char buf[256];
-	sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
-			b ? "True" : "False");
+	if (firmware_version_ < 2) {
+		sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
+				b ? "True" : "False");
+	} else if (n > 9) {
+		sprintf(buf,
+				"sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
+				n - 10, b ? "True" : "False");
+	} else if (n > 7) {
+		sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
+				n - 8, b ? "True" : "False");
+
+	} else {
+		sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
+				n, b ? "True" : "False");
+
+	}
 	rt_interface_->addCommandToQueue(buf);
 	print_debug(buf);
 
 }
 void UrDriver::setAnalogOut(unsigned int n, double f) {
 	char buf[256];
-	sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
+	if (firmware_version_ < 2) {
+		sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
+	} else {
+		sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
+	}
+
 	rt_interface_->addCommandToQueue(buf);
 	print_debug(buf);
 }