Disable ic error check, send another point on init

roboticslab-uc3m · Jul 19, 2019 · 08d82af · 08d82af
1 parent b0a36cf
commit 08d82af
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 40 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp
@@ -175,7 +175,7 @@ bool roboticslab::TechnosoftIpos::setPositionDirectModeRaw()
     //-- 11. Interpolated position buffer configuration. By setting the value A001 h , the buffer is
     //-- cleared and the integrity counter will be set to 1. Send the following message (SDO
     //-- access to object 2074 h , 16-bit value C h ):
-    uint8_t buffConf[]= {0x2B,0x74,0x20,0x00,0x01,0xA0,0x00,0x00};
+    uint8_t buffConf[]= {0x2B,0x74,0x20,0x00,0x01,0xC0,0x00,0x00};
     if ( ! send(0x600,8,buffConf) )
         return false;
     //*************************************************************
@@ -199,23 +199,24 @@ bool roboticslab::TechnosoftIpos::setPositionDirectModeRaw()
     pvtThread->setInitialPose(ref);
     pvtThread->updateTarget(ref);
     pvtThread->step();
+    pvtThread->step();
 
-    uint8_t startPT[]= {0x1F,0x00};
-
-    if( ! send(0x200,2,startPT) )
+    if (!pvtThread->start())
     {
-        CD_ERROR("Could not send \"startPT\". %s\n", msgToStr(0x200,2,startPT).c_str() );
+        CD_ERROR("Unable to start PVT thread.\n");
         return false;
     }
-    CD_SUCCESS("Sent \"startPT\". %s\n", msgToStr(0x200,2,startPT).c_str() );
 
     yarp::os::Time::delay(PVT_MODE_MS * 0.001 / 2);
 
-    if (!pvtThread->start())
+    uint8_t startPT[]= {0x1F,0x00};
+
+    if( ! send(0x200,2,startPT) )
     {
-        CD_ERROR("Unable to start PVT thread.\n");
+        CD_ERROR("Could not send \"startPT\". %s\n", msgToStr(0x200,2,startPT).c_str() );
         return false;
     }
+    CD_SUCCESS("Sent \"startPT\". %s\n", msgToStr(0x200,2,startPT).c_str() );
 
     return true;
 }

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp
@@ -11,38 +11,6 @@ bool roboticslab::TechnosoftIpos::setPositionRaw(int j, double ref)
     //-- Check index within range
     if ( j != 0 ) return false;
 
-    //*************************************************************
-    //-- 13. Send the 1 st PT point.
-    //-- Position= 20000 IU (0x00004E20)
-    //-- Time = 1000 IU (0x03E8)
-    //-- IC = 1 (0x01)
-    //-- 1IU = 1 encoder pulse
-    //-- 1IU = 1 control loop = 1ms by default
-    //-- IC=Integrity Counter
-    //-- The drive motor will do 10 rotations (20000 counts) in 1000 milliseconds.
-    //-- Send the following message:
-    //uint8_t ptpoint1[]={0x20,0x4E,0x00,0x00,0xE8,0x03,0x00,0x02};
-    if (std::abs(ref - lastPtRef) > maxPtDistance)
-    {
-        CD_WARNING("Max velocity exceeded, clipping travelled distance.\n");
-        ref = lastPtRef + maxPtDistance * (ref >= lastPtRef ? 1 : -1);
-        CD_INFO("New ref: %f.\n", ref);
-    }
-    uint8_t msg_ptPoint[8];
-    int32_t position = ref * this->tr * (encoderPulses / 360.0); // Apply tr & convert units to encoder increments
-    memcpy(msg_ptPoint+0,&position,4);
-    memcpy(msg_ptPoint+4,&ptModeMs,2);
-    uint8_t ic = (++pvtPointCounter) << 1;
-    memcpy(msg_ptPoint+7,&ic,1);
-    yarp::os::Time::delay(0.001); // 0.01 okay for 1 arm (6 motors), had to reduce to 0.0075 for two arms (12 motors)
-    if ( ! send(0x400,8,msg_ptPoint) )
-    {
-        CD_ERROR("msg_ptPoint in %d",canId);
-        return false;
-    }
-    CD_SUCCESS("Sent to canId %d: pos %f, time %d, ic %d.\n",canId,ref,ptModeMs,ic);
-    lastPtRef = ref;
-
     pvtThread->updateTarget(ref);
 
     return true;