From 2965b687049dedb024a15e88a8b2a5f6ab28ba59 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 4 May 2023 15:08:47 +0200
Subject: [PATCH] Added the implementation of the moving status register.

---
 src/dynamixel_servo.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 3bba671..2e53d92 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -807,6 +807,7 @@ namespace dynamixel_robot_gazebo
       }
       if(update)
       {
+        this->memory[123]=0x03;
         this->trajectory.clear();
         this->traj_index=0;
         if((!this->is_time_based() && speed!=0.0 && accel!=0.0) || (this->is_time_based() && traj_time!=0.0 && accel_time!=0.0))
@@ -859,6 +860,7 @@ namespace dynamixel_robot_gazebo
       }
       if(update)
       {
+        this->memory[123]=0x03;
         this->trajectory.clear();
         this->traj_index=0;
         if(speed!=0.0)
@@ -895,6 +897,10 @@ namespace dynamixel_robot_gazebo
     {
       this->traj_access.enter();
       target_angle=this->trajectory[this->traj_index];
+      if(fabs(this->get_target_angle()-real_angle)>0.02)
+        this->memory[123]=0x03;
+      else
+        this->memory[123]=0x00;
       if(this->pid!=NULL)
         command=this->pid->computeCommand(target_angle-real_angle,period);
       else if(this->compliance!=NULL)
@@ -908,6 +914,10 @@ namespace dynamixel_robot_gazebo
     else
     {
       target_pwm=this->get_target_pwm();
+      if(fabs(target_pwm)>0.1)
+        this->memory[123]=0x03;
+      else
+        this->memory[123]=0x00;
       command=this->saturate_command(target_pwm);
       this->joint.setCommand(command);
     }
-- 
GitLab