From fc10a2e7d4aaa4ded20ae20dc948beaeac089551 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Wed, 13 May 2015 14:45:11 +0000
Subject: [PATCH] Disabled the servos when changing the control mode to avoid
erratic motions.
---
src/dynamixel_motor.cpp | 17 +++++++++--------
src/dynamixel_motor_group.cpp | 1 +
2 files changed, 10 insertions(+), 8 deletions(-)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 7d98175..662ca88 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -329,15 +329,16 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
else
{
try{
- if(this->info.model=="XL_320")
+ if(this->mode!=mode)
{
- this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
- usleep(10000);
- this->mode=mode;
- }
- else
- {
- if(this->mode!=mode)
+ this->disable();
+ if(this->info.model=="XL_320")
+ {
+ this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
+ usleep(10000);
+ this->mode=mode;
+ }
+ else
{
if(mode==angle_ctrl)
{
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 38b96d6..276cc36 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -840,6 +840,7 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
if(this->mode!=mode)
{
+ this->disable();
for(i=0;i<this->servo_id.size();i++)
{
try{
--
GitLab