From 0851b50b5a600229b96a786945969aa0663ec48b Mon Sep 17 00:00:00 2001
From: zerom <zerom@robotis.com>
Date: Fri, 1 Jun 2018 13:33:01 +0900
Subject: [PATCH] - fixed motion module result gain bug

---
 .../src/robotis_controller/robotis_controller.cpp    | 12 ++++++------
 .../include/robotis_device/dynamixel_state.h         | 12 ++++++------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp
index 000bd59..1f78056 100755
--- a/robotis_controller/src/robotis_controller/robotis_controller.cpp
+++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp
@@ -1266,7 +1266,7 @@ void RobotisController::process()
                   port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data);
 
                 // if position p gain value is changed -> sync write
-                if (dxl_state->position_p_gain_ != result_state->position_p_gain_)
+                if (result_state->position_p_gain_ != 65535 && dxl_state->position_p_gain_ != result_state->position_p_gain_)
                 {
                   dxl_state->position_p_gain_ = result_state->position_p_gain_;
                   uint8_t sync_write_data[4] = { 0 };
@@ -1280,7 +1280,7 @@ void RobotisController::process()
                 }
 
                 // if position i gain value is changed -> sync write
-                if (dxl_state->position_i_gain_ != result_state->position_i_gain_)
+                if (result_state->position_i_gain_ != 65535 && dxl_state->position_i_gain_ != result_state->position_i_gain_)
                 {
                   dxl_state->position_i_gain_ = result_state->position_i_gain_;
                   uint8_t sync_write_data[4] = { 0 };
@@ -1294,7 +1294,7 @@ void RobotisController::process()
                 }
 
                 // if position d gain value is changed -> sync write
-                if (dxl_state->position_d_gain_ != result_state->position_d_gain_)
+                if (result_state->position_d_gain_ != 65535 && dxl_state->position_d_gain_ != result_state->position_d_gain_)
                 {
                   dxl_state->position_d_gain_ = result_state->position_d_gain_;
                   uint8_t sync_write_data[4] = { 0 };
@@ -1325,7 +1325,7 @@ void RobotisController::process()
                   port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data);
 
                 // if velocity p gain gain value is changed -> sync write
-                if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_)
+                if (result_state->velocity_p_gain_ != 65535 && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_)
                 {
                   dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_;
                   uint8_t sync_write_data[4] = { 0 };
@@ -1339,7 +1339,7 @@ void RobotisController::process()
                 }
 
                 // if velocity i gain value is changed -> sync write
-                if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_)
+                if (result_state->velocity_i_gain_ != 65535 && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_)
                 {
                   dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_;
                   uint8_t sync_write_data[4] = { 0 };
@@ -1353,7 +1353,7 @@ void RobotisController::process()
                 }
 
                 // if velocity d gain value is changed -> sync write
-                if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_)
+                if (result_state->velocity_d_gain_ != 65535 && dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_)
                 {
                   dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_;
                   uint8_t sync_write_data[4] = { 0 };
diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h
index b287bb3..05116c6 100755
--- a/robotis_device/include/robotis_device/dynamixel_state.h
+++ b/robotis_device/include/robotis_device/dynamixel_state.h
@@ -64,12 +64,12 @@ public:
       goal_position_(0.0),
       goal_velocity_(0.0),
       goal_torque_(0.0),
-      position_p_gain_(0),
-      position_i_gain_(0),
-      position_d_gain_(0),
-      velocity_p_gain_(0),
-      velocity_i_gain_(0),
-      velocity_d_gain_(0),
+      position_p_gain_(65535),
+      position_i_gain_(65535),
+      position_d_gain_(65535),
+      velocity_p_gain_(65535),
+      velocity_i_gain_(65535),
+      velocity_d_gain_(65535),
       position_offset_(0)
   {
     bulk_read_table_.clear();