diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 7d98175d3f91cb0b8d96ecd334811e332440cb0e..662ca88094931bb197df49cddcca4d079deb9c06 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 38b96d6d9c547bbe8636b576bbeb44fe51ceedfb..276cc36728d92c40fe9a17aed34fb7e94852b68a 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{