diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 850b7f86..a1e429db 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -180,7 +180,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); return 1; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString()); + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } else { diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 611f8154..77a562d0 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -34,6 +34,7 @@ #include "ur_client_library/rtde/data_package.h" #include "ur_client_library/comm/stream.h" #include "ur_client_library/queue/readerwriterqueue.h" +#include "ur_client_library/ur/datatypes.h" #include #include @@ -118,10 +119,14 @@ class RTDEWriter * * \param output_pin The pin to change * \param value The new value, it should be between 0 and 1, where 0 is 4mA and 1 is 20mA. + * \param type The domain for the output can be eitherAnalogOutputType::CURRENT or AnalogOutputType::VOLTAGE or + * AnalogOutputType::SET_ON_TEACH_PENDANT. In the latter case the domain is left untouched and the domain configured + * on the teach pendant will be used. * * \returns Success of the package creation */ - bool sendStandardAnalogOutput(uint8_t output_pin, double value); + bool sendStandardAnalogOutput(uint8_t output_pin, double value, + const AnalogOutputType type = AnalogOutputType::SET_ON_TEACH_PENDANT); /*! * \brief Creates a package to request setting a new value for an input_bit_register diff --git a/include/ur_client_library/ur/datatypes.h b/include/ur_client_library/ur/datatypes.h index a928e0fc..af6fecd2 100644 --- a/include/ur_client_library/ur/datatypes.h +++ b/include/ur_client_library/ur/datatypes.h @@ -79,6 +79,13 @@ enum class SafetyStatus : int8_t // Only available on 3.10/5.4 SYSTEM_THREE_POSITION_ENABLING_STOP = 13 }; +enum class AnalogOutputType : int8_t +{ + SET_ON_TEACH_PENDANT = -1, + CURRENT = 0, + VOLTAGE = 1 +}; + inline std::string robotModeString(const RobotMode& mode) { switch (mode) diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index d0d14253..ea543ab8 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -204,7 +204,7 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) return success; } -bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value) +bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, const AnalogOutputType type) { if (output_pin > 1) { @@ -216,18 +216,22 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value) if (value > 1.0 || value < 0.0) { std::stringstream ss; - ss << "Analog output value should be between 0 and 1. The value is " << static_cast(value); + ss << "Analog output value should be between 0 and 1. The value is " << static_cast(value); URCL_LOG_ERROR(ss.str().c_str()); return false; } std::lock_guard guard(package_mutex_); uint8_t mask = pinToMask(output_pin); - // default to current for now, as no functionality to choose included in set io service - uint8_t output_type = 0; + bool success = true; success = package_.setData("standard_analog_output_mask", mask); - success = success && package_.setData("standard_analog_output_type", output_type); + if (type != AnalogOutputType::SET_ON_TEACH_PENDANT) + { + auto output_type_bits = [](const uint8_t pin, const uint8_t type) { return type << pin; }; + uint8_t output_type = output_type_bits(output_pin, toUnderlying(type)); + success = success && package_.setData("standard_analog_output_type", output_type); + } success = success && package_.setData("standard_analog_output_0", value); success = success && package_.setData("standard_analog_output_1", value); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index bec76bd9..070e99b1 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -260,9 +260,12 @@ TEST_F(RTDEWriterTest, send_tool_digital_output) EXPECT_FALSE(writer_->sendToolDigitalOutput(pin, send_pin_value)); } -TEST_F(RTDEWriterTest, send_standard_analog_output) +TEST_F(RTDEWriterTest, send_standard_analog_output_unknown_domain) { + waitForMessageCallback(1000); + uint8_t expected_standard_analog_output_mask = 1; + uint8_t pin = 0; double send_analog_output = 1; EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output)); @@ -272,26 +275,100 @@ TEST_F(RTDEWriterTest, send_standard_analog_output) ASSERT_TRUE(dataFieldExist("standard_analog_output_0")); ASSERT_TRUE(dataFieldExist("standard_analog_output_1")); ASSERT_TRUE(dataFieldExist("standard_analog_output_mask")); + ASSERT_TRUE(dataFieldExist("standard_analog_output_type")); + + double received_analog_output = std::get(parsed_data_["standard_analog_output_0"]); + uint8_t received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + uint8_t received_standard_analog_output_type = std::get(parsed_data_["standard_analog_output_type"]); + + EXPECT_EQ(send_analog_output, received_analog_output); + EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + // The test server sets this to 0 if not given + EXPECT_EQ(0, received_standard_analog_output_type); +} + +TEST_F(RTDEWriterTest, send_standard_analog_output_voltage) +{ + uint8_t pin = 0; + AnalogOutputType type = AnalogOutputType::VOLTAGE; + double send_analog_output = 1; + + uint8_t expected_standard_analog_output_mask = 1; + uint8_t expected_standard_analog_output_type = 1; + + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type)); + + waitForMessageCallback(1000); double received_analog_output = std::get(parsed_data_["standard_analog_output_0"]); uint8_t received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + uint8_t received_standard_analog_output_type = std::get(parsed_data_["standard_analog_output_type"]); EXPECT_EQ(send_analog_output, received_analog_output); EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type); pin = 1; expected_standard_analog_output_mask = 2; - EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output)); + expected_standard_analog_output_type = 2; + + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type)); waitForMessageCallback(1000); received_analog_output = std::get(parsed_data_["standard_analog_output_1"]); received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + received_standard_analog_output_type = std::get(parsed_data_["standard_analog_output_type"]); EXPECT_EQ(send_analog_output, received_analog_output); EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type); +} - // Changing pins above 1, should return false. +TEST_F(RTDEWriterTest, send_standard_analog_output_current) +{ + uint8_t pin = 0; + AnalogOutputType type = AnalogOutputType::CURRENT; + double send_analog_output = 1; + + uint8_t expected_standard_analog_output_mask = 1; + uint8_t expected_standard_analog_output_type = 0; + + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type)); + + waitForMessageCallback(1000); + + double received_analog_output = std::get(parsed_data_["standard_analog_output_0"]); + uint8_t received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + uint8_t received_standard_analog_output_type = std::get(parsed_data_["standard_analog_output_type"]); + + EXPECT_EQ(send_analog_output, received_analog_output); + EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type); + + pin = 1; + expected_standard_analog_output_mask = 2; + expected_standard_analog_output_type = 0; + + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type)); + + waitForMessageCallback(1000); + + received_analog_output = std::get(parsed_data_["standard_analog_output_1"]); + received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + received_standard_analog_output_type = std::get(parsed_data_["standard_analog_output_type"]); + + EXPECT_EQ(send_analog_output, received_analog_output); + EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type); +} + +TEST_F(RTDEWriterTest, send_standard_analog_output_illegal_input_fails) +{ + uint8_t pin = 0; + double send_analog_output = 1; + + // Pin should be either 0 or 1 pin = 2; EXPECT_FALSE(writer_->sendStandardAnalogOutput(pin, send_analog_output));