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

- XM-430 / CM-740 device file added.

Browse Source 
- Sensor device added.
This commit is contained in:
ROBOTIS-zerom
2016-05-16 16:32:26 +09:00
parent 49f1fd9586
commit 9e1300eeda
12 changed files with 513 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
robotis_device/CMakeLists.txt
View File

@@ -39,6 +39,7 @@ include_directories(
## Declare a C++ library
add_library(robotis_device
src/robotis_device/Robot.cpp
src/robotis_device/Sensor.cpp
src/robotis_device/Dynamixel.cpp
)

77
robotis_device/devices/dynamixel/XM-430.device Normal file
View File

@@ -0,0 +1,77 @@
[device info]
model_name = XM-430
device_type = dynamixel
[type info]
current_ratio = 2.69
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_item_name = torque_enable
present_position_item_name = present_position
present_velocity_item_name = present_velocity
present_current_item_name = present_current
goal_position_item_name = goal_position
goal_velocity_item_name = goal_velocity
goal_current_item_name = goal_current
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed
0 | model_number | 2 | R | EEPROM | 0 | 65535 | N
6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N
7 | ID | 1 | RW | EEPROM | 0 | 252 | N
8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N
9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N
11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N
20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y
24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N
31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N
32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N
34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N
36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N
38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N
40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N
44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N
48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N
52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N
63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N
64 | torque_enable | 1 | RW | RAM | 0 | 1 | N
65 | LED | 1 | RW | RAM | 0 | 1 | N
68 | status_return_level | 1 | RW | RAM | 0 | 2 | N
69 | registered_instruction | 1 | R | RAM | 0 | 1 | N
70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N
76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N
78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N
80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N
82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N
84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N
88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N
90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N
100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N
102 | goal_current | 2 | RW | RAM | 0 | 1193 | N
104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N
108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N
112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N
116 | goal_position | 4 | RW | RAM | 0 | 4095 | N
120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N
122 | moving | 1 | R | RAM | 0 | 1 | N
123 | moving_status | 1 | R | RAM | 0 | 255 | N
124 | present_pwm | 2 | R | RAM | 0 | 885 | N
126 | present_current | 2 | R | RAM | 0 | 1193 | N
128 | present_velocity | 4 | R | RAM | 0 | 1023 | N
132 | present_position | 4 | R | RAM | 0 | 4095 | N
136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N
140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N
144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N
146 | present_temperature | 1 | R | RAM | 0 | 100 | N
168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N
224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N
578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N
634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N

25
robotis_device/devices/sensor/CM-740.device Normal file
View File

@@ -0,0 +1,25 @@
[device info]
model_name = CM-740
device_type = sensor
[control table]
# addr | item name | length | access | memory | min value | max value | signed
0 | model_number | 2 | R | EEPROM | 0 | 65535 | N
2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N
3 | ID | 1 | RW | EEPROM | 0 | 252 | N
4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N
5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N
16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N
24 | torque_enable | 1 | RW | RAM | 0 | 1 | N
25 | LED | 1 | RW | RAM | 0 | 7 | N
26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N
28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N
30 | button | 1 | RW | RAM | 0 | 3 | N
38 | gyro_z | 2 | R | RAM | 0 | 1023 | N
40 | gyro_y | 2 | R | RAM | 0 | 1023 | N
42 | gyro_x | 2 | R | RAM | 0 | 1023 | N
44 | acc_x | 2 | R | RAM | 0 | 1023 | N
46 | acc_y | 2 | R | RAM | 0 | 1023 | N
48 | acc_z | 2 | R | RAM | 0 | 1023 | N
50 | present_voltage | 1 | R | RAM | 50 | 250 | N

37
robotis_device/include/robotis_device/Device.h Normal file
View File

@@ -0,0 +1,37 @@
/*
* Device.h
*
* Created on: 2016. 5. 12.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#include <map>
#include <string>
#include <vector>
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Device
{
public:
UINT8_T id;
float protocol_version;
std::string model_name;
std::string port_name;
std::map<std::string, ControlTableItem *> ctrl_table;
std::vector<ControlTableItem *> bulk_read_items;
virtual ~Device() { }
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */

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

@@ -5,29 +5,23 @@
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#include <map>
#include <vector>
#include <string>
#include "Device.h"
#include "DynamixelState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Dynamixel
class Dynamixel : public Device
{
public:
UINT8_T id;
std::string model_name;
float protocol_version;
std::map<std::string, ControlTableItem *> ctrl_table; // string: item name
std::string port_name;
std::string ctrl_module_name;
DynamixelState *dxl_state;
@@ -48,9 +42,6 @@ public:
ControlTableItem *goal_velocity_item;
ControlTableItem *goal_current_item;
std::vector<ControlTableItem *> bulk_read_items;
std::map<std::string, UINT16_T> indirect_address_table;
Dynamixel(int id, std::string model_name, float protocol_version);
double ConvertValue2Radian(INT32_T value);
@@ -66,4 +57,4 @@ public:
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */

5
robotis_device/include/robotis_device/Robot.h
View File

@@ -21,14 +21,15 @@ class Robot
{
public:
std::map<std::string, PortHandler *> ports; // string: port name
std::map<std::string, std::string> port_default_joint; // port name, default joint name
std::map<std::string, std::string> port_default_device; // port name, default device name
std::map<std::string, Dynamixel *> dxls; // string: joint name
std::map<std::string, Sensor *> sensors; // string: sensor name
Robot(std::string robot_file_path, std::string dev_desc_dir_path);
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
Sensor *getSensor(std::string path, int id, std::string port, float protocol_version);
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
};
}

16
robotis_device/include/robotis_device/Sensor.h
View File

@@ -5,30 +5,26 @@
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#include <map>
#include <string>
#include <stdint.h>
#include "Device.h"
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Sensor
class Sensor : public Device
{
std::map<std::string, double> sensed_value;
public:
UINT8_T id;
std::string model_name;
float protocol_version;
std::map<UINT16_T, ControlTableItem *> ctrl_table;
Sensor(int id, std::string model_name, float protocol_version);
};
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */

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

@@ -5,16 +5,12 @@
* Author: zerom
*/
#include "../include/robotis_device/Dynamixel.h"
#include "robotis_device/Dynamixel.h"
using namespace ROBOTIS;
Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
: id(id),
model_name(model_name),
port_name(""),
ctrl_module_name("none"),
protocol_version(protocol_version),
: ctrl_module_name("none"),
current_ratio(1.0),
velocity_ratio(1.0),
value_of_0_radian_position(0),
@@ -30,11 +26,15 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
goal_velocity_item(0),
goal_current_item(0)
{
this->id = id;
this->model_name = model_name;
this->port_name = "";
this->protocol_version = protocol_version;
ctrl_table.clear();
dxl_state = new DynamixelState();
bulk_read_items.clear();
indirect_address_table.clear();
}
double Dynamixel::ConvertValue2Radian(INT32_T value)

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

@@ -78,7 +78,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str());
ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str()));
port_default_joint[tokens[0]] = tokens[2];
port_default_device[tokens[0]] = tokens[2];
}
else if(session == "device info")
{
@@ -129,6 +129,49 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
}
else if(tokens[0] == "sensor")
{
std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int _id = std::atoi(tokens[2].c_str());
std::string _port = tokens[1];
float _protocol = std::atof(tokens[4].c_str());
std::string _dev_name = tokens[5];
sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol);
Sensor *_sensor = sensors[_dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if(sub_tokens.size() > 0)
{
std::map<std::string, ControlTableItem *>::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1);
if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist
{
UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address;
for(int _i = 0; _i < sub_tokens.size(); _i++)
{
_sensor->bulk_read_items.push_back(new ControlTableItem());
ControlTableItem *_dest_item = _sensor->bulk_read_items[_i];
ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]];
_dest_item->item_name = _src_item->item_name;
_dest_item->address = _indirect_data_addr;
_dest_item->access_type = _src_item->access_type;
_dest_item->memory_type = _src_item->memory_type;
_dest_item->data_length = _src_item->data_length;
_dest_item->data_min_value = _src_item->data_min_value;
_dest_item->data_max_value = _src_item->data_max_value;
_dest_item->is_signed = _src_item->is_signed;
_indirect_data_addr += _dest_item->data_length;
}
}
else // INDIRECT_ADDRESS_1 exist
{
for(int _i = 0; _i < sub_tokens.size(); _i++)
_sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]);
}
}
}
}
}
file.close();
@@ -139,9 +182,92 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version)
{
Sensor *_sensor;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
{
std::string _session = "";
std::string _input_str;
while(!file.eof())
{
std::getline(file, _input_str);
// remove comment ( # )
std::size_t pos = _input_str.find("#");
if(pos != std::string::npos)
_input_str = _input_str.substr(0, pos);
// trim
_input_str = trim(_input_str);
if(_input_str == "")
continue;
// find _session
if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]"))
{
_input_str = _input_str.substr(1, _input_str.size()-2);
std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower);
_session = trim(_input_str);
continue;
}
if(_session == "device info")
{
std::vector<std::string> tokens = split(_input_str, '=');
if(tokens.size() != 2)
continue;
if(tokens[0] == "model_name")
_sensor = new Sensor(id, tokens[1], protocol_version);
}
else if(_session == "control table")
{
std::vector<std::string> tokens = split(_input_str, '|');
if(tokens.size() != 8)
continue;
ControlTableItem *item = new ControlTableItem();
item->item_name = tokens[1];
item->address = std::atoi(tokens[0].c_str());
item->data_length = std::atoi(tokens[2].c_str());
if(tokens[3] == "R")
item->access_type = READ;
else if(tokens[3] == "RW")
item->access_type = READ_WRITE;
if(tokens[4] == "EEPROM")
item->memory_type = EEPROM;
else if(tokens[4] == "RAM")
item->memory_type = RAM;
item->data_min_value = std::atol(tokens[5].c_str());
item->data_max_value = std::atol(tokens[6].c_str());
if(tokens[7] == "Y")
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
_sensor->ctrl_table[tokens[1]] = item;
}
}
_sensor->port_name = port;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str());
//std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl;
file.close();
}
else
std::cout << "Unable to open file : " + path << std::endl;
return _sensor;
}
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version)
{
Dynamixel *dxl;
Dynamixel *_dxl;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
@@ -187,7 +313,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
continue;
if(tokens[0] == "model_name")
dxl = new Dynamixel(id, tokens[1], protocol_version);
_dxl = new Dynamixel(id, tokens[1], protocol_version);
}
else if(_session == "type info")
{
@@ -196,20 +322,20 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
continue;
if(tokens[0] == "current_ratio")
dxl->current_ratio = std::atof(tokens[1].c_str());
_dxl->current_ratio = std::atof(tokens[1].c_str());
else if(tokens[0] == "velocity_ratio")
dxl->velocity_ratio = std::atof(tokens[1].c_str());
_dxl->velocity_ratio = std::atof(tokens[1].c_str());
else if(tokens[0] == "value_of_0_radian_position")
dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str());
_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());
_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());
_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());
_dxl->min_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "max_radian")
dxl->max_radian = std::atof(tokens[1].c_str());
_dxl->max_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "torque_enable_item_name")
_torque_enable_item_name = tokens[1];
@@ -250,33 +376,33 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
dxl->ctrl_table[tokens[1]] = item;
_dxl->ctrl_table[tokens[1]] = item;
}
}
dxl->port_name = port;
_dxl->port_name = port;
if(dxl->ctrl_table[_torque_enable_item_name] != NULL)
dxl->torque_enable_item = dxl->ctrl_table[_torque_enable_item_name];
if(dxl->ctrl_table[_present_position_item_name] != NULL)
dxl->present_position_item = dxl->ctrl_table[_present_position_item_name];
if(dxl->ctrl_table[_present_velocity_item_name] != NULL)
dxl->present_velocity_item = dxl->ctrl_table[_present_velocity_item_name];
if(dxl->ctrl_table[_present_current_item_name] != NULL)
dxl->present_current_item = dxl->ctrl_table[_present_current_item_name];
if(dxl->ctrl_table[_goal_position_item_name] != NULL)
dxl->goal_position_item = dxl->ctrl_table[_goal_position_item_name];
if(dxl->ctrl_table[_goal_velocity_item_name] != NULL)
dxl->goal_velocity_item = dxl->ctrl_table[_goal_velocity_item_name];
if(dxl->ctrl_table[_goal_current_item_name] != NULL)
dxl->goal_current_item = dxl->ctrl_table[_goal_current_item_name];
if(_dxl->ctrl_table[_torque_enable_item_name] != NULL)
_dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name];
if(_dxl->ctrl_table[_present_position_item_name] != NULL)
_dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name];
if(_dxl->ctrl_table[_present_velocity_item_name] != NULL)
_dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name];
if(_dxl->ctrl_table[_present_current_item_name] != NULL)
_dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name];
if(_dxl->ctrl_table[_goal_position_item_name] != NULL)
_dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name];
if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL)
_dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name];
if(_dxl->ctrl_table[_goal_current_item_name] != NULL)
_dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name];
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str());
//std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str());
//std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl;
file.close();
}
else
std::cout << "Unable to open file : " + path << std::endl;
return dxl;
return _dxl;
}

21
robotis_device/src/robotis_device/Sensor.cpp Normal file
View File

@@ -0,0 +1,21 @@
/*
* Sensor.cpp
*
* Created on: 2016. 5. 11.
* Author: zerom
*/
#include "robotis_device/Sensor.h"
using namespace ROBOTIS;
Sensor::Sensor(int id, std::string model_name, float protocol_version)
{
this->id = id;
this->model_name = model_name;
this->port_name = "";
this->protocol_version = protocol_version;
ctrl_table.clear();
bulk_read_items.clear();
}