Merge pull request #16 from tidewise/saturation_signal

feat: Add output port for the saturation signal

manifest.xml

@@ -8,6 +8,7 @@
   <depend package="drivers/motors_weg_cvw300" />
   <depend package="base/orogen/types" />
   <depend package="drivers/orogen/iodrivers_base" />
+  <depend package="control/orogen/control_base" />
 
   <test_depend package="tools/syskit" />
 </package>

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"
 

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<float>(), base::infinity<float>());
+        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()) {

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

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