diff --git a/CMakeLists.txt b/CMakeLists.txt
index ffdf225..79a5a8c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,6 +5,8 @@ set(CMAKE_CXX_STANDARD 11)
SET (CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/.orogen/config")
INCLUDE(motors_weg_cvw300Base)
-# FIND_PACKAGE(KDL)
-# FIND_PACKAGE(OCL)
-
+if (ROCK_TEST_ENABLED)
+ enable_testing()
+ find_package(Syskit REQUIRED)
+ syskit_orogen_tests(test)
+endif()
diff --git a/manifest.xml b/manifest.xml
index 7147e68..b6eab70 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -8,4 +8,6 @@
+
+
diff --git a/motors_weg_cvw300.orogen b/motors_weg_cvw300.orogen
index 149702e..0f9f6d1 100644
--- a/motors_weg_cvw300.orogen
+++ b/motors_weg_cvw300.orogen
@@ -1,3 +1,5 @@
+# frozen_string_literal: true
+
name 'motors_weg_cvw300'
import_types_from 'std'
diff --git a/tasks/Task.cpp b/tasks/Task.cpp
index 36280c6..c0be1a4 100644
--- a/tasks/Task.cpp
+++ b/tasks/Task.cpp
@@ -50,7 +50,9 @@ bool Task::configureHook()
driver->writeControlType(_control_type.get());
auto limits = _limits.get();
- driver->writeJointLimits(limits.elements.at(0));
+ if (!limits.elements.empty()) {
+ driver->writeJointLimits(limits.elements.at(0));
+ }
driver->writeRampConfiguration(_ramps.get());
m_cmd_in.elements.resize(1);
@@ -67,9 +69,9 @@ bool Task::startHook()
return false;
}
+ writeSpeedCommand(0);
m_driver->enable();
m_last_temperature_update = Time();
- writeSpeedCommand(0);
return true;
}
bool Task::commandTimedOut() const {
diff --git a/test/modbus_helpers.rb b/test/modbus_helpers.rb
new file mode 100644
index 0000000..e29addd
--- /dev/null
+++ b/test/modbus_helpers.rb
@@ -0,0 +1,120 @@
+# frozen_string_literal: true
+
+module ModbusHelpers
+ def setup
+ super
+
+ @modbus_registers = {}
+ end
+
+ def iodrivers_base_prepare(model)
+ task = syskit_deploy(model)
+ syskit_start_execution_agents(task)
+ reader = syskit_create_reader(task.io_raw_out_port, type: :buffer, size: 10)
+ writer = syskit_create_writer(task.io_raw_in_port)
+
+ [task, reader, writer]
+ end
+
+ def modbus_stop_task
+ expect_execution { task.stop! }
+ .join_all_waiting_work(false)
+ .poll do
+ while (sample = @reader.read_new)
+ @writer.write(modbus_reply(sample))
+ end
+ end
+ .to { emit task.stop_event }
+ end
+
+ def modbus_expect_during_configuration_and_start
+ expect_execution
+ .scheduler(true)
+ .join_all_waiting_work(false)
+ .poll do
+ while (sample = @reader.read_new)
+ @writer.write(modbus_reply(sample))
+ end
+ end
+ end
+
+ def modbus_configure_and_start
+ modbus_expect_during_configuration_and_start.to { emit task.start_event }
+ end
+
+ def modbus_set(register, value)
+ @modbus_registers[register] = value
+ end
+
+ def modbus_get(register)
+ @modbus_registers.fetch(register)
+ rescue KeyError
+ raise ArgumentError, "register #{register} not set"
+ end
+
+ def modbus_reply_until(writer, reader)
+ sample = nil
+ expect_execution
+ .join_all_waiting_work(false)
+ .poll do
+ while !sample && (s = reader.read_new)
+ writer.write(modbus_reply(s))
+ sample = s if yield(s)
+ end
+ end
+ .to { achieve { sample } }
+ end
+
+ def modbus_write?(sample, register: nil, value: nil)
+ return unless sample.data[1] == 6
+
+ actual_register = sample.data[2] << 8 | sample.data[3]
+ actual_value = sample.data[4] << 8 | sample.data[5]
+
+ (!register || register == actual_register) &&
+ (!value || value == actual_value)
+ end
+
+ def modbus_reply(sample)
+ reply = [sample.data[0], sample.data[1]]
+
+ function = sample.data[1]
+ if function == 6 # write
+ address = sample.data[2] << 8 | sample.data[3]
+ value = sample.data[4] << 8 | sample.data[5]
+ @modbus_registers[address] = value
+ elsif [3, 4].include?(function) # Read registers
+ start_address = sample.data[2] << 8 | sample.data[3]
+ register_count = sample.data[4] << 8 | sample.data[5]
+
+ reply << register_count * 2
+ register_count.times do |i|
+ register_value = modbus_get(start_address + i)
+ reply += [register_value >> 8, register_value & 0xFF]
+ end
+ end
+ reply += modbus_crc(reply)
+ Types.iodrivers_base.RawPacket.new(time: Time.now, data: reply)
+ end
+
+ def modbus_reply_to_write(id)
+ reply = [id, 6]
+ reply += modbus_crc(reply)
+ Types.iodrivers_base.RawPacket.new(time: Time.now, data: reply)
+ end
+
+ def modbus_crc(data)
+ crc = 0xFFFF
+ data.each do |byte|
+ crc ^= byte
+
+ 8.times do
+ odd = crc.odd?
+ crc >>= 1
+ crc ^= 0xA001 if odd
+ end
+ end
+
+ [crc & 0xFF, (crc & 0xFF00) >> 8]
+ end
+end
diff --git a/test/task_test.rb b/test/task_test.rb
new file mode 100644
index 0000000..7c351cc
--- /dev/null
+++ b/test/task_test.rb
@@ -0,0 +1,143 @@
+# frozen_string_literal: true
+
+require_relative 'modbus_helpers'
+
+using_task_library 'motors_weg_cvw300'
+
+describe OroGen.motors_weg_cvw300.Task do
+ run_live
+
+ attr_reader :task
+ attr_reader :reader
+
+ include ModbusHelpers
+
+ before do
+ @task, @reader, @writer = iodrivers_base_prepare(
+ OroGen.motors_weg_cvw300.Task
+ .deployed_as('motors_weg_cvw300_test')
+ )
+
+ @task.properties.io_read_timeout = Time.at(2)
+ @task.properties.modbus_interframe_delay = Time.at(0.01)
+ @task.properties.watchdog do |sw|
+ sw.timeout = Time.at(0.5)
+ sw
+ end
+
+ modbus_set(405, 512) # number encoder pulses
+ modbus_set(401, 10) # nominal current
+ modbus_set(402, 2000) # nominal speed (rpm)
+ modbus_set(404, 2) # 12 kW
+
+ modbus_set(2, 0) # actual rotation (rpm)
+ modbus_set(3, 0) # actual current (A)
+ modbus_set(4, 0) # battery voltage (0.1 V)
+ modbus_set(5, 0) # output frequency (0.1 Hz)
+ modbus_set(6, 1) # inverter state (1 == Run)
+ modbus_set(7, 0) # output voltage (0.1V)
+ modbus_set(8, 0) # not used - vehicle speed
+ modbus_set(9, 0) # motor torque (0.1 % torque nominal)
+
+ modbus_set(30, 0) # mosfet temperature (0.1 C)
+ modbus_set(34, 0) # air temperature (0.1 C)
+ end
+
+ after do
+ modbus_stop_task if task.running?
+ end
+
+ describe 'configuration' do
+ it 'sets the control type' do
+ task.properties.control_type = :CONTROL_SENSORLESS
+ modbus_configure_and_start
+ assert_equal 1, modbus_get(202)
+ end
+
+ it 'sets the ramps' do
+ task.properties.ramps = {
+ acceleration_time: Time.at(5.1),
+ deceleration_time: Time.at(7.2),
+ type: 'RAMP_S_CURVE'
+ }
+ modbus_configure_and_start
+ assert_equal 5, modbus_get(100)
+ assert_equal 7, modbus_get(101)
+ assert_equal 1, modbus_get(104)
+ end
+
+ it 'sets the speed limits' do
+ rpm100 = 100 * 2 * Math::PI / 60
+ task.properties.limits = Types.base.JointLimits.new(
+ elements: [{
+ min: Types.base.JointState.Speed(-rpm100),
+ max: Types.base.JointState.Speed(rpm100)
+ }]
+ )
+
+ modbus_configure_and_start
+
+ assert_equal 100, modbus_get(134)
+ end
+ end
+
+ describe 'command watchdog' do
+ it 'periodically sends zero velocity commands' do
+ @task.properties.watchdog do |sw|
+ sw.timeout = Time.at(0.01)
+ sw
+ end
+ modbus_configure_and_start
+ 10.times do
+ modbus_reply_until(@writer, @reader) do |sample|
+ modbus_write?(sample, register: 683, value: 0)
+ end
+ end
+ end
+
+ it 'waits watchdog.timeout after a new command before it sends a new velocity command again' do
+ modbus_configure_and_start
+
+ cmd = Types.base.samples.Joints.new(
+ elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+ )
+ syskit_write task.cmd_in_port, cmd
+ modbus_reply_until(@writer, @reader) do |sample|
+ # Note: register 683 is expressed in ratio of nominal speed,
+ # with 0x2000 (8192) == 100%
+ modbus_write?(sample, register: 683, value: 0x1000)
+ end
+ tic = Time.now
+ modbus_reply_until(@writer, @reader) do |sample|
+ modbus_write?(sample, register: 683, value: 0)
+ end
+ assert((Time.now - tic) > 0.4)
+ end
+
+ it 'does not send zero velocity commnds if new commands are regularly sent' do
+ @task.properties.watchdog do |sw|
+ sw.timeout = Time.at(0.1)
+ sw
+ end
+ modbus_configure_and_start
+
+ cmd = Types.base.samples.Joints.new(
+ elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+ )
+
+ 10.times do
+ syskit_write task.cmd_in_port, cmd
+ modbus_reply_until(@writer, @reader) do |sample|
+ # Note: register 683 is expressed in ratio of nominal speed,
+ # with 0x2000 (8192) == 100%
+ if modbus_write?(sample, register: 683, value: 0)
+ flunk('received zero velocity command')
+ else
+ modbus_write?(sample, register: 683, value: 0x1000)
+ sleep 0.02
+ end
+ end
+ end
+ end
+ end
+end