BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

- variable name changed.

Browse Source 
- ConvertRadian2Value / ConvertValue2Radian function bug fixed.
This commit is contained in:
ROBOTIS 2016-03-14 15:23:01 +09:00
parent c14f0b2c34
commit ba234e8e06
17 changed files with 290 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
robotis_device/devices/dynamixel/GRIPPER.device
View File

@ -3,22 +3,22 @@ model_name = GRIPPER
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = 0
max_radian_position_value = 750
min_radian = 0
max_radian = 1.150767
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = 0
value_of_max_radian_position = 750
min_radian = 0
max_radian = 1.150767
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/H42-20-S300-R.device
View File

@ -3,22 +3,22 @@ model_name = H42-20-S300-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -151900
max_radian_position_value = 151900
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -151900
value_of_max_radian_position = 151900
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/H54-100-S500-R.device
View File

@ -3,22 +3,22 @@ model_name = H54-100-S500-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -250950
max_radian_position_value = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -250950
value_of_max_radian_position = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/H54-200-B500-R.device
View File

@ -3,22 +3,22 @@ model_name = H54-200-B500-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -250950
max_radian_position_value = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -250950
value_of_max_radian_position = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/H54-200-S500-R.device
View File

@ -3,22 +3,22 @@ model_name = H54-200-S500-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -250950
max_radian_position_value = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -250950
value_of_max_radian_position = 250950
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/L42-10-S300-R.device
View File

@ -3,22 +3,22 @@ model_name = L42-10-S300-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -2047
max_radian_position_value = 2048
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -2047
value_of_max_radian_position = 2048
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/L54-30-S400-R.device
View File

@ -3,22 +3,22 @@ model_name = L54-30-S400-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -144198
max_radian_position_value = 144198
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -144198
value_of_max_radian_position = 144198
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/L54-30-S500-R.device
View File

@ -3,22 +3,22 @@ model_name = L54-30-S500-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -180684
max_radian_position_value = 180684
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -180684
value_of_max_radian_position = 180684
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/L54-50-S290-R.device
View File

@ -3,22 +3,22 @@ model_name = L54-50-S290-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -103860
max_radian_position_value = 103860
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -103860
value_of_max_radian_position = 103860
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/L54-50-S500-R.device
View File

@ -3,22 +3,22 @@ model_name = L54-50-S500-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -180684
max_radian_position_value = 180684
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -180684
value_of_max_radian_position = 180684
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/M42-10-S260-R.device
View File

@ -3,22 +3,22 @@ model_name = M42-10-S260-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -131584
max_radian_position_value = 131584
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -131584
value_of_max_radian_position = 131584
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/M54-40-S250-R.device
View File

@ -3,22 +3,22 @@ model_name = M54-40-S250-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -125700
max_radian_position_value = 125700
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -125700
value_of_max_radian_position = 125700
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/M54-60-S250-R.device
View File

@ -3,22 +3,22 @@ model_name = M54-60-S250-R
device_type = Dynamixel
[type info]
0_radian_position_value = 0
min_radian_position_value = -125700
max_radian_position_value = 125700
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
value_of_0_radian_position = 0
value_of_min_radian_position = -125700
value_of_max_radian_position = 125700
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 562
position_d_gain_address =
position_i_gain_address =
position_p_gain_address = 594
goal_position_address = 596
goal_velocity_address = 600
goal_torque_address = 604
present_position_address = 611
present_velocity_address = 615
present_load_address = 621
is_moving_address = 610
[control table]
# addr | item name | length | access | memory | min value | max value | signed

32
robotis_device/devices/dynamixel/MX-28.device
View File

