Fixed DIOs settup in Drive.

Author: vcrocher

CMakeLists.txt

@@ -55,7 +55,11 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 if(USE_ROS)
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context" )
 else()
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context -static-libstdc++ -static-libgcc" )
+    if(CMAKE_CROSSCOMPILING)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context -static" )
+    else()
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context" )
+    endif()
 endif()
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 set(THREADS_PREFER_PTHREAD_FLAG TRUE)

script/uploadBB.sh

@@ -21,8 +21,8 @@ fi
 #Check if BB is connected
 ping -W 1 -c 1 $SSH_IP_ADDR >/dev/null 2>&1
 if [ $? -ne 0 ] ; then 
-	echo "Nothing connected on ${SSH_IP_ADDR}. Exiting."
-	exit
+    echo "Nothing connected on ${SSH_IP_ADDR}. Exiting."
+    exit
 fi
 
 #set in root folder

src/core/robot/Drive.cpp

@@ -208,10 +208,10 @@ bool Drive::initPDOs() {
 
     // Calculate COB_ID. If RPDO:
     //int COB_ID = 0x100 * (PDO_Num+1) + NodeID;
-    spdlog::debug("Set up CONTROL_WORD RPDO on Node {}", NodeID);
+    spdlog::debug("Set up CONTROL_WORD and DIGITAL_OUT RPDO on Node {}", NodeID);
     int RPDO_Num = 1;
     if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
-        spdlog::error("Set up CONTROL_WORD RPDO FAILED on node {}", NodeID);
+        spdlog::error("Set up CONTROL_WORD and DIGITAL_OUT RPDO FAILED on node {}", NodeID);
         return false;
     }
     spdlog::debug("Set up TARGET_POS RPDO on Node {}", NodeID);

src/core/robot/Drive.h

@@ -231,10 +231,10 @@ class Drive {
     std::vector<std::string> generateTorqueControlConfigSDO();
 
     /**
-        * \brief Send a list (vector) of properly formatted SDO Messages
-        *
-        * \return int -number_of_unsuccesfull messages (0 means OK for all). vcan will always return 0 (no reply check).
-              */
+     * \brief Send a list (vector) of properly formatted SDO Messages
+     *
+     * \return int -number_of_unsuccesfull messages (0 means OK for all). vcan will always return 0 (no reply check).
+     */
     int sendSDOMessages(std::vector<std::string> messages);
 
 
@@ -295,8 +295,8 @@ class Drive {
         {TARGET_POS, 4},
         {TARGET_VEL, 4},
         {TARGET_TOR, 2},
-        {DIGITAL_IN, 2},
-        {DIGITAL_OUT, 2}};
+        {DIGITAL_IN, 4},
+        {DIGITAL_OUT, 4}};
 
     /**
      * \brief Map between the Commonly-used OD entries and their addresses and sub-index - used to generate PDO Configurations
@@ -305,16 +305,16 @@ class Drive {
      */
     std::map<OD_Entry_t, std::array<int, 2>> OD_Addresses = {
         {STATUS_WORD, {0x6041, 0x00}},
-        {ERROR_WORD, {0x2601, 0x00}},
+        {ERROR_WORD, {0x603F, 0x00}},
         {ACTUAL_POS, {0x6064, 0x00}},
         {ACTUAL_VEL, {0x606C, 0x00}},
         {ACTUAL_TOR, {0x6077, 0x00}},
         {CONTROL_WORD, {0x6040, 0x00}},
         {TARGET_POS, {0x607A, 0x00}},
         {TARGET_VEL, {0x60FF, 0x00}},
         {TARGET_TOR, {0x6071, 0x00}},
-        {DIGITAL_IN, {0x2501, 0x00}},
-        {DIGITAL_OUT, {0x2601, 0x00}}};
+        {DIGITAL_IN, {0x60FD, 0x00}},
+        {DIGITAL_OUT, {0x60FE, 0x01}}};
 
     std::map<OD_Entry_t, void *> OD_MappedObjectAddresses = {
         {STATUS_WORD, (void *)&statusWord},
@@ -343,8 +343,8 @@ class Drive {
     INTEGER32 targetPos=0;
     INTEGER32 targetVel=0;
     INTEGER16 targetTor=0;
-    UNSIGNED16 digitalIn=0;
-    UNSIGNED16 digitalOut=0;
+    UNSIGNED32 digitalIn=0;
+    UNSIGNED32 digitalOut=0;
     /**
      * \brief Current error state of the drive
      *

src/hardware/drives/CopleyDrive.cpp

@@ -7,7 +7,8 @@
 #include <iostream>
 
 CopleyDrive::CopleyDrive(int NodeID) : Drive::Drive(NodeID) {
-    this->NodeID = NodeID;
+    OD_Addresses[DIGITAL_IN] = {0x219A, 0x00}; //Use INPUT PIN STATE instead of standard DI. Not tested.
+    OD_Addresses[DIGITAL_OUT] = {0X2194, 0x00}; //Not tested. Need dedicated configuration before use. See Copley doc.
 }
 CopleyDrive::~CopleyDrive() {
     spdlog::debug("CopleyDrive Deleted");
@@ -97,7 +98,6 @@ bool CopleyDrive::setPositionOffset(int offset) {
 
 }
 
-
 bool CopleyDrive::setTrackingWindow(INTEGER32 window) {
     spdlog::debug("NodeID {} Tracking Window", NodeID);
 

src/hardware/drives/KincoDrive.cpp

@@ -6,7 +6,8 @@ KincoDrive::KincoDrive(int NodeID) : Drive::Drive(NodeID) {
     //Remap torque reading and writting registers
     OD_Addresses[ACTUAL_TOR] = {0x6078, 0x00};
     OD_Addresses[TARGET_TOR] = {0x60F6, 0x08};
-    //Weird Kinco DIOs addresses
+    //Weird Kinco error and DIOs addresses
+    OD_Addresses[ERROR_WORD] = {0x2601, 0x00};
     OD_Addresses[DIGITAL_IN] = {0x2010, 0x0B};
     OD_Addresses[DIGITAL_OUT] = {0x2010, 0x0E};
 }