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],¤t_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],¤t_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