@ -3,22 +3,22 @@ model_name = MX-28
device_type = Dynamixel
[type info]
0_radian_position_value = 2048
min_radian_position_value = 0
max_radian_position_value = 4095
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 24
position_d_gain_address = 26
position_i_gain_address = 27
position_p_gain_address = 28
goal_position_address = 30
goal_velocity_address = 32
goal_torque_address = 34
present_position_address = 36
present_velocity_address = 38
present_load_address = 40
is_moving_address = 46
value_of_0_radian_position = 2048
value_of_min_radian_position = 0
value_of_max_radian_position = 4095
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_address = 24
position_d_gain_address = 26
position_i_gain_address = 27
position_p_gain_address = 28
goal_position_address = 30
goal_velocity_address = 32
goal_torque_address = 34
present_position_address = 36
present_velocity_address = 38
present_load_address = 40
is_moving_address = 46
[control table]
# addr | item name | length | access | memory | min value | max value | signed

10
robotis_device/include/robotis_device/Dynamixel.h
View File

@ -30,9 +30,9 @@ public:
std::string ctrl_module_name;
DynamixelState *dxl_state;
INT32_T zero_radian_position_value;
INT32_T min_radian_position_value;
INT32_T max_radian_position_value;
INT32_T value_of_0_radian_position;
INT32_T value_of_min_radian_position;
INT32_T value_of_max_radian_position;
double min_radian;
double max_radian;
@ -50,8 +50,8 @@ public:
Dynamixel(int id, std::string model_name, float protocol_version);
double ConvertValue2Radian(int32_t value) { return value * max_radian / max_radian_position_value; }
INT32_T ConvertRadian2Value(double radian) { return radian * max_radian_position_value / max_radian; }
double ConvertValue2Radian(INT32_T value);
INT32_T ConvertRadian2Value(double radian);
};
}

58
robotis_device/src/robotis_device/Dynamixel.cpp
View File

@ -15,9 +15,9 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
port_name(""),
ctrl_module_name("none"),
protocol_version(protocol_version),
zero_radian_position_value(0),
min_radian_position_value(0),
max_radian_position_value(0),
value_of_0_radian_position(0),
value_of_min_radian_position(0),
value_of_max_radian_position(0),
min_radian(-3.14),
max_radian(3.14),
torque_enable_address(0),
@ -35,3 +35,55 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
ctrl_table.clear();
dxl_state = new DynamixelState();
}
double Dynamixel::ConvertValue2Radian(INT32_T value)
{
double _radian = 0.0;
if(value > value_of_0_radian_position)
{
if(max_radian <= 0)
return max_radian;
_radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position);
}
else if(value < value_of_0_radian_position)
{
if(min_radian >= 0)
return min_radian;
_radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position);
}
if(_radian > max_radian)
return max_radian;
else if(_radian < min_radian)
return min_radian;
return _radian;
}
INT32_T Dynamixel::ConvertRadian2Value(double radian)
{
//return radian * value_of_max_radian_position / max_radian;
INT32_T _value = 0;
if(radian > 0)
{
if(value_of_max_radian_position <= value_of_0_radian_position)
return value_of_max_radian_position;
_value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position;
}
else if(radian < 0)
{
if(value_of_min_radian_position >= value_of_0_radian_position)
return value_of_min_radian_position;
_value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position;
}
else
_value = value_of_0_radian_position;
if(_value > value_of_max_radian_position)
return value_of_max_radian_position;
else if(_value < value_of_min_radian_position)
return value_of_min_radian_position;
return _value;
}

12
robotis_device/src/robotis_device/Robot.cpp
View File

@ -152,12 +152,12 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
if(tokens.size() != 2)
continue;
if(tokens[0] == "0_radian_position_value")
dxl->zero_radian_position_value = std::atoi(tokens[1].c_str());
else if(tokens[0] == "min_radian_position_value")
dxl->min_radian_position_value = std::atoi(tokens[1].c_str());
else if(tokens[0] == "max_radian_position_value")
dxl->max_radian_position_value = std::atoi(tokens[1].c_str());
if(tokens[0] == "value_of_0_radian_position")
dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_min_radian_position")
dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_max_radian_position")
dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "min_radian")
dxl->min_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "max_radian")