fixed joint limits problem

Closes #213.
roboticslab-uc3m · May 22, 2019 · 9600e49 · 9600e49
1 parent d7773a1
commit 9600e49
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlLimitsRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlLimitsRawImpl.cpp
@@ -23,8 +23,9 @@ bool roboticslab::TechnosoftIpos::setMinLimitRaw(double min)
 {
     //*************************************************************
     uint8_t msg_position_min[]= {0x23,0x7D,0x60,0x01,0x00,0x00,0x00,0x00}; // 0x01 is subindex 1, Manual 607Dh: Software position limit
+    if(this->tr < 0) msg_position_min[3] = 0x02;
 
-    int sendMin = min * std::abs(this->tr) * (encoderPulses / 360.0);  // Appply tr & convert units to encoder increments
+    int sendMin = min * this->tr * (encoderPulses / 360.0);  // Appply tr & convert units to encoder increments
     memcpy(msg_position_min+4,&sendMin,4);
 
     if( ! send( 0x600, 8, msg_position_min ) )
@@ -47,8 +48,9 @@ bool roboticslab::TechnosoftIpos::setMaxLimitRaw(double max)
 {
     //*************************************************************
     uint8_t msg_position_max[]= {0x23,0x7D,0x60,0x02,0x00,0x00,0x00,0x00}; // 0x02 is subindex 2, Manual 607Dh: Software position limit
+    if(this->tr < 0) msg_position_max[3] = 0x01;
 
-    int sendMax = max * std::abs(this->tr) * (encoderPulses / 360.0);  // Appply tr & convert units to encoder increments
+    int sendMax = max * this->tr * (encoderPulses / 360.0);  // Appply tr & convert units to encoder increments
     memcpy(msg_position_max+4,&sendMax,4);
 
     if( ! send( 0x600, 8, msg_position_max ) )