Add --movi support (WiimoteSensor)

roboticslab-uc3m · May 16, 2019 · e4dc75b · e4dc75b
1 parent b23dfec
commit e4dc75b
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 14 deletions.
diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp
@@ -31,7 +31,7 @@ StreamingDevice * StreamingDeviceFactory::makeDevice(const std::string & deviceN
     }
     else if (deviceName == "WiimoteSensor")
     {
-        return new WiimoteSensorDevice(deviceConfig);
+        return new WiimoteSensorDevice(deviceConfig, usingMovi);
     }
     else
     {

diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.cpp b/programs/streamingDeviceController/WiimoteSensorDevice.cpp
@@ -7,6 +7,18 @@
 
 #include <ColorDebug.h>
 
+roboticslab::WiimoteSensorDevice::WiimoteSensorDevice(yarp::os::Searchable & config, bool usingMovi)
+    : StreamingDevice(config),
+      iAnalogSensor(NULL),
+      mode(NONE),
+      usingMovi(usingMovi),
+      step(0.0)
+{
+    data.resize(3);  // already called by base constructor
+    buffer.resize(5);
+    step = config.check("step", yarp::os::Value(DEFAULT_STEP), "").asDouble();
+}
+
 bool roboticslab::WiimoteSensorDevice::acquireInterfaces()
 {
     bool ok = true;
@@ -22,12 +34,23 @@ bool roboticslab::WiimoteSensorDevice::acquireInterfaces()
 
 bool roboticslab::WiimoteSensorDevice::initialize(bool usingStreamingPreset)
 {
-    if (usingStreamingPreset && !iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING, VOCAB_CC_TWIST))
+    if (usingMovi && step <= 0.0)
     {
-        CD_WARNING("Unable to preset streaming command.\n");
+        CD_WARNING("Invalid step: %f.\n", step);
         return false;
     }
 
+    if (usingStreamingPreset)
+    {
+        int cmd = usingMovi ? VOCAB_CC_MOVI : VOCAB_CC_TWIST;
+
+        if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING, cmd))
+        {
+            CD_WARNING("Unable to preset streaming command.\n");
+            return false;
+        }
+    }
+
     if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_FRAME, ICartesianSolver::TCP_FRAME))
     {
         CD_WARNING("Unable to set TCP reference frame.\n");
@@ -123,7 +146,14 @@ void roboticslab::WiimoteSensorDevice::sendMovementCommand()
         return;
     }
 
-    iCartesianControl->twist(xdot);
+    if (usingMovi)
+    {
+        iCartesianControl->movi(xdot);
+    }
+    else
+    {
+        iCartesianControl->twist(xdot);
+    }
 }
 
 void roboticslab::WiimoteSensorDevice::stopMotion()

diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.hpp b/programs/streamingDeviceController/WiimoteSensorDevice.hpp
@@ -24,16 +24,7 @@ class WiimoteSensorDevice : public StreamingDevice
 public:
 
     //! Constructor
-    WiimoteSensorDevice(yarp::os::Searchable & config)
-        : StreamingDevice(config),
-          iAnalogSensor(NULL),
-          mode(NONE),
-          step(0.0)
-    {
-        data.resize(3);  // already called by base constructor
-        buffer.resize(5);
-        step = config.check("step", yarp::os::Value(DEFAULT_STEP), "").asDouble();
-    }
+    WiimoteSensorDevice(yarp::os::Searchable & config, bool usingMovi);
 
     virtual bool acquireInterfaces();
 
@@ -59,6 +50,7 @@ class WiimoteSensorDevice : public StreamingDevice
 
     std::vector<double> buffer;
 
+    bool usingMovi;
     double step;
 };
-Original file line number
+Diff line change
@@ Expand Up @@
         }
         else if (deviceName == "WiimoteSensor")
         {
-            return new WiimoteSensorDevice(deviceConfig);
+            return new WiimoteSensorDevice(deviceConfig, usingMovi);
         }
         else
         {
@@ Expand Down @@