Clamp spnav values

roboticslab-uc3m · Jun 8, 2024 · ac8c342 · ac8c342
1 parent f11c1c5
commit ac8c342
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 43 deletions.
diff --git a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp
@@ -2,46 +2,17 @@
 
 #include "SpaceNavigator.hpp"
 
-using namespace roboticslab;
+#include <algorithm> // std::clamp
 
-// -----------------------------------------------------------------------------
+using namespace roboticslab;
 
-SpaceNavigator::SpaceNavigator()
-    : dx(0.0), dy(0.0), dz(0.0),
-      droll(0.0), dpitch(0.0), dyaw(0.0),
-      button1(0), button2(0),
-      noDataCounter(0), deadband(0.0)
-{ }
-
-// -----------------------------------------------------------------------------
-
-SpaceNavigator::~SpaceNavigator()
-{
-    close();
-}
+constexpr auto RANGE = 1.0;
 
 // -----------------------------------------------------------------------------
 
 double SpaceNavigator::enforceRange(double in)
 {
-    double out;
-
-    if (in > 1)
-    {
-        out = 1;
-    }
-    else
-    {
-        if (in < -1)
-        {
-            out = -1;
-        }
-        else
-        {
-            out = in;
-        }
-    }
-    return out;
+    return std::clamp(in, -RANGE, RANGE);
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.hpp b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.hpp
@@ -35,11 +35,11 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
 {
 public:
 
-    SpaceNavigator();
-    ~SpaceNavigator() override;
+    ~SpaceNavigator() override
+    { close(); }
 
     //  --------- DeviceDriver Declarations. Implementation in DeviceDriverImpl.cpp ---------
-    bool open(yarp::os::Searchable& config) override;
+    bool open(yarp::os::Searchable & config) override;
     bool close() override;
 
     //  --------- IAnalogSensor Declarations. Implementation in IAnalogSensorImpl.cpp ---------
@@ -48,7 +48,7 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
      * @param out a vector containing the sensor's last readings.
      * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
      */
-    int read(yarp::sig::Vector &out) override;
+    int read(yarp::sig::Vector & out) override;
 
     /**
      * Check the state value of a given channel.
@@ -74,7 +74,7 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
      * @param value a vector of calibration values.
      * @return status.
      */
-    int calibrateSensor(const yarp::sig::Vector& value) override;
+    int calibrateSensor(const yarp::sig::Vector & value) override;
 
     /**
      * Calibrates one single channel.
@@ -101,11 +101,19 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
 
 private:
 
-    double dx, dy, dz;
-    double droll, dpitch, dyaw;
-    int button1, button2;
-    unsigned int noDataCounter;
-    double deadband;
+    double dx {0.0};
+    double dy {0.0};
+    double dz {0.0};
+
+    double droll {0.0};
+    double dpitch {0.0};
+    double dyaw {0.0};
+
+    int button1 {0};
+    int button2 {0};
+
+    unsigned int noDataCounter {0};
+    double deadband {0.0};
 };
 
 } // namespace roboticslab