From 420356c8f3fbb7b7564b4c089210de125203a8ce Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 17 Sep 2024 15:42:28 +0200
Subject: [PATCH] Implemented also the moving register.
---
src/dynamixel_servo.cpp | 14 ++++++++++++++
1 file changed, 14 insertions(+)
diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index f6b0478..d8602b5 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -839,6 +839,7 @@ namespace dynamixel_robot_gazebo
if(update)
{
this->memory[123]=0x03;
+ this->memory[122]=0x01;
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))
@@ -892,6 +893,7 @@ namespace dynamixel_robot_gazebo
if(update)
{
this->memory[123]=0x03;
+ this->memory[122]=0x01;
this->trajectory.clear();
this->traj_index=0;
if(speed!=0.0)
@@ -929,9 +931,15 @@ 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;
+ this->memory[122]=0x01;
+ }
else
+ {
this->memory[123]=0x00;
+ this->memory[122]=0x00;
+ }
if(this->pid!=NULL)
command=this->pid->computeCommand(target_angle-real_angle,period);
else if(this->compliance!=NULL)
@@ -946,9 +954,15 @@ namespace dynamixel_robot_gazebo
{
target_pwm=this->get_target_pwm();
if(fabs(target_pwm)>0.1)
+ {
this->memory[123]=0x03;
+ this->memory[122]=0x01;
+ }
else
+ {
this->memory[123]=0x00;
+ this->memory[122]=0x00;
+ }
command=this->saturate_command(target_pwm);
this->joint.setCommand(command);
}
--
GitLab