Transform Lacquey into a PWM-controlled device

libraries/YarpPlugins/LacqueyFetch/CMakeLists.txt

@@ -13,20 +13,11 @@ if(NOT SKIP_LacqueyFetch)
 
     set(CMAKE_INCLUDE_CURRENT_DIR TRUE) # yarp plugin builder needs this
 
-    yarp_add_plugin(LacqueyFetch LacqueyFetch.cpp
-                                 LacqueyFetch.hpp
+    yarp_add_plugin(LacqueyFetch LacqueyFetch.hpp
                                  DeviceDriverImpl.cpp
                                  ICanBusSharerImpl.cpp
-                                 IControlLimitsRawImpl.cpp
                                  IControlModeRawImpl.cpp
-                                 IEncodersRawImpl.cpp
-                                 IEncodersTimedRawImpl.cpp
-                                 IInteractionModeRawImpl.cpp
-                                 IPositionControlRawImpl.cpp
-                                 IPositionDirectRawImpl.cpp
-                                 IRemoteVariablesRawImpl.cpp
-                                 ITorqueControlRawImpl.cpp
-                                 IVelocityControlRawImpl.cpp)
+                                 IPWMControlRawImpl.cpp)
 
     target_link_libraries(LacqueyFetch YARP::YARP_OS
                                        YARP::YARP_dev

libraries/YarpPlugins/LacqueyFetch/DeviceDriverImpl.cpp

@@ -2,23 +2,22 @@
 
 #include "LacqueyFetch.hpp"
 
+#include <ColorDebug.h>
+
+using namespace roboticslab;
+
 // -----------------------------------------------------------------------------
-bool roboticslab::LacqueyFetch::open(yarp::os::Searchable& config)
+
+bool LacqueyFetch::open(yarp::os::Searchable& config)
 {
-    this->canId = config.check("canId",yarp::os::Value(0),"can bus ID").asInt32();
-    this->tr = config.check("tr",yarp::os::Value(0),"reduction").asInt32();
-    this->max = config.check("max",yarp::os::Value(0),"max").asFloat64();
-    this->min = config.check("min",yarp::os::Value(0),"min").asFloat64();
-    this->refAcceleration = 0;
-    this->refSpeed = 0;
-    this->encoder = 0;
-
-    CD_SUCCESS("Created LacqueyFetch with canId %d and tr %f, and all local parameters set to 0.\n",canId,tr);
+    canId = config.check("canId", yarp::os::Value(0), "can bus ID").asInt8();
+    CD_SUCCESS("Created LacqueyFetch with canId %d.\n", canId);
     return true;
 }
 
 // -----------------------------------------------------------------------------
-bool roboticslab::LacqueyFetch::close()
+
+bool LacqueyFetch::close()
 {
     CD_INFO("\n");
     return true;

libraries/YarpPlugins/LacqueyFetch/ICanBusSharerImpl.cpp

@@ -2,65 +2,60 @@
 
 #include "LacqueyFetch.hpp"
 
+using namespace roboticslab;
+
 // -----------------------------------------------------------------------------
 
-unsigned int roboticslab::LacqueyFetch::getId()
+unsigned int LacqueyFetch::getId()
 {
     return canId;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::start()
+bool LacqueyFetch::start()
 {
-
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::readyToSwitchOn()
+bool LacqueyFetch::readyToSwitchOn()
 {
-
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::switchOn()
+bool LacqueyFetch::switchOn()
 {
-
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::enable()
+bool LacqueyFetch::enable()
 {
-
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::recoverFromError()
+bool LacqueyFetch::recoverFromError()
 {
-
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::interpretMessage(const yarp::dev::CanMessage & message)
+bool LacqueyFetch::interpretMessage(const yarp::dev::CanMessage & message)
 {
-
     return true;
-
-}  //-- ends interpretMessage
+}
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::registerSender(CanSenderDelegate * sender)
+bool LacqueyFetch::registerSender(CanSenderDelegate * sender)
 {
     this->sender = sender;
     return true;

libraries/YarpPlugins/LacqueyFetch/IControlModeRawImpl.cpp

@@ -2,70 +2,59 @@
 
 #include "LacqueyFetch.hpp"
 
-#include <yarp/os/Vocab.h>
+#include <ColorDebug.h>
 
-// ############################## IControlModeRaw Related ##############################
+using namespace roboticslab;
 
-bool roboticslab::LacqueyFetch::getControlModeRaw(int j, int *mode)
-{
-    //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    *mode = VOCAB_CM_POSITION;
+// -----------------------------------------------------------------------------
 
+bool LacqueyFetch::getControlModeRaw(int j, int * mode)
+{
+    //CD_DEBUG("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream
+    CHECK_JOINT(j);
+    *mode = VOCAB_CM_PWM;
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getControlModesRaw(int *modes)
+bool LacqueyFetch::getControlModesRaw(int * modes)
 {
-    CD_ERROR("Missing implementation\n");
+    CD_DEBUG("\n");
     return false;
 }
 
-// ############################## IControlMode2Raw Related ##############################
+// -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getControlModesRaw(const int n_joint, const int *joints, int *modes)
+bool LacqueyFetch::getControlModesRaw(const int n_joint, const int * joints, int * modes)
 {
-    CD_DEBUG("\n");
-
-    //-- Check array size
-    if ( n_joint != 1 ) return false;
-
+    //CD_DEBUG("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream
     return getControlModeRaw(0, &modes[0]);
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setControlModeRaw(const int j, const int mode)
+bool LacqueyFetch::setControlModeRaw(const int j, const int mode)
 {
-    CD_DEBUG("(%d, %s)\n", j, yarp::os::Vocab::decode(mode).c_str());
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
+    CHECK_JOINT(j);
+    CD_WARNING("Not supported.\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setControlModesRaw(const int n_joint, const int *joints, int *modes)
+bool LacqueyFetch::setControlModesRaw(const int n_joint, const int * joints, int * modes)
 {
-    CD_DEBUG("(%d)\n",n_joint);
-
-    //-- Check array size
-    if ( n_joint != 1 ) return false;
-
-    return setControlModeRaw(0, modes[0]);
+    CD_WARNING("Not supported.\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setControlModesRaw(int *modes)
+bool LacqueyFetch::setControlModesRaw(int * modes)
 {
-    CD_DEBUG("\n");
-    return setControlModeRaw(0, modes[0]);
+    CD_WARNING("Not supported.\n");
+    return false;
 }
+
+// -----------------------------------------------------------------------------