Merge pull request #76 from ROBOTIS-GIT/develop

1.1.0 release

Author: ROBOTIS-Will

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench/control_table_item.h arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/control_table_item.h

@@ -114,6 +114,9 @@ class DynamixelDriver
   int32_t convertRadian2Value(uint8_t id, float radian);
   float convertValue2Radian(uint8_t id, int32_t value);
 
+  int32_t convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian = 3.14, float min_radian = -3.14);
+  float convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian = 3.14, float min_radian = -3.14);
+
   int32_t convertVelocity2Value(uint8_t id, float velocity);
   float convertValue2Velocity(uint8_t id, int32_t value);
 

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench/dynamixel_driver.h arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_driver.h

@@ -64,7 +64,7 @@ class DynamixelWorkbench
   bool wheelMode(uint8_t id, uint16_t vel = 0, uint16_t acc = 0);
   bool currentMode(uint8_t id, uint8_t cur = 50);
 
-  bool goalPosition(uint8_t id, uint16_t goal);
+  bool goalPosition(uint8_t id, int32_t goal);
   bool goalSpeed(uint8_t id, int32_t goal);
 
   bool itemWrite(uint8_t id, const char* item_name, int32_t value);  // write value to item
@@ -88,6 +88,10 @@ class DynamixelWorkbench
   int32_t convertRadian2Value(uint8_t id, float radian);
   float convertValue2Radian(uint8_t id, int32_t value);
 
+  int32_t convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian = 3.14, float min_radian = -3.14);
+  float convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian = 3.14, float min_radian = -3.14);
+
+
   int32_t convertVelocity2Value(uint8_t id, float velocity);
   float convertValue2Velocity(uint8_t id, int32_t value);
 

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench/dynamixel_item.h arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_item.h

@@ -1,2 +1,2 @@
-#include "../include/dynamixel_workbench/dynamixel_workbench.h"
+#include "../include/dynamixel_workbench_toolbox/dynamixel_workbench.h"
 

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench/dynamixel_tool.h arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_tool.h

@@ -16,7 +16,7 @@
 
 /* Authors: Taehun Lim (Darby) */
 
-#include "../../include/dynamixel_workbench/dynamixel_driver.h"
+#include "../../include/dynamixel_workbench_toolbox/dynamixel_driver.h"
 
 DynamixelDriver::DynamixelDriver() : tools_cnt_(0), sync_write_handler_cnt_(0), sync_read_handler_cnt_(0) {}
 
@@ -820,26 +820,16 @@ int32_t DynamixelDriver::convertRadian2Value(uint8_t id, float radian)
 
   if (radian > 0)
   {
-    if (tools_[factor].getValueOfMaxRadianPosition() <= tools_[factor].getValueOfZeroRadianPosition())
-      return tools_[factor].getValueOfMaxRadianPosition();
-
     value = (radian * (tools_[factor].getValueOfMaxRadianPosition() - tools_[factor].getValueOfZeroRadianPosition()) / tools_[factor].getMaxRadian()) + tools_[factor].getValueOfZeroRadianPosition();
   }
   else if (radian < 0)
   {
-    if (tools_[factor].getValueOfMinRadianPosition() >= tools_[factor].getValueOfZeroRadianPosition())
-      return tools_[factor].getValueOfMinRadianPosition();
-
     value = (radian * (tools_[factor].getValueOfMinRadianPosition() - tools_[factor].getValueOfZeroRadianPosition()) / tools_[factor].getMinRadian()) + tools_[factor].getValueOfZeroRadianPosition();
   }
   else
   {
     value = tools_[factor].getValueOfZeroRadianPosition();
   }
-  // if (value[id-1] > tools_[num].getValueOfMaxRadianPosition())
-  //   value[id-1] =  tools_[num].getValueOfMaxRadianPosition();
-  // else if (value[id-1] < tools_[num].getValueOfMinRadianPosition())
-  //   value[id-1] =  tools_[num].getValueOfMinRadianPosition();
 
   return value;
 }
@@ -851,22 +841,50 @@ float DynamixelDriver::convertValue2Radian(uint8_t id, int32_t value)
 
   if (value > tools_[factor].getValueOfZeroRadianPosition())
   {
-    if (tools_[factor].getMaxRadian() <= 0)
-      return tools_[factor].getMaxRadian();
-
     radian = (float)(value - tools_[factor].getValueOfZeroRadianPosition()) * tools_[factor].getMaxRadian() / (float)(tools_[factor].getValueOfMaxRadianPosition() - tools_[factor].getValueOfZeroRadianPosition());
   }
   else if (value < tools_[factor].getValueOfZeroRadianPosition())
   {
-    if (tools_[factor].getMinRadian() >= 0)
-      return tools_[factor].getMinRadian();
-
     radian = (float)(value - tools_[factor].getValueOfZeroRadianPosition()) * tools_[factor].getMinRadian() / (float)(tools_[factor].getValueOfMinRadianPosition() - tools_[factor].getValueOfZeroRadianPosition());
   }
-  //  if (radian[id-1] > tools_[num].getMaxRadian())
-  //    radian[id-1] =  tools_[num].getMaxRadian();
-  //  else if (radian[id-1] < tools_[num].min_radian_)
-  //    radian[id-1] =  tools_[num].min_radian_;
+
+  return radian;
+}
+
+int32_t DynamixelDriver::convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
+{
+  int32_t value = 0;
+  int32_t zero_position = (max_position + min_position)/2;
+
+  if (radian > 0)
+  {
+    value = (radian * (max_position - zero_position) / max_radian) + zero_position;
+  }
+  else if (radian < 0)
+  {
+    value = (radian * (min_position - zero_position) / min_radian) + zero_position;
+  }
+  else
+  {
+    value = zero_position;
+  }
+
+  return value;
+}
+
+float DynamixelDriver::convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
+{
+  float radian = 0.0;
+  int32_t zero_position = (max_position + min_position)/2;
+
+  if (value > zero_position)
+  {
+    radian = (float)(value - zero_position) * max_radian / (float)(max_position - zero_position);
+  }
+  else if (value < zero_position)
+  {
+    radian = (float)(value - zero_position) * min_radian / (float)(min_position - zero_position);
+  }
 
   return radian;
 }

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench/dynamixel_workbench.h arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h

