From 1ab0c27b2209660f0e621d89000be304e9d2439e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Wed, 16 Mar 2016 12:26:03 +0000
Subject: [PATCH] In the stop() function, if the current position is beyond the
 software position limits, the limits are loaded instead of the current
 position. Both in the dynamixel_motor and the dynamixel_motor_group moudles.

---
 src/dynamixel_motor.cpp       | 6 +++++-
 src/dynamixel_motor_group.cpp | 4 ++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 5b6ca9f..c729a8d 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1202,7 +1202,11 @@ void CDynamixelMotor::stop(void)
       if(this->mode==angle_ctrl)
       {
 	this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_position);
-	this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
+        if(this->to_angles(current_position)>this->config.max_angle)
+          current_position=from_angles(this->config.max_angle);
+        else if(this->to_angles(current_position)<this->config.min_angle)
+          current_position=from_angles(this->config.min_angle);
+        this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
       }
       else
 	this->dynamixel_dev->write_word_register(this->registers[goal_speed],0);
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 1ad63f8..2ff5f4b 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1152,6 +1152,10 @@ void CDynamixelMotorGroup::stop(void)
 	if(this->mode==angle_ctrl)
 	{
 	  this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&current_position);
+          if(current_position>this->to_angles(i,this->config[i].max_angle))
+            current_position=this->from_angles(i,this->config[i].max_angle);
+          else if(current_position<this->to_angles(i,this->config[i].min_angle))
+            current_position=this->from_angles(i,this->config[i].min_angle);
 	  this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos],current_position);
 	}
 	else
-- 
GitLab