Substitute motor encoder position with raw trajectory in raw values

robotology · Oct 28, 2024 · 3e3ce83 · 3e3ce83
1 parent 48349c0
commit 3e3ce83
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c
@@ -1669,7 +1669,7 @@ static eOresult_t s_eo_motioncontrol_updatedPositionsFromEncoders(EOtheMotionCon
         if(NULL != (jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), i)))
         {
             jstatus->addinfo.multienc[0] = rawValsArray.rawvalues[0].val;
-            jstatus->addinfo.multienc[1] = rawValsArray.rawvalues[1].val;
+            //jstatus->addinfo.multienc[1] = rawValsArray.rawvalues[1].val; for now do not update the raw value for the motor encoder since we are adding to it the raw planned target --> see lines 292..294 in Joint.c
             jstatus->addinfo.multienc[2] = rawValsArray.rawvalues[0].diagnInfo;
 
         }

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
@@ -288,7 +288,10 @@ void Joint_update_status_reference(Joint* o)
         default:
             ;
     }
-
+
+    // Debug for temporary adding the raw planned target position to the motor encoder value which
+    o->eo_joint_ptr->status.addinfo.multienc[1] = (int32_t)Trajectory_get_target_position(&(o->trajectory));
+    //ends here
 }
 
 static void Joint_send_debug_message(const char *message, uint8_t jid, uint16_t par16, uint64_t par64)