diff --git a/manifest.xml b/manifest.xml index b6eab70..bd076ec 100644 --- a/manifest.xml +++ b/manifest.xml @@ -8,6 +8,7 @@ + diff --git a/motors_weg_cvw300.orogen b/motors_weg_cvw300.orogen index 7c58a7f..d6fb75c 100644 --- a/motors_weg_cvw300.orogen +++ b/motors_weg_cvw300.orogen @@ -13,6 +13,8 @@ import_types_from "motors_weg_cvw300/FaultState.hpp" import_types_from "motors_weg_cvw300Types.hpp" using_task_library "iodrivers_base" +using_library "control_base" +import_types_from "control_base" task_context "Task", subclasses: "iodrivers_base::Task" do needs_configuration @@ -83,6 +85,8 @@ task_context "Task", subclasses: "iodrivers_base::Task" do output_port "alarm_state", "/motors_weg_cvw300/AlarmState" output_port "fault_state", "/motors_weg_cvw300/FaultState" + output_port "saturation_signal", "/control_base/SaturationSignal" + exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER", "CONTROLLER_FAULT", "CONTROLLER_UNDER_VOLTAGE" diff --git a/tasks/Task.cpp b/tasks/Task.cpp index e76d27a..4615eaa 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -7,6 +7,7 @@ using namespace std; using namespace base; using namespace motors_weg_cvw300; +using namespace control_base; Task::Task(std::string const& name) : TaskBase(name) @@ -55,10 +56,19 @@ bool Task::configureHook() m_cmd_timeout = wd.timeout; driver->writeControlType(_control_type.get()); - auto limits = _limits.get(); - if (!limits.elements.empty()) { - driver->writeJointLimits(limits.elements.at(0)); + m_limits = _limits.get(); + if (m_limits.elements.empty()) { + // Initialize speed limits as infinity + JointLimits infinity; + JointLimitRange range; + range.Speed(-base::infinity(), base::infinity()); + infinity.elements.push_back(range); + m_limits = infinity; } + else { + driver->writeJointLimits(m_limits.elements.at(0)); + } + driver->writeRampConfiguration(_ramps.get()); m_cmd_in.elements.resize(1); @@ -95,6 +105,12 @@ void Task::writeSpeedCommand(float cmd) m_cmd_deadline = base::Time::now() + m_cmd_timeout; } } +bool Task::checkSpeedSaturation(base::commands::Joints const& cmd) +{ + return cmd.elements[0].speed >= m_limits.elements[0].max.speed || + cmd.elements[0].speed <= m_limits.elements[0].min.speed; +} + void Task::updateHook() { while (_cmd_in.read(m_cmd_in) == RTT::NewData) { @@ -108,6 +124,11 @@ void Task::updateHook() } writeSpeedCommand(joint.speed); + + SaturationSignal saturation_signal; + saturation_signal.value = checkSpeedSaturation(m_cmd_in); + saturation_signal.time = m_cmd_in.time; + _saturation_signal.write(saturation_signal); } if (commandTimedOut()) { diff --git a/tasks/Task.hpp b/tasks/Task.hpp index d0e8a47..5fd83ad 100644 --- a/tasks/Task.hpp +++ b/tasks/Task.hpp @@ -33,6 +33,7 @@ namespace motors_weg_cvw300 { protected: base::commands::Joints m_cmd_in; base::samples::Joints m_sample; + base::JointLimits m_limits; base::Time m_last_temperature_update; base::Time m_cmd_timeout; base::Time m_cmd_deadline; @@ -44,6 +45,7 @@ namespace motors_weg_cvw300 { void writeSpeedCommand(float cmd); bool commandTimedOut() const; void publishFault(); + bool checkSpeedSaturation(base::commands::Joints const& cmd); public: /** TaskContext constructor for Task diff --git a/test/task_test.rb b/test/task_test.rb index 24fe235..fc53295 100644 --- a/test/task_test.rb +++ b/test/task_test.rb @@ -110,6 +110,64 @@ end end + describe "saturation signal reporting" do + before 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) + }] + ) + end + + it "does not output a saturation signal if there is no new command" do + modbus_configure_and_start + modbus_expect_execution(@writer, @reader).to do + have_no_new_sample(task.saturation_signal_port, at_least_during: 0.5) + end + end + + it "outputs a saturated signal if the command is greater than the maximum limit" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(1, signal.value) + end + + it "outputs a saturated signal if the command is less than the minimum limit" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(-1000 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(1, signal.value) + end + + it "outputs an unsaturated signal" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(10 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(0, signal.value) + end + end + describe "fault and alarm reporting" do it "does not output on the fault_state port if there is no alarm or fault " do modbus_configure_and_start