Merge branch 'issue-176-dextra' into develop

libraries/YarpPlugins/CMakeLists.txt

@@ -30,6 +30,7 @@ add_subdirectory(CanBusHico)
 add_subdirectory(CanBusPeak)
 add_subdirectory(CanBusSocket)
 add_subdirectory(CuiAbsolute)
+add_subdirectory(DextraControlboardUSB)
 add_subdirectory(FakeControlboard)
 add_subdirectory(FakeJoint)
 add_subdirectory(Jr3)

libraries/YarpPlugins/DextraControlboardUSB/CMakeLists.txt

@@ -0,0 +1,43 @@
+# Copyright: (C) 2019 Universidad Carlos III de Madrid
+# Author: Juan G. Victores & Jennifer Joana Gago Muñoz
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+yarp_prepare_plugin(DextraControlboardUSB
+                    CATEGORY device
+                    TYPE roboticslab::DextraControlboardUSB
+                    INCLUDE DextraControlboardUSB.hpp
+                    DEFAULT ON
+                    DEPENDS UNIX)
+
+if(NOT SKIP_DextraControlboardUSB)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # yarp plugin builder needs this
+
+yarp_add_plugin(DextraControlboardUSB DextraControlboardUSB.cpp
+                                      DextraControlboardUSB.hpp
+                                      DeviceDriverImpl.cpp
+                                      IControlLimits2Impl.cpp
+                                      IControlMode2Impl.cpp
+                                      IEncodersImpl.cpp
+                                      IEncodersTimedImpl.cpp
+                                      IInteractionModeImpl.cpp
+                                      IPositionControl2Impl.cpp
+                                      IPositionDirectImpl.cpp
+                                      ITorqueControlImpl.cpp
+                                      IVelocityControl2Impl.cpp)
+
+target_link_libraries(DextraControlboardUSB YARP::YARP_OS
+                                            YARP::YARP_dev
+                                            ROBOTICSLAB::ColorDebug
+                                            YarpDevicesInterfaces)
+
+yarp_install(TARGETS DextraControlboardUSB
+             COMPONENT runtime
+             LIBRARY DESTINATION ${ROBOTICSLAB-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR}
+             ARCHIVE DESTINATION ${ROBOTICSLAB-YARP-DEVICES_STATIC_PLUGINS_INSTALL_DIR})
+
+yarp_install(FILES DextraControlboardUSB.ini
+             COMPONENT runtime
+             DESTINATION ${ROBOTICSLAB-YARP-DEVICES_PLUGIN_MANIFESTS_INSTALL_DIR})
+
+endif()

libraries/YarpPlugins/DextraControlboardUSB/DeviceDriverImpl.cpp

@@ -0,0 +1,49 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "DextraControlboardUSB.hpp"
+
+// -----------------------------------------------------------------------------
+bool roboticslab::DextraControlboardUSB::open(yarp::os::Searchable& config)
+{
+
+    /*this->canId = config.check("canId",0,"can bus ID").asInt();
+    this->tr = config.check("tr",0,"reduction").asInt();
+    this->ptModeMs  = config.check("ptModeMs",0,"ptMode ms").asInt();
+    this->ptPointCounter = 0;
+    this->ptMovementDone = false;
+    this->targetReached = false;
+    this->max = 0;
+    this->min = 0;
+    this->refAcceleration = 0;
+    this->refSpeed = 0;
+    this->encoder = 0;
+
+    CD_SUCCESS("Created DextraControlboardUSB with canId %d and tr %f, and all local parameters set to 0.\n",canId,tr);
+    */
+
+    char serialport[13] = "/dev/ttyACM0";  // Was /dev/ttyUSB0
+    int baudrate = B115200;  // Should match https://github.com/Alvipe/Dextra/blob/master/Control/DextraControl.py
+    char buf[256];
+    int rc,n;
+
+    fd = serialport_init(serialport, baudrate);
+
+    if ( fd <= 0 )
+    {
+        CD_ERROR("Could not open %s (fd = %d <= 0). Bye!\n",serialport,fd);
+        return false;
+    }
+
+    CD_SUCCESS("Opened %s (fd: %d)\n",serialport,fd);
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+bool roboticslab::DextraControlboardUSB::close()
+{
+    CD_INFO("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/DextraControlboardUSB/DextraControlboardUSB.cpp

@@ -0,0 +1,103 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "DextraControlboardUSB.hpp"
+
+// -----------------------------------------------------------------------------
+
+int roboticslab::DextraControlboardUSB::serialport_writebyte( int fd, uint8_t b)
+{
+    int n = write(fd,&b,1);
+    if( n!=1)
+        return -1;
+    return 0;
+}
+
+// -----------------------------------------------------------------------------
+
+int roboticslab::DextraControlboardUSB::serialport_write(int fd, const char* str)
+{
+    int len = strlen(str);
+    int n = write(fd, str, len);
+    if( n!=len )
+        return -1;
+    return 0;
+}
+
+// -----------------------------------------------------------------------------
+
+int roboticslab::DextraControlboardUSB::serialport_read_until(int fd, char* buf, char until)
+{
+    char b[1];
+    int i=0;
+    do
+    {
+        int n = read(fd, b, 1);  // read a char at a time
+        if( n==-1) return -1;    // couldn't read
+        if( n==0 )
+        {
+            usleep( 10 * 1000 ); // wait 10 msec try again
+            continue;
+        }
+        buf[i] = b[0];
+        i++;
+    }
+    while( b[0] != until );
+
+    buf[i] = 0;  // null terminate the string
+    return 0;
+}
+
+// -----------------------------------------------------------------------------
+
+int roboticslab::DextraControlboardUSB::serialport_init(const char* serialport, int baud)
+{
+    struct termios toptions;
+    int fd;
+
+    //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n",
+    //        serialport,baud);
+
+    fd = ::open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd == -1)
+    {
+        perror("init_serialport: Unable to open port ");
+        return -1;
+    }
+
+    if (tcgetattr(fd, &toptions) < 0)
+    {
+        perror("init_serialport: Couldn't get term attributes");
+        return -1;
+    }
+    speed_t brate = baud; // let you override switch below if needed
+    cfsetispeed(&toptions, brate);
+    cfsetospeed(&toptions, brate);
+
+    // 8N1
+    toptions.c_cflag &= ~PARENB;
+    toptions.c_cflag &= ~CSTOPB;
+    toptions.c_cflag &= ~CSIZE;
+    toptions.c_cflag |= CS8;
+    // no flow control
+    toptions.c_cflag &= ~CRTSCTS;
+
+    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
+    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
+
+    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
+    toptions.c_oflag &= ~OPOST; // make raw
+
+    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
+    toptions.c_cc[VMIN]  = 0;
+    toptions.c_cc[VTIME] = 20;
+
+    if( tcsetattr(fd, TCSANOW, &toptions) < 0)
+    {
+        perror("init_serialport: Couldn't set term attributes");
+        return -1;
+    }
+
+    return fd;
+}
+
+// -----------------------------------------------------------------------------