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