@@ -16,7 +16,7 @@
 
 /* Authors: Taehun Lim (Darby) */
 
-#include "../../include/dynamixel_workbench/dynamixel_item.h"
+#include "../../include/dynamixel_workbench_toolbox/dynamixel_item.h"
 
 static uint8_t the_number_of_item = 0;
 
@@ -90,8 +90,8 @@ static void setAXInfo()
 {
   model_info.velocity_to_value_ratio         = 86.03; // AX series don't support exact speed in wheel mode.
 
-  model_info.value_of_0_radian_position      = 512;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 512;
   model_info.value_of_max_radian_position    = 1023;
 
   model_info.min_radian                      = -2.61799;
@@ -160,8 +160,8 @@ static void setRXInfo(void)
 {
   model_info.velocity_to_value_ratio         = 86.03; // RX series don't support exact speed in wheel mode.
 
-  model_info.value_of_0_radian_position      = 512;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 512;
   model_info.value_of_max_radian_position    = 1023;
 
   model_info.min_radian                      = -2.61799;
@@ -233,8 +233,8 @@ static void setEXInfo()
 {
   model_info.velocity_to_value_ratio         = 86.03; // EX series don't support exact speed in wheel mode.
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048; 
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -2.18969008;
@@ -306,8 +306,8 @@ static void setMXInfo()
 {
   model_info.velocity_to_value_ratio         = 86.81;
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048;  
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -3.14159265;
@@ -394,8 +394,8 @@ static void setMX2Info(void)
 {
   model_info.velocity_to_value_ratio         = 41.70;
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048;  
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -3.14159265;
@@ -473,8 +473,8 @@ static void setExtMXInfo()
 {
   model_info.velocity_to_value_ratio         = 86.81;
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048;  
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -3.14159265;
@@ -563,8 +563,8 @@ static void setExtMX2Info(void)
 {
   model_info.velocity_to_value_ratio         = 41.70;
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048;  
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -3.14159265;
@@ -633,8 +633,8 @@ static void setXL320Info()
 {
   model_info.velocity_to_value_ratio         = 86.03; // XL320 don't support exact speed in wheel mode.
 
-  model_info.value_of_0_radian_position      = 512;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 512;  
   model_info.value_of_max_radian_position    = 1023;
 
   model_info.min_radian                      = -2.61799;
@@ -721,8 +721,8 @@ static void setXLInfo()
 {
   model_info.velocity_to_value_ratio         = 41.70;
 
-  model_info.value_of_0_radian_position      = 2048;
   model_info.value_of_min_radian_position    = 0;
+  model_info.value_of_0_radian_position      = 2048;  
   model_info.value_of_max_radian_position    = 4095;
 
   model_info.min_radian                      = -3.14159265;
@@ -1078,9 +1078,9 @@ static void setPROInfo()
 {
   model_info.velocity_to_value_ratio         = 4792.8;
 
-  model_info.value_of_0_radian_position      = 0;
   model_info.value_of_min_radian_position    = -250961;
-  model_info.value_of_max_radian_position    =  250961;
+  model_info.value_of_0_radian_position      = 0;
+  model_info.value_of_max_radian_position    = 250961;
 
   model_info.min_radian                      = -3.14159265;
   model_info.max_radian                      =  3.14159265;

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/DynamixelWorkbench.h

@@ -16,7 +16,7 @@
 
 /* Authors: Taehun Lim (Darby) */
 
-#include "../../include/dynamixel_workbench/dynamixel_tool.h"
+#include "../../include/dynamixel_workbench_toolbox/dynamixel_tool.h"
 
 DynamixelTool::DynamixelTool() : dxl_info_cnt_(0), the_number_of_item_(0){}
 

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench/dynamixel_driver.cpp arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp

@@ -16,7 +16,7 @@
 
 /* Authors: Taehun Lim (Darby) */
 
-#include "../../include/dynamixel_workbench/dynamixel_workbench.h"
+#include "../../include/dynamixel_workbench_toolbox/dynamixel_workbench.h"
 
 DynamixelWorkbench::DynamixelWorkbench()
 {
@@ -278,7 +278,7 @@ bool DynamixelWorkbench::currentMode(uint8_t id, uint8_t cur)
   return comm_result;
 }
 
-bool DynamixelWorkbench::goalPosition(uint8_t id, uint16_t goal)
+bool DynamixelWorkbench::goalPosition(uint8_t id, int32_t goal)
 {
   bool comm_result = false;
 
@@ -438,6 +438,16 @@ float DynamixelWorkbench::convertValue2Radian(uint8_t id, int32_t value)
   return driver_.convertValue2Radian(id, value);
 }
 
+int32_t DynamixelWorkbench::convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
+{
+  return driver_.convertRadian2Value(radian, max_position, min_position, max_radian, min_radian);
+}
+
+float DynamixelWorkbench::convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
+{
+  return driver_.convertValue2Radian(value, max_position, min_position, max_radian, min_radian);
+}
+
 int32_t DynamixelWorkbench::convertVelocity2Value(uint8_t id, float velocity)
 {
   return driver_.convertVelocity2Value(id, velocity);