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