merge master
This commit is contained in:
NuoDaJia02
93
ecrt_dev/CMakeLists.txt
Normal file
93
ecrt_dev/CMakeLists.txt
Normal file
@@ -0,0 +1,93 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(ecrt_driver)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_library(ECRT_LIB
|
||||
NAMES ethercat ecrt
|
||||
PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
|
||||
)
|
||||
if(NOT ECRT_LIB)
|
||||
message(FATAL_ERROR "ecrt not found; set CMAKE_PREFIX_PATH or system ld path.")
|
||||
endif()
|
||||
add_library(${PROJECT_NAME} SHARED src/ethercat_driver.cpp)
|
||||
target_link_libraries(${PROJECT_NAME} ${ECRT_LIB} pthread)
|
||||
target_include_directories(
|
||||
${PROJECT_NAME}
|
||||
PRIVATE
|
||||
include
|
||||
)
|
||||
|
||||
ament_target_dependencies(
|
||||
${PROJECT_NAME}
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
)
|
||||
|
||||
|
||||
# 构建可执行文件
|
||||
add_executable(test_motor src/test_motor.cpp)
|
||||
ament_target_dependencies(test_motor
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
trajectory_msgs
|
||||
control_msgs
|
||||
)
|
||||
install(TARGETS test_motor DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
|
||||
|
||||
# Causes the visibility macros to use dllexport rather than dllimport,
|
||||
# which is appropriate when building the dll but not consuming it.
|
||||
target_compile_definitions(${PROJECT_NAME} PRIVATE "ETHERCAT_DRIVER_BUILDING_LIBRARY")
|
||||
# prevent pluginlib from using boost
|
||||
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
|
||||
pluginlib_export_plugin_description_file(hardware_interface ethercat_driver_plugin.xml)
|
||||
|
||||
# INSTALL
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
DESTINATION lib
|
||||
)
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
install(
|
||||
DIRECTORY config description launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
endif()
|
||||
|
||||
## EXPORTS
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
ament_export_libraries(
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
ament_export_dependencies(
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
ethercat_interface
|
||||
)
|
||||
ament_package()
|
||||
142
ecrt_dev/config/controllers.yaml
Normal file
142
ecrt_dev/config/controllers.yaml
Normal file
@@ -0,0 +1,142 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
trajectory_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
gpio_command_controller:
|
||||
type: gpio_controllers/GpioCommandController
|
||||
|
||||
trajectory_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- joint_1
|
||||
- joint_2
|
||||
- joint_3
|
||||
- joint_4
|
||||
- joint_5
|
||||
- joint_6
|
||||
- joint_7
|
||||
- joint_8
|
||||
- joint_9
|
||||
- joint_10
|
||||
- joint_11
|
||||
- joint_12
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 200.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
open_loop_control: true
|
||||
allow_integration_in_goal_trajectories: true
|
||||
constraints:
|
||||
goal_time: 0.5
|
||||
stopped_velocity_tolerance: 0.02
|
||||
gpio_command_controller:
|
||||
ros__parameters:
|
||||
gpios:
|
||||
- joint_1
|
||||
- joint_2
|
||||
- joint_3
|
||||
- joint_4
|
||||
- joint_5
|
||||
- joint_6
|
||||
- joint_7
|
||||
- joint_8
|
||||
- joint_9
|
||||
- joint_10
|
||||
- joint_11
|
||||
- joint_12
|
||||
command_interfaces:
|
||||
joint_1:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_2:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_3:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_4:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_5:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_6:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_7:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_8:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_9:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_10:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_11:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
joint_12:
|
||||
- interfaces:
|
||||
- fault
|
||||
- enable
|
||||
state_interfaces:
|
||||
joint_1:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_2:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_3:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_4:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_5:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_6:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_7:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_8:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_9:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_10:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_11:
|
||||
- interfaces:
|
||||
- fault
|
||||
joint_12:
|
||||
- interfaces:
|
||||
- fault
|
||||
34
ecrt_dev/config/zeroErr_config.yaml
Normal file
34
ecrt_dev/config/zeroErr_config.yaml
Normal file
@@ -0,0 +1,34 @@
|
||||
# Configuration file for Maxon EPOS3 drive
|
||||
vendor_id: 0x5a65726f
|
||||
product_id: 0x00029252
|
||||
assign-activate: 0x0300 #DC synch register
|
||||
auto_fault_reset: true
|
||||
auto_enable_set: true
|
||||
auto_state_transitions: true
|
||||
sdo: # sdo data to be transferred at drive startup
|
||||
- {index: 0x6060, sub_index: 0, type: uint16, value: 8} # Set 0x1C33:01h = 0x02 DC设置
|
||||
#- {index: 0x6040, sub_index: 0, type: uint16, value: 0x80} # 清除错误
|
||||
#- {index: 0x6040, sub_index: 0, type: uint16, value: 0}
|
||||
|
||||
rpdo: #PxPDO = receive PDO Mapping
|
||||
- index: 0x1600
|
||||
channels:
|
||||
- {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position
|
||||
# - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0} #target velocity
|
||||
# - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0} #target torque
|
||||
# - {index: 0x6072, sub_index: 0, type: uint16, default: 500} # Max torque
|
||||
- {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
|
||||
- {index: 0x6060, sub_index: 0, type: uint8, default: 8} # Control operation
|
||||
- {index: 0xf0ff, sub_index: 0, type: uint8} # Dummy byte
|
||||
|
||||
tpdo: #TxPDO = transmit PDO Mapping
|
||||
- index: 0x1a00
|
||||
channels:
|
||||
- {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value
|
||||
# - {index: 0x60F4, sub_index: 0, type: int32} #Position Following error actual value
|
||||
- {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 } # Velocity actual value
|
||||
# - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 } # Torque actual value
|
||||
- {index: 0x6041, sub_index: 0, type: uint16} # State word
|
||||
# - {index: 0x603f, sub_index: 0, type: uint16} # Error code
|
||||
- {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation} # Mode of operation display
|
||||
- {index: 0xf0ff, sub_index: 0, type: uint8} # dummy byte
|
||||
9
ecrt_dev/description/motor_drive.config.xacro
Normal file
9
ecrt_dev/description/motor_drive.config.xacro
Normal file
@@ -0,0 +1,9 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
|
||||
|
||||
<link name="world"/>
|
||||
|
||||
<xacro:include filename="motor_drive.ros2_control.xacro" />
|
||||
|
||||
<xacro:motor_drive/>
|
||||
</robot>
|
||||
62
ecrt_dev/description/motor_drive.ros2_control.xacro
Normal file
62
ecrt_dev/description/motor_drive.ros2_control.xacro
Normal file
@@ -0,0 +1,62 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- 公共参数定义（一次修改，全局生效） -->
|
||||
<xacro:property name="master_id" value="0" />
|
||||
<xacro:property name="control_frequency" value="10" />
|
||||
<xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
|
||||
<xacro:property name="alias" value="0" />
|
||||
<xacro:property name="mode_of_operation" value="8" />
|
||||
<xacro:property name="slave_config_path" value="config/zeroErr_config.yaml" />
|
||||
|
||||
<!-- 子宏：封装单个关节的重复配置 -->
|
||||
<!-- 参数：joint_name（关节名称）、ec_position（EtherCAT位置索引） -->
|
||||
<xacro:macro name="single_joint_config" params="joint_name ec_position">
|
||||
<joint name="${joint_name}">
|
||||
<!-- 状态接口（位置/速度/力矩） -->
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="fault"/>
|
||||
|
||||
<!-- 命令接口（位置控制/故障重置/使能） -->
|
||||
|
||||
<command_interface name="fault"/>
|
||||
<command_interface name="enable"/>
|
||||
<command_interface name="position"/>
|
||||
<!-- EtherCAT模块配置（复用公共参数） -->
|
||||
<ec_module name="zeroErr">
|
||||
<plugin>${ec_plugin}</plugin>
|
||||
<param name="alias">${alias}</param>
|
||||
<param name="position">${ec_position}</param>
|
||||
<param name="mode_of_operation">${mode_of_operation}</param>
|
||||
<param name="slave_config">${slave_config_path}</param>
|
||||
</ec_module>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- 主宏：电机驱动系统配置 -->
|
||||
<xacro:macro name="motor_drive">
|
||||
<ros2_control name="motor_drive" type="system">
|
||||
<hardware>
|
||||
<plugin>ethercat_driver/EthercatDriver</plugin>
|
||||
<param name="master_id">${master_id}</param>
|
||||
<param name="control_frequency">${control_frequency}</param>
|
||||
</hardware>
|
||||
|
||||
<!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
|
||||
<xacro:single_joint_config joint_name="joint_1" ec_position="1" />
|
||||
<xacro:single_joint_config joint_name="joint_2" ec_position="2" />
|
||||
<xacro:single_joint_config joint_name="joint_3" ec_position="3" />
|
||||
<xacro:single_joint_config joint_name="joint_4" ec_position="4" />
|
||||
<xacro:single_joint_config joint_name="joint_5" ec_position="5" />
|
||||
<xacro:single_joint_config joint_name="joint_6" ec_position="6" />
|
||||
<xacro:single_joint_config joint_name="joint_7" ec_position="7" />
|
||||
<xacro:single_joint_config joint_name="joint_8" ec_position="8" />
|
||||
<xacro:single_joint_config joint_name="joint_9" ec_position="9" />
|
||||
<xacro:single_joint_config joint_name="joint_10" ec_position="10" />
|
||||
<xacro:single_joint_config joint_name="joint_11" ec_position="11" />
|
||||
<xacro:single_joint_config joint_name="joint_12" ec_position="12" />
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
9
ecrt_dev/ethercat_driver_plugin.xml
Normal file
9
ecrt_dev/ethercat_driver_plugin.xml
Normal file
@@ -0,0 +1,9 @@
|
||||
<library path="ecrt_driver">
|
||||
<class name="ethercat_driver/EthercatDriver"
|
||||
type="ethercat_driver::EthercatDriver"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
EtherCAT Driver for ros2_control.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
141
ecrt_dev/include/ethercat_driver/ethercat_driver.hpp
Normal file
141
ecrt_dev/include/ethercat_driver/ethercat_driver.hpp
Normal file
@@ -0,0 +1,141 @@
|
||||
// Copyright 2022 ICUBE Laboratory, University of Strasbourg
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
|
||||
#define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
|
||||
#include<thread>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include "hardware_interface/handle.hpp"
|
||||
#include "hardware_interface/hardware_info.hpp"
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
#include "ethercat_driver/visibility_control.h"
|
||||
//#include "ethercat_interface/ec_slave.hpp"
|
||||
//#include "ethercat_interface/ec_master.hpp"
|
||||
#include "zer_config.hpp"
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
#define CYCLIC_POSITION 8 //CSP 周期同步位置模式
|
||||
#define CYCLIC_VELOCITY 9 //CSP 周期同步速度模式
|
||||
#define CYCLIC_TORQUE 10 //CSP 周期同步扭矩模式
|
||||
#define PVT_MODE 5 //PVT模式
|
||||
namespace ethercat_driver
|
||||
{
|
||||
|
||||
class EthercatDriver : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
RCLCPP_SHARED_PTR_DEFINITIONS(EthercatDriver)
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
|
||||
void readData();
|
||||
void writeData();
|
||||
void check_master_state();
|
||||
void check_domain1_state();
|
||||
void check_slave_config_states();
|
||||
void set_motor_enable(int id,bool enable){
|
||||
if(id>0&&id<13){
|
||||
motor_enable_arr[id-1].store(enable);
|
||||
}
|
||||
};
|
||||
bool get_motor_enable(int id){
|
||||
if(id>0&&id<13){
|
||||
return motor_enable_arr[id-1].load();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
struct timespec timespec_add(struct timespec time1, struct timespec time2);
|
||||
private:
|
||||
std::array<std::atomic<bool>, NUM_SLAVES> motor_enable_arr;
|
||||
std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
|
||||
std::string & urdf, std::string component_name, std::string component_type);
|
||||
|
||||
///std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
|
||||
std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
|
||||
|
||||
std::vector<std::vector<double>> hw_joint_commands_;
|
||||
std::vector<double> hw_cmd_position_;
|
||||
std::vector<std::vector<double>> hw_sensor_commands_;
|
||||
std::vector<std::vector<double>> hw_gpio_commands_;
|
||||
std::vector<std::vector<double>> hw_joint_states_;
|
||||
std::vector<std::vector<double>> hw_sensor_states_;
|
||||
std::vector<std::vector<double>> hw_gpio_states_;
|
||||
|
||||
|
||||
//pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{"ethercat_interface", "ethercat_interface::EcSlave"};
|
||||
struct timespec wakeupTime,time;
|
||||
int control_frequency_;
|
||||
///ethercat_interface::EcMaster master_;
|
||||
ec_master_t *master = NULL;
|
||||
ec_domain_t *domain1 = NULL;
|
||||
ec_slave_config_t *sc[NUM_SLAVES] = {0};
|
||||
uint8_t *domain1_pd = NULL;
|
||||
ec_domain_state_t domain1_state = {};
|
||||
ec_master_state_t master_state = {};
|
||||
ec_slave_config_state_t sc_state[NUM_SLAVES] = {};
|
||||
|
||||
std::mutex ec_mutex_;
|
||||
bool activated_;
|
||||
#define FREQUENCY 1000
|
||||
#define NSEC_PER_SEC (1000000000L)
|
||||
#define CSP_MAX_VEL_COUNTS_PER_S 65536
|
||||
#define CSP_POS_DEADBAND 10 //CSP允许的误差(计数)
|
||||
#define CLOCK_TO_USE CLOCK_MONOTONIC
|
||||
#define NSEC_PER_SEC (1000000000L)
|
||||
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms
|
||||
#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
|
||||
#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
|
||||
const struct timespec cycletime = {0, PERIOD_NS};
|
||||
uint16_t command[NUM_SLAVES]; //状态字掩码
|
||||
uint16_t status[NUM_SLAVES]; //状态字
|
||||
uint16_t last_status[NUM_SLAVES]; //上个循环的状态字
|
||||
int8_t last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};
|
||||
int8_t mode_cmd=8;
|
||||
int inited = 0; //初始化
|
||||
unsigned int counter = 0;
|
||||
unsigned int sync_ref_counter = 0;
|
||||
};
|
||||
} // namespace ethercat_driver
|
||||
|
||||
#endif // ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
|
||||
49
ecrt_dev/include/ethercat_driver/visibility_control.h
Normal file
49
ecrt_dev/include/ethercat_driver/visibility_control.h
Normal file
@@ -0,0 +1,49 @@
|
||||
// Copyright 2022 ICUBE Laboratory, University of Strasbourg
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
|
||||
#define ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define ETHERCAT_DRIVER_EXPORT __attribute__ ((dllexport))
|
||||
#define ETHERCAT_DRIVER_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define ETHERCAT_DRIVER_EXPORT __declspec(dllexport)
|
||||
#define ETHERCAT_DRIVER_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef ETHERCAT_DRIVER_BUILDING_LIBRARY
|
||||
#define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_EXPORT
|
||||
#else
|
||||
#define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_IMPORT
|
||||
#endif
|
||||
#define ETHERCAT_DRIVER_PUBLIC_TYPE ETHERCAT_DRIVER_PUBLIC
|
||||
#define ETHERCAT_DRIVER_LOCAL
|
||||
#else
|
||||
#define ETHERCAT_DRIVER_EXPORT __attribute__ ((visibility("default")))
|
||||
#define ETHERCAT_DRIVER_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define ETHERCAT_DRIVER_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define ETHERCAT_DRIVER_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define ETHERCAT_DRIVER_PUBLIC
|
||||
#define ETHERCAT_DRIVER_LOCAL
|
||||
#endif
|
||||
#define ETHERCAT_DRIVER_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
|
||||
94
ecrt_dev/include/ethercat_driver/zer_config.hpp
Normal file
94
ecrt_dev/include/ethercat_driver/zer_config.hpp
Normal file
@@ -0,0 +1,94 @@
|
||||
#include "ecrt.h"
|
||||
#include <math.h>
|
||||
#define NUM_SLAVES 12
|
||||
#define ZER_VID_PID 0x5a65726f,0x00029252
|
||||
typedef struct {
|
||||
unsigned int ctrl_word; // 0x6040:00
|
||||
unsigned int target_position; // 0x607A:00
|
||||
unsigned int target_velocity; // 0x60FF:00
|
||||
unsigned int target_torque; // 0x6071:00
|
||||
unsigned int max_torque; // 0x6072:00
|
||||
unsigned int operation_mode; // 0x6060:00
|
||||
unsigned int reserved1; // 0xf0ff:00
|
||||
unsigned int status_word; // 0x6041:00
|
||||
unsigned int position_actual_value; // 0x6064:00
|
||||
unsigned int velocity_actual_value; // 0x606C:00
|
||||
unsigned int torque_actual_value; // 0x6077:00
|
||||
unsigned int error_code; // 0x603F:00
|
||||
unsigned int modes_of_operation_display; // 0x6061:00
|
||||
unsigned int reserved2; // 0xf0ff:00
|
||||
} zer_offset_t;
|
||||
|
||||
static zer_offset_t zer_offsets[12];
|
||||
|
||||
constexpr double rad_to_count= 524288.0/(2*M_PI);
|
||||
constexpr double count_to_rad=2*M_PI/524288.0;
|
||||
|
||||
#define PDO_ENTRY(alias,position,index) \
|
||||
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
|
||||
{alias,position, ZER_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x60FF, 0, &zer_offsets[index].target_velocity,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6071, 0, &zer_offsets[index].target_torque,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6072, 0, &zer_offsets[index].max_torque,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6060, 0, &zer_offsets[index].operation_mode,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved1,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6041, 0, &zer_offsets[index].status_word,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6064, 0, &zer_offsets[index].position_actual_value,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x606C, 0, &zer_offsets[index].velocity_actual_value,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6077, 0, &zer_offsets[index].torque_actual_value,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x603F, 0, &zer_offsets[index].error_code,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\
|
||||
{alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL},
|
||||
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
|
||||
// 下列结构由IGH主站 cstruct 自动生成
|
||||
const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
|
||||
PDO_ENTRY(0,1,0)
|
||||
PDO_ENTRY(0,2,1)
|
||||
PDO_ENTRY(0,3,2)
|
||||
PDO_ENTRY(0,4,3)
|
||||
PDO_ENTRY(0,5,4)
|
||||
PDO_ENTRY(0,6,5)
|
||||
|
||||
PDO_ENTRY(0,7,6)
|
||||
PDO_ENTRY(0,8,7)
|
||||
PDO_ENTRY(0,9,8)
|
||||
PDO_ENTRY(0,10,9)
|
||||
PDO_ENTRY(0,11,10)
|
||||
PDO_ENTRY(0,12,11)
|
||||
{} // 结束标记
|
||||
};
|
||||
|
||||
|
||||
static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
|
||||
/*RxPdo 0x1600*/
|
||||
{0x6040, 0x00, 16}, /* control word */
|
||||
{0x607a, 0x00, 32}, /* target position */
|
||||
{0x60ff, 0x00, 32}, /* target velocity */
|
||||
{0x6071, 0x00, 16}, /* Target Torque */
|
||||
{0x6072, 0x00, 16}, /* Max Torque */
|
||||
{0x6060, 0x00, 8}, /* modes of operation */
|
||||
{0xf0ff, 0x00, 8},
|
||||
/*TxPdo 0x1A00*/
|
||||
{0x6041, 0x00, 16}, /* status word */
|
||||
{0x6064, 0x00, 32}, /* position actual value */
|
||||
{0x606c, 0x00, 32}, /* velocity actual value */
|
||||
{0x6077, 0x00, 16}, /* torque actual value */
|
||||
{0x603f, 0x00, 16}, /* error code */
|
||||
{0x6061, 0x00, 8}, /* modes of operation display */
|
||||
{0xf0ff, 0x00, 8},
|
||||
};
|
||||
|
||||
static ec_pdo_info_t zer_device_pdos[] = {
|
||||
//RxPdo
|
||||
{0x1600, 7, zer_device_pdo_entries + 0 },
|
||||
//TxPdo
|
||||
{0x1A00, 7, zer_device_pdo_entries + 7 }
|
||||
};
|
||||
|
||||
static ec_sync_info_t zer_device_syncs[] = {
|
||||
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
|
||||
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
|
||||
{2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE},
|
||||
{3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
|
||||
{0xff}
|
||||
};
|
||||
95
ecrt_dev/launch/motor_drive.launch.py
Normal file
95
ecrt_dev/launch/motor_drive.launch.py
Normal file
@@ -0,0 +1,95 @@
|
||||
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
|
||||
from launch.actions import DeclareLaunchArgument,TimerAction
|
||||
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
'description_file',
|
||||
default_value='motor_drive.config.xacro',
|
||||
description='URDF/XACRO description file with the axis.',
|
||||
)
|
||||
)
|
||||
|
||||
description_file = LaunchConfiguration('description_file')
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("ecrt_driver"),
|
||||
"description",
|
||||
description_file,
|
||||
]
|
||||
),
|
||||
]
|
||||
)
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("ecrt_driver"),
|
||||
"config",
|
||||
"controllers.yaml",
|
||||
]
|
||||
)
|
||||
|
||||
control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[robot_description, robot_controllers,
|
||||
{
|
||||
"lock_memory": True,
|
||||
"thread_priority": 90
|
||||
}
|
||||
],
|
||||
output="both",
|
||||
)
|
||||
|
||||
robot_state_pub_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
parameters=[robot_description],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
|
||||
)
|
||||
|
||||
gpio_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["gpio_command_controller", "-c", "/controller_manager"],
|
||||
)
|
||||
|
||||
trajectory_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["trajectory_controller", "-c", "/controller_manager"],
|
||||
)
|
||||
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
|
||||
nodes = [
|
||||
control_node,
|
||||
robot_state_pub_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
delay_node,
|
||||
]#position_controller_spawner,
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments +
|
||||
nodes)
|
||||
25
ecrt_dev/package.xml
Normal file
25
ecrt_dev/package.xml
Normal file
@@ -0,0 +1,25 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>ecrt_driver</name>
|
||||
<version>1.2.0</version>
|
||||
<description>EtherCAT Driver for `ros2_control`</description>
|
||||
<maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>ethercat_interface</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
818
ecrt_dev/src/ethercat_driver.cpp
Normal file
818
ecrt_dev/src/ethercat_driver.cpp
Normal file
@@ -0,0 +1,818 @@
|
||||
|
||||
#include "ethercat_driver/ethercat_driver.hpp"
|
||||
#include <tinyxml2.h>
|
||||
#include <string>
|
||||
#include <regex>
|
||||
#include"ecrt.h"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace ethercat_driver
|
||||
{
|
||||
CallbackReturn EthercatDriver::on_init(
|
||||
const hardware_interface::HardwareInfo & info)
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
const std::lock_guard<std::mutex> lock(ec_mutex_);
|
||||
activated_ = false;
|
||||
for(int i=0;i<NUM_SLAVES;i++){
|
||||
set_motor_enable(i+1,false);
|
||||
}
|
||||
hw_joint_states_.resize(info_.joints.size());
|
||||
for (uint j = 0; j < info_.joints.size(); j++) {
|
||||
hw_joint_states_[j].resize(
|
||||
info_.joints[j].state_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
hw_sensor_states_.resize(info_.sensors.size());
|
||||
for (uint s = 0; s < info_.sensors.size(); s++) {
|
||||
hw_sensor_states_[s].resize(
|
||||
info_.sensors[s].state_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
hw_gpio_states_.resize(info_.gpios.size());
|
||||
for (uint g = 0; g < info_.gpios.size(); g++) {
|
||||
hw_gpio_states_[g].resize(
|
||||
info_.gpios[g].state_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
hw_cmd_position_.resize(info_.joints.size());
|
||||
#if 1
|
||||
hw_joint_commands_.resize(info_.joints.size());
|
||||
for (uint j = 0; j < info_.joints.size(); j++) {
|
||||
hw_joint_commands_[j].resize(
|
||||
info_.joints[j].command_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
#endif
|
||||
hw_sensor_commands_.resize(info_.sensors.size());
|
||||
for (uint s = 0; s < info_.sensors.size(); s++) {
|
||||
hw_sensor_commands_[s].resize(
|
||||
info_.sensors[s].command_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
hw_gpio_commands_.resize(info_.gpios.size());
|
||||
for (uint g = 0; g < info_.gpios.size(); g++) {
|
||||
hw_gpio_commands_[g].resize(
|
||||
info_.gpios[g].command_interfaces.size(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%d,%d,%d",info_.joints.size(),info_.joints[0].state_interfaces.size(),info_.joints[1].state_interfaces.size());
|
||||
for (uint j = 0; j < info_.joints.size(); j++) {
|
||||
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints");
|
||||
// check all joints for EC modules and load into ec_modules_
|
||||
auto module_params = getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint");
|
||||
ec_module_parameters_.insert(
|
||||
ec_module_parameters_.end(), module_params.begin(), module_params.end());
|
||||
for (auto i = 0ul; i < module_params.size(); i++) {
|
||||
for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) {
|
||||
module_params[i]["state_interface/" +
|
||||
info_.joints[j].state_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) {
|
||||
module_params[i]["command_interface/" +
|
||||
info_.joints[j].command_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios.size:%d",info_.gpios.size());
|
||||
for (uint g = 0; g < info_.gpios.size(); g++) {
|
||||
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios");
|
||||
// check all gpios for EC modules and load into ec_modules_
|
||||
auto module_params = getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio");
|
||||
ec_module_parameters_.insert(
|
||||
ec_module_parameters_.end(), module_params.begin(), module_params.end());
|
||||
for (auto i = 0ul; i < module_params.size(); i++) {
|
||||
for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) {
|
||||
module_params[i]["state_interface/" +
|
||||
info_.gpios[g].state_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) {
|
||||
module_params[i]["command_interface/" +
|
||||
info_.gpios[g].command_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors.size:%d",info_.joints.size());
|
||||
for (uint s = 0; s < info_.sensors.size(); s++) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors");
|
||||
// check all sensors for EC modules and load into ec_modules_
|
||||
auto module_params = getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor");
|
||||
ec_module_parameters_.insert(
|
||||
ec_module_parameters_.end(), module_params.begin(), module_params.end());
|
||||
for (auto i = 0ul; i < module_params.size(); i++) {
|
||||
for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) {
|
||||
module_params[i]["state_interface/" +
|
||||
info_.sensors[s].state_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); k++) {
|
||||
module_params[i]["command_interface/" +
|
||||
info_.sensors[s].command_interfaces[k].name] = std::to_string(k);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
//
|
||||
master = ecrt_request_master(0);
|
||||
if (!master) {
|
||||
perror("ecrt_request_master");
|
||||
//g_started.store(false); return false;
|
||||
}
|
||||
domain1 = ecrt_master_create_domain(master);
|
||||
if (!domain1){
|
||||
perror("ecrt_master_create_domain");
|
||||
//g_started.store(false); return false;
|
||||
}
|
||||
for (int i = 0; i < NUM_SLAVES; i++) {
|
||||
std::cout << "Configuring slave " << i << "..." << std::endl;
|
||||
{
|
||||
sc[i] = ecrt_master_slave_config(master, 0, i+1, ZER_VID_PID);
|
||||
if (!sc[i])
|
||||
{
|
||||
std::cout << "Failed slave cfg at pos " << i << std::endl;
|
||||
}
|
||||
if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs))
|
||||
{
|
||||
std::cout << "Failed PDO config " << i << std::endl;
|
||||
}
|
||||
}
|
||||
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
|
||||
}
|
||||
ecrt_domain_reg_pdo_entry_list(domain1,zer_domain1_regs);
|
||||
if (ecrt_master_activate(master)) {
|
||||
perror("ecrt_master_activate");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
domain1_pd = ecrt_domain_data(domain1);
|
||||
if (!domain1_pd)
|
||||
{
|
||||
fprintf(stderr,"domain1_pd null\n");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", info_.joints.size());
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn EthercatDriver::on_configure(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/)
|
||||
{
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
EthercatDriver::export_state_interfaces()
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_state_interfaces:%d,%d,%d",info_.joints.size(),info_.sensors.size(),info_.gpios.size());
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
// export joint state interface
|
||||
for (uint j = 0; j < info_.joints.size(); j++) {
|
||||
for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s\n",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[j].name,
|
||||
info_.joints[j].state_interfaces[i].name,
|
||||
&hw_joint_states_[j][i]));
|
||||
}
|
||||
}
|
||||
// export sensor state interface
|
||||
for (uint s = 0; s < info_.sensors.size(); s++) {
|
||||
for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) {
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.sensors[s].name,
|
||||
info_.sensors[s].state_interfaces[i].name,
|
||||
&hw_sensor_states_[s][i]));
|
||||
}
|
||||
}
|
||||
// export gpio state interface
|
||||
for (uint g = 0; g < info_.gpios.size(); g++) {
|
||||
for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) {
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.gpios[g].name,
|
||||
info_.gpios[g].state_interfaces[i].name,
|
||||
&hw_gpio_states_[g][i]));
|
||||
}
|
||||
}
|
||||
return state_interfaces;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
EthercatDriver::export_command_interfaces()
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%d",info_.joints.size());
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
// export joint command interface
|
||||
///std::vector<double> test;
|
||||
#if 0
|
||||
for (uint i = 0; i < info_.joints.size(); i++) {
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[i].name,
|
||||
hardware_interface::HW_IF_POSITION,
|
||||
&hw_cmd_position_[i]));
|
||||
}
|
||||
#endif
|
||||
for (uint j = 0; j < info_.joints.size(); j++) {
|
||||
////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"size:%d\n",info_.joints[j].command_interfaces.size());
|
||||
for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s\n",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[j].name,
|
||||
info_.joints[j].command_interfaces[i].name,
|
||||
&hw_joint_commands_[j][i]));
|
||||
}
|
||||
}
|
||||
// export sensor command interface
|
||||
for (uint s = 0; s < info_.sensors.size(); s++) {
|
||||
for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) {
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.sensors[s].name,
|
||||
info_.sensors[s].command_interfaces[i].name,
|
||||
&hw_sensor_commands_[s][i]));
|
||||
}
|
||||
}
|
||||
// export gpio command interface
|
||||
for (uint g = 0; g < info_.gpios.size(); g++) {
|
||||
for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) {
|
||||
printf("gpios:%s,%s\n",info_.gpios[g].name,info_.gpios[g].command_interfaces[i].name);
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.gpios[g].name,
|
||||
info_.gpios[g].command_interfaces[i].name,
|
||||
&hw_gpio_commands_[g][i]));
|
||||
}
|
||||
}
|
||||
return command_interfaces;
|
||||
}
|
||||
|
||||
CallbackReturn EthercatDriver::on_activate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/)
|
||||
{
|
||||
const std::lock_guard<std::mutex> lock(ec_mutex_);
|
||||
if (activated_) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("EthercatDriver"), "Double on_activate()");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "on activate ...please wait...");
|
||||
if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) {
|
||||
control_frequency_ = 100;
|
||||
} else {
|
||||
control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]);
|
||||
}
|
||||
control_frequency_=1000;
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"control_frequency_:%d\n",control_frequency_);
|
||||
if (control_frequency_ < 0) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
clock_gettime(CLOCK_TO_USE, &wakeupTime);
|
||||
for(int i=0;i<NUM_SLAVES;i++){
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"slave:%d,ctl offset:%p,sta offset:%p\n",i,zer_offsets[i].ctrl_word,zer_offsets[i].status_word);
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"activate finish");
|
||||
activated_ = true;
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn EthercatDriver::on_deactivate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/)
|
||||
{
|
||||
const std::lock_guard<std::mutex> lock(ec_mutex_);
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "SSSSSSSSSSSSSSSSSSSSSSSS...");
|
||||
for(int i=0;i<NUM_SLAVES;i++){
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
|
||||
// CSV 安全：速度清零
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
|
||||
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
|
||||
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
|
||||
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
|
||||
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
|
||||
}
|
||||
activated_ = false;
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
hardware_interface::return_type EthercatDriver::read(
|
||||
const rclcpp::Time & /*time*/,
|
||||
const rclcpp::Duration & /*period*/)
|
||||
{
|
||||
// try to lock so we can avoid blocking the read/write loop on the lock.
|
||||
const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
|
||||
//printf("read data...\n");
|
||||
if (lock.owns_lock() && activated_) {
|
||||
////master_.readData();
|
||||
readData();
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
void EthercatDriver::check_domain1_state(void)
|
||||
{
|
||||
ec_domain_state_t ds;
|
||||
|
||||
ecrt_domain_state(domain1, &ds);
|
||||
|
||||
if (ds.working_counter != domain1_state.working_counter)
|
||||
//printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包
|
||||
if (ds.wc_state != domain1_state.wc_state)
|
||||
//printf("Domain1: State %u.\n", ds.wc_state);
|
||||
|
||||
domain1_state = ds;
|
||||
}
|
||||
void EthercatDriver::check_master_state(void)
|
||||
{
|
||||
ec_master_state_t ms;
|
||||
|
||||
ecrt_master_state(master, &ms);
|
||||
|
||||
if (ms.slaves_responding != master_state.slaves_responding)
|
||||
//printf("%u slave(s).\n", ms.slaves_responding);
|
||||
if (ms.al_states != master_state.al_states)
|
||||
//printf("AL states: 0x%02X.\n", ms.al_states);
|
||||
if (ms.link_up != master_state.link_up)
|
||||
//printf("Link is %s.\n", ms.link_up ? "up" : "down");
|
||||
|
||||
master_state = ms;
|
||||
}
|
||||
void EthercatDriver::check_slave_config_states(void)
|
||||
{
|
||||
ec_slave_config_state_t s;
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
if (!sc[i]) continue;
|
||||
ecrt_slave_config_state(sc[i], &s);
|
||||
if (s.al_state != sc_state[i].al_state)
|
||||
//printf("[S%02d] State 0x%02X.\n", i, s.al_state);
|
||||
if (s.online != sc_state[i].online)
|
||||
//printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
|
||||
if (s.operational != sc_state[i].operational)
|
||||
//printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
|
||||
sc_state[i] = s;
|
||||
}
|
||||
}
|
||||
struct timespec EthercatDriver::timespec_add(struct timespec time1, struct timespec time2)
|
||||
{
|
||||
struct timespec result;
|
||||
|
||||
if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
|
||||
result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
|
||||
result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
|
||||
} else {
|
||||
result.tv_sec = time1.tv_sec + time2.tv_sec;
|
||||
result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
void EthercatDriver::readData(){
|
||||
|
||||
#if 0
|
||||
static int print_cnt=0;
|
||||
if(print_cnt++%1000==0){
|
||||
for(int i=0;i<info_.joints.size();i++){
|
||||
printf("[%d]%s:",i,info_.joints[i].name.c_str());
|
||||
for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
|
||||
printf("%.2f,",hw_joint_commands_[i][j]);
|
||||
printf("\n");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
#endif
|
||||
wakeupTime = timespec_add(wakeupTime, cycletime);
|
||||
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
|
||||
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间
|
||||
ecrt_master_receive(master); //接收 EtherCAT 帧
|
||||
ecrt_domain_process(domain1); //处理域数据
|
||||
// check process data state (optional)
|
||||
check_domain1_state(); //检查域状态
|
||||
if (counter) {
|
||||
counter--;
|
||||
} else { // do this at 1 Hz
|
||||
counter = FREQUENCY;
|
||||
// check for master state (optional)
|
||||
//check_master_state(); //检查主站状态
|
||||
// check for slave configuration state(s)
|
||||
//check_slave_config_states(); //检查从站状态
|
||||
}
|
||||
if (!inited) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
command[i] = 0x004F;
|
||||
status[i] = 0x000F;
|
||||
last_status[i] = status[i];
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
|
||||
}
|
||||
inited = 1;
|
||||
}
|
||||
for (int i = 0; i < NUM_SLAVES; ++i)
|
||||
{
|
||||
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
|
||||
int32_t pos = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
int32_t vel = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
|
||||
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
|
||||
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
|
||||
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
|
||||
hw_joint_states_[i][0]=pos*count_to_rad;
|
||||
hw_joint_states_[i][1]=vel;
|
||||
hw_joint_states_[i][2]=40960;//err
|
||||
//hw_joint_states_[i][2]=err;
|
||||
///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],state:%.1f,%.1f,0x%04x",i,hw_joint_states_[i][0],hw_joint_states_[i][1],err);
|
||||
status[i] = sw; //侦测子站状态字变化
|
||||
if (status[i] != last_status[i]) {
|
||||
last_status[i] = status[i];
|
||||
}
|
||||
|
||||
|
||||
if ((status[i]&0x006f)==0x0008){
|
||||
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
|
||||
double fault=hw_joint_commands_[i][0];
|
||||
if(fault==1.0&&err>0){
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],fault code 0x%04x",i,err);
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
///static double last_enable[12];
|
||||
double enable=hw_joint_commands_[i][1];
|
||||
if(enable!=1){
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
|
||||
continue;
|
||||
}
|
||||
//write
|
||||
|
||||
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
|
||||
command[i] = 0x006F;
|
||||
///printf("SS:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0021) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
|
||||
command[i] = 0x006F;
|
||||
// 同步当前位置为目标位置
|
||||
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
|
||||
/////g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
|
||||
//printf("TTTTTTTTT:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0023) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
|
||||
command[i] = 0x006F;
|
||||
//printf("VVVVVVVV:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0027) {
|
||||
// ---- 4) 运行态写目标（按模式/运行意图） ----
|
||||
//if(enable==1)
|
||||
if(true)
|
||||
{
|
||||
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
|
||||
switch (mode_cmd) {
|
||||
case CYCLIC_VELOCITY: // 9
|
||||
////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
|
||||
break;
|
||||
case CYCLIC_POSITION: { // 8
|
||||
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
|
||||
////printf("%02d-->%d,%d,%d\n",i,hw_joint_commands_[i][0],hw_joint_commands_[i][1],hw_joint_commands_[i][2]);
|
||||
///break;
|
||||
// 每轴记住“上次下发的命令位置”
|
||||
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
|
||||
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
|
||||
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
|
||||
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
|
||||
|
||||
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
|
||||
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
|
||||
csp_cmd_pos[i] = pos; // 先对齐
|
||||
vmax_acc[i] = 0;
|
||||
csp_inited[i] = 1;
|
||||
}
|
||||
double target_pos=hw_joint_commands_[i][2]*rad_to_count;
|
||||
static double last_pos[12];
|
||||
if(target_pos!=last_pos[i]){
|
||||
last_pos[i]=target_pos;
|
||||
}
|
||||
|
||||
// 期望的“最终绝对目标”
|
||||
const int32_t goal = target_pos;///g_cmd.target_pos[i].load(std::memory_order_relaxed);
|
||||
///if(i==0)
|
||||
///printf("%d,goal:%d,pv:%d,flag:%d\n",i,goal,pv,goal_flag[i]);
|
||||
static int pos_cnt=0;
|
||||
//if(pos_cnt++%500==0)
|
||||
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pos,target_pos);
|
||||
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
|
||||
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
|
||||
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
|
||||
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
|
||||
|
||||
// 相对“命令轨迹”的误差（不是相对 pv）
|
||||
const int32_t err_cmd = goal - csp_cmd_pos[i];
|
||||
|
||||
// 死区：足够近就贴合到 goal
|
||||
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
|
||||
csp_cmd_pos[i] = goal;
|
||||
} else {
|
||||
// 限速迈步：命令轨迹只按时间往 goal 逼近
|
||||
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
|
||||
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
|
||||
}
|
||||
|
||||
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
|
||||
// const int32_t follow_err = csp_cmd_pos[i] - pv;
|
||||
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
|
||||
// if (std::abs(follow_err) > FE_LIMIT) {
|
||||
// // 例如：冻结或减半 max_step，给驱动时间追上
|
||||
// }
|
||||
#if 1
|
||||
// 下发“绝对目标”（不是增量）
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
|
||||
// 回显给上层（/joint_states_cmd 用）
|
||||
//////g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case CYCLIC_TORQUE: // 10
|
||||
///////EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
|
||||
break;
|
||||
default:
|
||||
// 未知模式：默认速度模式安全置 0
|
||||
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
|
||||
break;
|
||||
|
||||
}
|
||||
} else {
|
||||
// run_enable=false：清零目标
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
|
||||
// CSV 安全：速度清零
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
|
||||
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
|
||||
|
||||
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
|
||||
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
|
||||
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
|
||||
}
|
||||
}
|
||||
}
|
||||
//Final
|
||||
if (sync_ref_counter) {
|
||||
sync_ref_counter--;
|
||||
} else {
|
||||
sync_ref_counter = 1; // sync every 2 cycle
|
||||
|
||||
clock_gettime(CLOCK_TO_USE, &time);
|
||||
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
|
||||
}
|
||||
ecrt_master_sync_slave_clocks(master);
|
||||
|
||||
// send process data
|
||||
ecrt_domain_queue(domain1); //排队域数据
|
||||
ecrt_master_send(master); //发送ETHERCAT帧
|
||||
}
|
||||
void EthercatDriver::writeData(){
|
||||
|
||||
///return;
|
||||
|
||||
for (int i = 0; i < NUM_SLAVES; ++i){
|
||||
int8_t mode_cmd=8;
|
||||
|
||||
int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
|
||||
status[i] = sw; //侦测子站状态字变化
|
||||
if (status[i] != last_status[i]) {
|
||||
// printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
|
||||
last_status[i] = status[i];
|
||||
}
|
||||
///printf("%d error status:%04x,%04x\n",i,status[i],status[i]&0x006f);
|
||||
|
||||
#if 0
|
||||
static double last_reset[12];
|
||||
if(last_reset[i]!=reset){
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],reset:%.1f",i,reset);
|
||||
last_reset[i]=reset;
|
||||
}
|
||||
#endif
|
||||
|
||||
if ((status[i]&0x006f)==0x0008){
|
||||
static int err_cnt=0;
|
||||
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
|
||||
if(err_cnt++%500==0)
|
||||
printf("%d error status:%04x,err:%04x\n",i,status[i],err);
|
||||
}
|
||||
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
|
||||
command[i] = 0x006F;
|
||||
///printf("SS:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0021) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
|
||||
command[i] = 0x006F;
|
||||
// 同步当前位置为目标位置
|
||||
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
|
||||
/////g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
|
||||
//printf("TTTTTTTTT:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0023) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
|
||||
command[i] = 0x006F;
|
||||
//printf("VVVVVVVV:%d\n",i);
|
||||
} else if ((status[i] & command[i]) == 0x0027) {
|
||||
// ---- 4) 运行态写目标（按模式/运行意图） ----
|
||||
static double last_enable[12];
|
||||
double enable=hw_joint_commands_[i][2];
|
||||
#if 0
|
||||
if(last_enable[i]!=enable){
|
||||
//std::cout<<i<<",enable:"<<enable<<std::endl;
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],enable:%.1f",i,enable);
|
||||
last_enable[i]=enable;
|
||||
}
|
||||
#endif
|
||||
if(enable==1)
|
||||
{
|
||||
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
|
||||
switch (mode_cmd) {
|
||||
case CYCLIC_VELOCITY: // 9
|
||||
////////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
|
||||
break;
|
||||
case CYCLIC_POSITION: { // 8
|
||||
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
|
||||
////printf("%02d-->%d,%d,%d\n",i,hw_joint_commands_[i][0],hw_joint_commands_[i][1],hw_joint_commands_[i][2]);
|
||||
///break;
|
||||
// 每轴记住“上次下发的命令位置”
|
||||
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
|
||||
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
|
||||
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
|
||||
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
|
||||
|
||||
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
|
||||
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
|
||||
csp_cmd_pos[i] = pv; // 先对齐
|
||||
vmax_acc[i] = 0;
|
||||
csp_inited[i] = 1;
|
||||
}
|
||||
double target_pos=hw_joint_commands_[i][0];
|
||||
static double last_pos[12];
|
||||
if(target_pos!=last_pos[i]){
|
||||
|
||||
last_pos[i]=target_pos;
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],pos:%.1f",i,target_pos);
|
||||
// 期望的“最终绝对目标”
|
||||
const int32_t goal = pv;///g_cmd.target_pos[i].load(std::memory_order_relaxed);
|
||||
///if(i==0)
|
||||
///printf("%d,goal:%d,pv:%d,flag:%d\n",i,goal,pv,goal_flag[i]);
|
||||
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
|
||||
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
|
||||
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
|
||||
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
|
||||
|
||||
// 相对“命令轨迹”的误差（不是相对 pv）
|
||||
const int32_t err_cmd = goal - csp_cmd_pos[i];
|
||||
|
||||
// 死区：足够近就贴合到 goal
|
||||
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
|
||||
csp_cmd_pos[i] = goal;
|
||||
} else {
|
||||
// 限速迈步：命令轨迹只按时间往 goal 逼近
|
||||
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
|
||||
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
|
||||
}
|
||||
|
||||
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
|
||||
// const int32_t follow_err = csp_cmd_pos[i] - pv;
|
||||
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
|
||||
// if (std::abs(follow_err) > FE_LIMIT) {
|
||||
// // 例如：冻结或减半 max_step，给驱动时间追上
|
||||
// }
|
||||
#if 0
|
||||
// 下发“绝对目标”（不是增量）
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
|
||||
// 回显给上层（/joint_states_cmd 用）
|
||||
//////g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case CYCLIC_TORQUE: // 10
|
||||
///////EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
|
||||
break;
|
||||
default:
|
||||
// 未知模式：默认速度模式安全置 0
|
||||
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
|
||||
break;
|
||||
|
||||
}
|
||||
} else {
|
||||
// run_enable=false：清零目标
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
|
||||
// CSV 安全：速度清零
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
|
||||
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
|
||||
|
||||
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
|
||||
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
|
||||
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
|
||||
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sync_ref_counter) {
|
||||
sync_ref_counter--;
|
||||
} else {
|
||||
sync_ref_counter = 1; // sync every 2 cycle
|
||||
|
||||
clock_gettime(CLOCK_TO_USE, &time);
|
||||
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
|
||||
}
|
||||
ecrt_master_sync_slave_clocks(master);
|
||||
|
||||
// send process data
|
||||
ecrt_domain_queue(domain1); //排队域数据
|
||||
ecrt_master_send(master); //发送ETHERCAT帧
|
||||
|
||||
}
|
||||
hardware_interface::return_type EthercatDriver::write(
|
||||
const rclcpp::Time & /*time*/,
|
||||
const rclcpp::Duration & /*period*/)
|
||||
{
|
||||
// try to lock so we can avoid blocking the read/write loop on the lock.
|
||||
const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
|
||||
//printf("write data...\n");
|
||||
if (lock.owns_lock() && activated_) {
|
||||
//writeData();
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
std::vector<std::unordered_map<std::string, std::string>> EthercatDriver::getEcModuleParam(
|
||||
std::string & urdf,
|
||||
std::string component_name,
|
||||
std::string component_type)
|
||||
{
|
||||
// Check if everything OK with URDF string
|
||||
if (urdf.empty()) {
|
||||
throw std::runtime_error("empty URDF passed to robot");
|
||||
}
|
||||
tinyxml2::XMLDocument doc;
|
||||
if (!doc.Parse(urdf.c_str()) && doc.Error()) {
|
||||
throw std::runtime_error("invalid URDF passed in to robot parser");
|
||||
}
|
||||
if (doc.Error()) {
|
||||
throw std::runtime_error("invalid URDF passed in to robot parser");
|
||||
}
|
||||
|
||||
tinyxml2::XMLElement * robot_it = doc.RootElement();
|
||||
if (std::string("robot").compare(robot_it->Name())) {
|
||||
throw std::runtime_error("the robot tag is not root element in URDF");
|
||||
}
|
||||
|
||||
const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement("ros2_control");
|
||||
if (!ros2_control_it) {
|
||||
throw std::runtime_error("no ros2_control tag");
|
||||
}
|
||||
|
||||
std::vector<std::unordered_map<std::string, std::string>> module_params;
|
||||
std::unordered_map<std::string, std::string> module_param;
|
||||
|
||||
while (ros2_control_it) {
|
||||
const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(component_type.c_str());
|
||||
while (ros2_control_child_it) {
|
||||
if (!component_name.compare(ros2_control_child_it->Attribute("name"))) {
|
||||
const auto * ec_module_it = ros2_control_child_it->FirstChildElement("ec_module");
|
||||
while (ec_module_it) {
|
||||
module_param.clear();
|
||||
module_param["name"] = ec_module_it->Attribute("name");
|
||||
const auto * plugin_it = ec_module_it->FirstChildElement("plugin");
|
||||
if (NULL != plugin_it) {
|
||||
module_param["plugin"] = plugin_it->GetText();
|
||||
}
|
||||
const auto * param_it = ec_module_it->FirstChildElement("param");
|
||||
while (param_it) {
|
||||
module_param[param_it->Attribute("name")] = param_it->GetText();
|
||||
param_it = param_it->NextSiblingElement("param");
|
||||
}
|
||||
module_params.push_back(module_param);
|
||||
ec_module_it = ec_module_it->NextSiblingElement("ec_module");
|
||||
}
|
||||
}
|
||||
ros2_control_child_it = ros2_control_child_it->NextSiblingElement(component_type.c_str());
|
||||
}
|
||||
ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control");
|
||||
}
|
||||
|
||||
return module_params;
|
||||
}
|
||||
|
||||
} // namespace ethercat_driver
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
ethercat_driver::EthercatDriver, hardware_interface::SystemInterface)
|
||||
304
ecrt_dev/src/test_motor.cpp
Normal file
304
ecrt_dev/src/test_motor.cpp
Normal file
@@ -0,0 +1,304 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include <filesystem>
|
||||
#include <fstream> // 添加这行来支持文件流操作
|
||||
#include <time.h>
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
|
||||
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
|
||||
#include "control_msgs/action/follow_joint_trajectory.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
namespace fs = std::filesystem;
|
||||
constexpr double rad_to_count= 524288.0/(2*M_PI);
|
||||
constexpr double count_to_rad=2*M_PI/524288.0;
|
||||
class TestMotor : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TestMotor();
|
||||
~TestMotor();
|
||||
|
||||
int motor_dst(std::string name,double dst);//add by hehe
|
||||
//void ctrl_motor(int id,double pos,int reset,int enable);
|
||||
void motor_fault(int id,double fault);
|
||||
void motor_enable(int id,double enable);
|
||||
void motor_pos(int id,double pos);
|
||||
void motor_loop(int motor_id,int cnt);
|
||||
void motor_action(int id,double angle);
|
||||
void all_motor();
|
||||
void ControlLoop();
|
||||
private:
|
||||
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
|
||||
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
|
||||
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr client_;
|
||||
// 文件流相关
|
||||
std::ofstream data_file_; // 用于写入数据的文件流
|
||||
std::string data_file_path_; // 数据文件路径
|
||||
|
||||
rclcpp::TimerBase::SharedPtr controlTimer_;
|
||||
rclcpp::Time lastTime_; // 移至类成员
|
||||
|
||||
bool isRunning_;
|
||||
bool isPaused_;
|
||||
bool isFinished_;
|
||||
bool isRobotEnabled_;
|
||||
bool enableCommandExecuted_;
|
||||
int loop_cnt=0;
|
||||
int jogDirection_;
|
||||
//add by hehe
|
||||
std::unordered_map<std::string,double> curMap_;
|
||||
std::unordered_map<std::string,double> dstMap_;
|
||||
//control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
|
||||
sensor_msgs::msg::JointState curJointSta_;
|
||||
//add by hehe end
|
||||
// 执行当前状态对应的动作
|
||||
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
};
|
||||
|
||||
// 机器人控制器构造函数
|
||||
TestMotor::TestMotor() : Node("test_motor_node")
|
||||
{
|
||||
std::cout << "TestMotor Node is created!" << std::endl;
|
||||
// 创建发布者
|
||||
cmdPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
|
||||
//action
|
||||
client_=rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(this,"/trajectory_controller/follow_joint_trajectory");
|
||||
// 创建发布者
|
||||
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
|
||||
// 订阅仿真状态
|
||||
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&TestMotor::JointStatesCallback, this, std::placeholders::_1));
|
||||
lastTime_ = this->now(); // 初始化时间
|
||||
// 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
|
||||
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(5000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数);
|
||||
curMap_.clear();
|
||||
dstMap_.clear();
|
||||
//posMsg_.interface_groups.resize(12);
|
||||
//posMsg_.interface_values.resize(12);
|
||||
motor_enable(6,1);
|
||||
motor_enable(13,1);
|
||||
std::cout << "TestMotor Node is created finished!" << std::endl;
|
||||
}
|
||||
|
||||
TestMotor::~TestMotor()
|
||||
{
|
||||
std::cout << "Robot controller stopped." << std::endl;
|
||||
}
|
||||
|
||||
void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
if (!msg) { // 检查消息是否有效
|
||||
std::cout << "get null joint states!" << std::endl;
|
||||
return;
|
||||
}
|
||||
for(int i=0;i<msg->name.size();i++)
|
||||
{
|
||||
curMap_[msg->name[i]] = msg->position[i];
|
||||
///std::cout<<"cur:"<<msg->name[i]<<":"<<curMap_[msg->name[i]]<<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int TestMotor::motor_dst(std::string name,double dst){
|
||||
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
|
||||
double val=curMap_[name];
|
||||
double diff=dst-val;
|
||||
double tempJointValue=0;
|
||||
float delta=0.0;
|
||||
if(diff>600){
|
||||
delta=120.0;
|
||||
tempJointValue=val+delta;
|
||||
}else if(diff<-600){
|
||||
delta=-120.0;
|
||||
tempJointValue=val+delta;
|
||||
}else{
|
||||
return 0;
|
||||
}
|
||||
posMsg_.interface_groups.push_back(name);
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
tempValue.interface_names = {"position"};
|
||||
tempValue.values = {tempJointValue};
|
||||
posMsg_.interface_values.push_back(tempValue);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 状态机主循环
|
||||
void TestMotor::ControlLoop() {
|
||||
//Gpio_publish_joint_trajectory();
|
||||
all_motor();
|
||||
}
|
||||
void TestMotor::motor_pos(int id,double delta)
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
|
||||
std::string target_motor="joint_" + std::to_string(id);
|
||||
for(int i=0;i<12;i++){
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
posMsg_.interface_groups.push_back(tempInterfaceGroup);
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
//position
|
||||
tempValue.interface_names = {"position"};
|
||||
if(id==0){
|
||||
tempValue.values = {curMap_[tempInterfaceGroup]+delta};
|
||||
}else{
|
||||
if(tempInterfaceGroup!=target_motor)
|
||||
tempValue.values = {curMap_[tempInterfaceGroup]};
|
||||
else
|
||||
tempValue.values = {curMap_[tempInterfaceGroup]+delta};
|
||||
}
|
||||
std::cout<<tempInterfaceGroup<<":"<<curMap_[tempInterfaceGroup]<<"-->"<<tempValue.values[0]<<std::endl;
|
||||
posMsg_.interface_values.push_back(tempValue);
|
||||
}
|
||||
|
||||
gpioPub_->publish(posMsg_);
|
||||
usleep(10000);
|
||||
}
|
||||
void TestMotor::motor_fault(int id,double fault)
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
|
||||
std::string target_motor="joint_" + std::to_string(id);
|
||||
std::cout<<"fault:";
|
||||
for(int i=0;i<12;i++){
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
posMsg_.interface_groups.push_back(tempInterfaceGroup);
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
//reset
|
||||
tempValue.interface_names = {"fault"};
|
||||
if(id==0){
|
||||
tempValue.values = {fault};
|
||||
}else{
|
||||
if(tempInterfaceGroup!=target_motor)
|
||||
tempValue.values = {0};
|
||||
else
|
||||
tempValue.values = {fault};
|
||||
}
|
||||
|
||||
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
|
||||
posMsg_.interface_values.push_back(tempValue);
|
||||
}
|
||||
std::cout<<std::endl;
|
||||
gpioPub_->publish(posMsg_);
|
||||
usleep(50000);
|
||||
}
|
||||
void TestMotor::motor_enable(int id,double enable)
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
|
||||
std::string target_motor="joint_" + std::to_string(id);
|
||||
std::cout<<"enable:";
|
||||
for(int i=0;i<12;i++){
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
posMsg_.interface_groups.push_back(tempInterfaceGroup);
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
//enable
|
||||
tempValue.interface_names = {"enable"};
|
||||
#if 1
|
||||
if(i<id)
|
||||
tempValue.values = {enable};
|
||||
else
|
||||
tempValue.values = {0};
|
||||
#else
|
||||
if(id==0){
|
||||
tempValue.values = {enable};
|
||||
}else{
|
||||
if(tempInterfaceGroup!=target_motor)
|
||||
tempValue.values = {0};
|
||||
else
|
||||
tempValue.values = {enable};
|
||||
}
|
||||
#endif
|
||||
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
|
||||
posMsg_.interface_values.push_back(tempValue);
|
||||
}
|
||||
std::cout<<std::endl;
|
||||
gpioPub_->publish(posMsg_);
|
||||
usleep(1000000);
|
||||
}
|
||||
void TestMotor::motor_action(int id,double delta)
|
||||
{
|
||||
//std::cout<<"motor_action:"<<id<<","<<delta<<std::endl;
|
||||
if(client_->wait_for_action_server(std::chrono::seconds(5))){
|
||||
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
|
||||
goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
|
||||
trajectory_msgs::msg::JointTrajectoryPoint point;
|
||||
for(int i=0;i<12;i++){
|
||||
std::string joint="joint_"+std::to_string(i+1);
|
||||
//if(id==(i+1))
|
||||
if(i<12)
|
||||
point.positions.push_back(curMap_[joint]+delta);
|
||||
else
|
||||
point.positions.push_back(curMap_[joint]);
|
||||
printf("%s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
|
||||
}
|
||||
goal.trajectory.points.push_back(point);
|
||||
auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
|
||||
send_goal_option.goal_response_callback=[this](auto res){
|
||||
auto goal_handle=res.get();
|
||||
if(goal_handle){
|
||||
printf("goal has be accept!!!\n");
|
||||
}
|
||||
};
|
||||
send_goal_option.feedback_callback=[this](auto,auto feedback){
|
||||
for(int i=0;i<12;i++){
|
||||
printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
|
||||
}
|
||||
};
|
||||
send_goal_option.result_callback=[this](auto ret){
|
||||
if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
|
||||
printf("action ret succeed\n");
|
||||
else if(ret.code==rclcpp_action::ResultCode::ABORTED)
|
||||
printf("action ret aborted\n");
|
||||
};
|
||||
client_->async_send_goal(goal,send_goal_option);
|
||||
}else{
|
||||
printf("wait action server error\n");
|
||||
}
|
||||
}
|
||||
void TestMotor::motor_loop(int motor_id,int loop_cnt){
|
||||
std::cout<<"start test:"<<motor_id<<std::endl;
|
||||
motor_fault(0,1);
|
||||
//motor_enable(motor_id,1);
|
||||
if(loop_cnt%2==0)
|
||||
//motor_pos(motor_id,3000);
|
||||
motor_action(motor_id,-0.05);
|
||||
else
|
||||
//motor_pos(motor_id,-3000);
|
||||
motor_action(motor_id,0.05);
|
||||
///usleep(3*1000000);
|
||||
}
|
||||
void TestMotor::all_motor(){
|
||||
loop_cnt+=1;
|
||||
static int sw=0;
|
||||
|
||||
motor_loop(0,loop_cnt);
|
||||
#if 0
|
||||
int mid=loop_cnt%100;
|
||||
if(mid<13&&mid>0)
|
||||
motor_loop(mid,sw);
|
||||
else{
|
||||
loop_cnt=0;
|
||||
sw+=1;
|
||||
motor_loop(1,sw);
|
||||
}
|
||||
|
||||
#endif
|
||||
#if 0
|
||||
motor_loop(2,loop_cnt);
|
||||
motor_loop(3,loop_cnt);
|
||||
motor_loop(4,loop_cnt);
|
||||
motor_loop(5,loop_cnt);
|
||||
motor_loop(6,loop_cnt);
|
||||
motor_loop(7,loop_cnt);
|
||||
motor_loop(8,loop_cnt);
|
||||
motor_loop(9,loop_cnt);
|
||||
motor_loop(10,loop_cnt);
|
||||
motor_loop(11,loop_cnt);
|
||||
motor_loop(12,loop_cnt);
|
||||
#endif
|
||||
}
|
||||
int main(int argc,char* argv[]){
|
||||
rclcpp::init(argc,argv);
|
||||
auto node=std::make_shared<TestMotor>();
|
||||
///usleep(10000000);
|
||||
///node->all_motor();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
128
gripper_dev/gripper_dev/gripper_dev_node.py
Normal file
128
gripper_dev/gripper_dev/gripper_dev_node.py
Normal file
@@ -0,0 +1,128 @@
|
||||
|
||||
from jodellSdk.jodellSdkDemo import RgClawControl
|
||||
import time
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.action import ActionServer
|
||||
from rclpy.action import ActionServer
|
||||
from std_msgs.msg import String
|
||||
from interfaces.action import GripperCmd
|
||||
import sys
|
||||
import os
|
||||
|
||||
class GripperDevNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('gripper_dev_node')
|
||||
self.declare_parameter('port','/dev/ttyUSB0')
|
||||
param = self.get_parameter('port')
|
||||
self.com_dev = param.value
|
||||
self.declare_parameter('devid',9)
|
||||
param = self.get_parameter('devid')
|
||||
self.devid = param.value
|
||||
|
||||
self.declare_parameter('name','/gripper_cmd0')
|
||||
param = self.get_parameter('name')
|
||||
self.srv_name = param.value
|
||||
print('param:',self.com_dev,self.devid,self.srv_name)
|
||||
self.get_logger().info(f'ros param:{self.com_dev},{self.devid},{self.srv_name}')
|
||||
self.action=ActionServer(self,GripperCmd,self.srv_name,self.gripper_cmd_callback_action)
|
||||
|
||||
self.clawControl = RgClawControl()
|
||||
self.target_loc=0
|
||||
self.target_torque=0
|
||||
self.target_speed=0
|
||||
self.cur_loc=-1
|
||||
self.cur_torque=-1
|
||||
self.cur_goal=None
|
||||
self.timer_on=False
|
||||
self.target_mode=0
|
||||
print('gripper_dev_node init:',self.com_dev)
|
||||
#self.feedback_timer = self.create_timer(0.1, self.update_feedback)
|
||||
def gripper_cmd_callback_action(self,goal):
|
||||
self.cur_goal=goal
|
||||
req=goal.request
|
||||
self.target_loc=req.loc
|
||||
self.target_speed=req.speed
|
||||
self.target_torque=req.torque
|
||||
self.target_mode=req.mode
|
||||
print('recv goal',self.devid,self.target_loc,self.target_speed,self.target_torque,self.target_mode)
|
||||
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
|
||||
if flag==1:
|
||||
ret = self.clawControl.enableClamp(self.devid, True)
|
||||
if ret!=1:
|
||||
print('enble fail!')
|
||||
result=GripperCmd.Result()
|
||||
result.state="gripper enable fail!"
|
||||
result.result=0
|
||||
goal.abort()
|
||||
return result
|
||||
print('start run...')
|
||||
self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
|
||||
##feedback
|
||||
loop_flag=True
|
||||
while loop_flag:
|
||||
loc = self.clawControl.getClampCurrentLocation(self.devid)
|
||||
speed = self.clawControl.getClampCurrentSpeed(self.devid)
|
||||
torque = self.clawControl.getClampCurrentTorque(self.devid)
|
||||
self.cur_loc=loc[0]
|
||||
self.cur_speed=speed[0]
|
||||
if self.target_mode==0:
|
||||
loop_flag=abs(self.cur_loc-self.target_loc)>2
|
||||
elif self.target_mode==1:
|
||||
loop_flag=abs(self.cur_torque-self.target_torque)>2
|
||||
else:
|
||||
loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_loc-self.target_loc)>2
|
||||
#print('feedback:',self.cur_loc,speed,torque)
|
||||
feedback_msg = GripperCmd.Feedback()
|
||||
feedback_msg.loc=loc[0]
|
||||
feedback_msg.speed=speed[0]
|
||||
feedback_msg.torque=torque[0]
|
||||
print('update feedback:',feedback_msg)
|
||||
self.cur_goal.publish_feedback(feedback_msg)
|
||||
time.sleep(1)
|
||||
else:
|
||||
result=GripperCmd.Result()
|
||||
result.state="uart open fail!"
|
||||
result.result=0
|
||||
goal.abort()
|
||||
return result
|
||||
#print('start timer')
|
||||
self.timer_on=True
|
||||
print('location diff:',self.cur_loc,self.target_loc)
|
||||
state = self.clawControl.getClampCurrentState(self.devid)
|
||||
result=GripperCmd.Result()
|
||||
result.state=state[0]
|
||||
result.result=1
|
||||
goal.succeed()
|
||||
print('action finish',state)
|
||||
return result
|
||||
|
||||
def gripper_cmd_callback(self, request, response):
|
||||
self.cur_loc=request.loc
|
||||
self.cur_speed=request.speed
|
||||
self.cur_torque=request.torque
|
||||
print('recv gripper cmd:',self.cur_loc,self.cur_speed,self.cur_torque)
|
||||
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
|
||||
if flag == 1:
|
||||
#self.runCmd()
|
||||
#self.runCmd()
|
||||
response.state = self.clawControl.getClampCurrentState(self.devid)
|
||||
self.clawControl.serialOperation(self.com_dev, 115200, False)
|
||||
print('send gripper sta:',self.devid,response.state)
|
||||
else:
|
||||
response.state = 'uart open failed'
|
||||
return response
|
||||
def main():
|
||||
rclpy.init()
|
||||
node=GripperDevNode()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
pass
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
7
gripper_dev/launch/gripper_dev.launch.py
Normal file
7
gripper_dev/launch/gripper_dev.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(package="gripper_dev",executable="gripper_dev_node",name="gripper_dev_node",
|
||||
parameters=[{'port':'/dev/ttyUSB0'}, {'name':'/gripper_cmd0'}, {'devid':6}])
|
||||
])
|
||||
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>jz_dev</name>
|
||||
<name>gripper_dev</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="you@example.com">h</maintainer>
|
||||
4
gripper_dev/setup.cfg
Normal file
4
gripper_dev/setup.cfg
Normal file
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/gripper_dev
|
||||
[install]
|
||||
install_scripts=$base/lib/gripper_dev
|
||||
24
gripper_dev/setup.py
Normal file
24
gripper_dev/setup.py
Normal file
@@ -0,0 +1,24 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
package_name='gripper_dev'
|
||||
setup(
|
||||
name='gripper_dev',
|
||||
version='0.1',
|
||||
description='A simple package for gripper_dev',
|
||||
author='Gripper Dev',
|
||||
author_email='gripper_dev@example.com',
|
||||
url='https://github.com/gripper_dev',
|
||||
packages=['gripper_dev'],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
|
||||
],
|
||||
install_requires=[
|
||||
'setuptools',
|
||||
],
|
||||
entry_points={
|
||||
'console_scripts':['gripper_dev_node = gripper_dev.gripper_dev_node:main']
|
||||
}
|
||||
)
|
||||
@@ -8,21 +8,21 @@ def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
# 启动orbbec_camera
|
||||
#ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'multi_camera.launch.py'],output='screen'),
|
||||
# ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'),
|
||||
ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'),
|
||||
# ExecuteProcess(cmd=['sleep', '2'],output='screen'),
|
||||
ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
|
||||
#ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
|
||||
|
||||
ExecuteProcess(cmd=['sleep', '2'],output='screen'),
|
||||
|
||||
# 等待一段时间确保相机启动完成
|
||||
|
||||
# 启动img_dev_node
|
||||
# Node(
|
||||
# package='img_dev',
|
||||
# executable='img_dev_node',
|
||||
# name='img_dev_node',
|
||||
# output='screen',
|
||||
# parameters=[]
|
||||
# )
|
||||
Node(
|
||||
package='img_dev',
|
||||
executable='img_dev_node',
|
||||
name='img_dev_node',
|
||||
output='screen',
|
||||
parameters=[]
|
||||
)
|
||||
])
|
||||
|
||||
|
||||
@@ -23,7 +23,7 @@ void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_
|
||||
img_pub->publish(*img_msg);
|
||||
// pub_cnt+=1;
|
||||
|
||||
// std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
|
||||
std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -16,9 +16,9 @@ using namespace img_dev;
|
||||
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,sensor_msgs::msg::Image>;
|
||||
static shared_ptr<ImageSubscriber> cur_node=nullptr;
|
||||
ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera/color/image_raw","/camera/depth/image_raw");
|
||||
ImgCfg cfg1=ImgCfg("orbbec", "myType","right","/camera_02/color/image_raw","/camera_02/depth/image_raw");
|
||||
ImgCfg cfg1=ImgCfg("orbbec", "myType","top","/camera_02/color/image_raw","/camera_02/depth/image_raw");
|
||||
//
|
||||
ImgCfg cfg2=ImgCfg("realsense", "myType","hand_l","/camera/camera/rgbd","/camera/camera/rgbd");
|
||||
ImgCfg cfg2=ImgCfg("realsense", "myType","right","/camera/camera/rgbd","/camera/camera/rgbd");
|
||||
ImgCfg cfg3=ImgCfg("realsense", "myType","hand_r","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
|
||||
void sync_cb0(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
|
||||
cur_node->pub_msg(c_img,d_img,cfg0);
|
||||
|
||||
@@ -1,81 +0,0 @@
|
||||
|
||||
from jodellSdk.jodellSdkDemo import RgClawControl
|
||||
import time
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from interfaces.srv import JzCmd
|
||||
import sys
|
||||
import os
|
||||
|
||||
class JzDevNode(Node):
|
||||
|
||||
def __init__(self,com_dev):
|
||||
super().__init__('jz_dev_node')
|
||||
self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
|
||||
self.com_dev = com_dev
|
||||
self.clawControl = RgClawControl()
|
||||
self.cur_loc=0
|
||||
self.cur_torque=0
|
||||
self.cur_speed=0
|
||||
self.cur_state=0
|
||||
print('jz_dev_node init')
|
||||
def runCmd(self,id):
|
||||
ret=self.clawControl.runWithParam(id,self.cur_loc,self.cur_speed,self.cur_torque)
|
||||
return ret
|
||||
def getState(self,id):
|
||||
loc = self.clawControl.getClampCurrentLocation(id)
|
||||
speed = self.clawControl.getClampCurrentSpeed(id)
|
||||
state = self.clawControl.getClampCurrentState(id)
|
||||
def jz_cmd_callback(self, request, response):
|
||||
devid=request.devid
|
||||
self.cur_loc=request.loc
|
||||
self.cur_speed=request.speed
|
||||
self.cur_torque=request.torque
|
||||
print('recv jz cmd:',devid,self.cur_loc,self.cur_speed,self.cur_torque)
|
||||
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
|
||||
if flag == 1:
|
||||
self.runCmd()
|
||||
response.state = self.clawControl.getClampCurrentState(devid)
|
||||
self.clawControl.serialOperation(self.com_dev, 115200, False)
|
||||
print('send jz sta:',devid,response.state)
|
||||
else:
|
||||
response.state = 'uart open failed'
|
||||
return response
|
||||
'''devid=6
|
||||
clawControl = RgClawControl()
|
||||
comList = clawControl.searchCom()
|
||||
#ver = clawControl.readSoftwareVersion(devid)
|
||||
print(comList)
|
||||
|
||||
####ret = clawControl.enableClamp(devid, False)
|
||||
ret=clawControl.forceOpenBrake(devid,True)
|
||||
print(ret)
|
||||
if flag==1:
|
||||
loc = clawControl.getClampCurrentLocation(devid)
|
||||
speed = clawControl.getClampCurrentSpeed(devid)
|
||||
state = clawControl.getClampCurrentState(devid)
|
||||
print('loc:',loc,'speed:',speed,'state:',state)
|
||||
|
||||
while True:
|
||||
ret=clawControl.runWithoutParam(devid,1)
|
||||
if ret==1:
|
||||
print('start run')
|
||||
time.sleep(3)
|
||||
ret = clawControl.enableClamp(devid, False)
|
||||
time.sleep(3)'''
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node=JzDevNode('/dev/ttyUSB0')
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
pass
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/jz_dev
|
||||
[install]
|
||||
install_scripts=$base/lib/jz_dev
|
||||
@@ -1,22 +0,0 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
package_name='jz_dev'
|
||||
setup(
|
||||
name='jz_dev',
|
||||
version='0.1',
|
||||
description='A simple package for jz_dev',
|
||||
author='JZ Dev',
|
||||
author_email='jz_dev@example.com',
|
||||
url='https://github.com/jz_dev',
|
||||
packages=['jz_dev'],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=[
|
||||
'setuptools',
|
||||
],
|
||||
entry_points={
|
||||
'console_scripts':['jz_dev_node = jz_dev.jz_dev_node:main']
|
||||
}
|
||||
)
|
||||
31
meta_dev/CMakeLists.txt
Normal file
31
meta_dev/CMakeLists.txt
Normal file
@@ -0,0 +1,31 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(meta_dev)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
add_executable(meta_dev_node src/main.cpp)
|
||||
ament_target_dependencies(meta_dev_node interfaces rclcpp)
|
||||
install(TARGETS meta_dev_node DESTINATION lib/${PROJECT_NAME})
|
||||
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
6
meta_dev/launch/meta_dev.launch.py
Normal file
6
meta_dev/launch/meta_dev.launch.py
Normal file
@@ -0,0 +1,6 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(package="meta_dev",executable="meta_dev_node",name="meta_dev_node",output="screen",parameters=[])
|
||||
])
|
||||
18
meta_dev/package.xml
Normal file
18
meta_dev/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>meta_dev</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="you@example.com">h</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
165
meta_dev/src/main.cpp
Normal file
165
meta_dev/src/main.cpp
Normal file
@@ -0,0 +1,165 @@
|
||||
#include"rclcpp/rclcpp.hpp"
|
||||
#include"sys/socket.h"
|
||||
#include "netinet/in.h"
|
||||
#include "unistd.h"
|
||||
#include "interfaces/msg/meta_key.hpp"
|
||||
#include <fcntl.h>
|
||||
|
||||
class UdpServerNode:public rclcpp::Node
|
||||
{
|
||||
private:
|
||||
rclcpp::Publisher<interfaces::msg::MetaKey>::SharedPtr pub_meta;
|
||||
std::thread udp_thread;
|
||||
unsigned char crc_table[256];
|
||||
struct HandData {
|
||||
uint8_t addr; // 0xf1
|
||||
uint8_t func; // 1
|
||||
int8_t a;
|
||||
int8_t b;
|
||||
int8_t sk;
|
||||
int8_t grab;
|
||||
int8_t sk_x;
|
||||
int8_t sk_y;
|
||||
int32_t x;
|
||||
int32_t y;
|
||||
int32_t z;
|
||||
int32_t xr;
|
||||
int32_t yr;
|
||||
int32_t zr;
|
||||
int32_t wr;
|
||||
};
|
||||
HandData hand_data;
|
||||
public:
|
||||
UdpServerNode():Node("meta_dev_node"){
|
||||
pub_meta=create_publisher<interfaces::msg::MetaKey>("/meta_key",10);
|
||||
init_crc8();
|
||||
udp_thread=std::thread([this](){
|
||||
struct sockaddr_in server_addr,client_addr;
|
||||
socklen_t client_len = sizeof(client_addr);
|
||||
const int PORT = 9999;
|
||||
const int udp_buf_len=256;
|
||||
unsigned char udp_buf[256];
|
||||
int sockfd;
|
||||
if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
||||
printf("UDP socket creation failed");
|
||||
return;
|
||||
}
|
||||
int flags = fcntl(sockfd, F_GETFL, 0);
|
||||
fcntl(sockfd, F_SETFL, flags | SOCK_NONBLOCK);
|
||||
// 配置服务器地址
|
||||
memset(&server_addr, 0, sizeof(server_addr));
|
||||
server_addr.sin_family = AF_INET;
|
||||
server_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
server_addr.sin_port = htons(PORT);
|
||||
|
||||
// 绑定端口
|
||||
if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
|
||||
printf("Bind failed on port %d", PORT);
|
||||
close(sockfd);
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout<<"UDP listener bound to port:"<< PORT<<std::endl;
|
||||
while (rclcpp::ok()) {
|
||||
//std::cout<<"start recv..."<<std::endl;
|
||||
ssize_t ret = recvfrom(sockfd, udp_buf, udp_buf_len, 0,
|
||||
(struct sockaddr *)&client_addr, &client_len);
|
||||
if (ret > 0) {
|
||||
std::cout<<"udp recv:"<<std::endl;
|
||||
for(int i=0;i<ret;i++){
|
||||
std::cout<<std::hex<<(unsigned int)udp_buf[i]<<",";
|
||||
}
|
||||
std::cout<<std::endl;
|
||||
unsigned char crc=calc_crc8(udp_buf,ret-1);
|
||||
if(udp_buf[ret-1]!=crc)
|
||||
std::cout<<"crc:"<<std::hex<<(unsigned int)crc<<(unsigned int)udp_buf[ret-1]<<std::endl;
|
||||
hand_data.addr=udp_buf[0];
|
||||
hand_data.func=udp_buf[1];
|
||||
hand_data.a=udp_buf[2];
|
||||
hand_data.b=udp_buf[3];
|
||||
hand_data.sk=udp_buf[4];
|
||||
hand_data.grab=udp_buf[5];
|
||||
hand_data.sk_x=udp_buf[6];
|
||||
hand_data.sk_y=udp_buf[7];
|
||||
hand_data.x=udp_buf[11]<<24|udp_buf[10]<<16|udp_buf[9]<<8|udp_buf[8];
|
||||
hand_data.y=udp_buf[15]<<24|udp_buf[14]<<16|udp_buf[13]<<8|udp_buf[12];
|
||||
hand_data.z=udp_buf[19]<<24|udp_buf[18]<<16|udp_buf[17]<<8|udp_buf[16];
|
||||
hand_data.xr=udp_buf[23]<<24|udp_buf[22]<<16|udp_buf[21]<<8|udp_buf[20];
|
||||
hand_data.yr=udp_buf[27]<<24|udp_buf[26]<<16|udp_buf[25]<<8|udp_buf[24];
|
||||
hand_data.zr=udp_buf[31]<<24|udp_buf[30]<<16|udp_buf[29]<<8|udp_buf[28];
|
||||
hand_data.wr=udp_buf[35]<<24|udp_buf[34]<<16|udp_buf[33]<<8|udp_buf[32];
|
||||
std::cout<<"hand:"<<std::dec<<(int)hand_data.a<<","<<(int)hand_data.b<<","<<(int)hand_data.sk<<","<<(int)hand_data.grab<<","
|
||||
<<(int)hand_data.sk_x<<","<<(int)hand_data.sk_y<<","<<(int)hand_data.x<<","<<(int)hand_data.y<<","<<(int)hand_data.z<<","
|
||||
<<(int)hand_data.xr<<","<<(int)hand_data.yr<<","<<(int)hand_data.zr<<","<<(int)hand_data.wr<<std::endl;
|
||||
interfaces::msg::MetaKey metaKey;
|
||||
metaKey.source="quest3";
|
||||
if(udp_buf[0]==0xf0)
|
||||
metaKey.type="right";
|
||||
else if(udp_buf[0]==0xf1)
|
||||
metaKey.type="left";
|
||||
metaKey.a=hand_data.a;
|
||||
metaKey.b=hand_data.b;
|
||||
metaKey.sk=hand_data.sk;
|
||||
metaKey.trigger=hand_data.grab;
|
||||
metaKey.sk_x=hand_data.sk_x;
|
||||
metaKey.sk_y=hand_data.sk_y;
|
||||
metaKey.pose.position.x=hand_data.x/100.0f;
|
||||
metaKey.pose.position.y=hand_data.y/100.0f;
|
||||
metaKey.pose.position.z=hand_data.z/100.0f;
|
||||
|
||||
metaKey.pose.orientation.x=hand_data.xr/10000.0f;
|
||||
metaKey.pose.orientation.y=hand_data.yr/10000.0f;
|
||||
metaKey.pose.orientation.z=hand_data.zr/10000.0f;
|
||||
metaKey.pose.orientation.w=hand_data.wr/10000.0f;
|
||||
this->pub_meta->publish(metaKey);
|
||||
} else if (ret < 0) {
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK ) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
}
|
||||
std::cout<<"recvfrom error:"<<std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
close(sockfd);
|
||||
std::cout<<"UDP listener stopped."<<std::endl;
|
||||
});
|
||||
}
|
||||
void pub_info(){
|
||||
|
||||
}
|
||||
|
||||
void init_crc8(){
|
||||
const unsigned char poly = 0x07;
|
||||
for (int i = 0; i < 256; i++)
|
||||
{
|
||||
unsigned char crc = (unsigned char)i;
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
if ((crc & 0x80) != 0)
|
||||
crc = (unsigned char)((crc << 1) ^ poly);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
crc_table[i] = crc;
|
||||
}
|
||||
}
|
||||
unsigned char calc_crc8(const unsigned char *data, int len){
|
||||
unsigned char crc = 0x00;
|
||||
for(int i=0;i<len;i++)
|
||||
{
|
||||
unsigned char b=data[i];
|
||||
crc = crc_table[(crc ^ b) & 0xFF];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
int main(int argc,char* argv[]){
|
||||
rclcpp::init(argc,argv);
|
||||
auto node =std::make_shared<UdpServerNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -5,7 +5,7 @@
|
||||
#include <vector>
|
||||
#include <termios.h>
|
||||
#include <mutex>
|
||||
|
||||
#include<map>
|
||||
namespace motor_dev
|
||||
{
|
||||
class RS485Driver
|
||||
@@ -16,7 +16,7 @@ namespace motor_dev
|
||||
|
||||
// 打开串口
|
||||
bool openPort(const std::string &port_name, int baud_rate = 115200);
|
||||
|
||||
std::map<uint8_t, float> speedMap;
|
||||
// 关闭串口
|
||||
void closePort();
|
||||
|
||||
@@ -25,13 +25,14 @@ namespace motor_dev
|
||||
|
||||
// 接收数据
|
||||
bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
|
||||
std::vector<uint8_t> sendCan(uint8_t cmd,const std::vector<uint8_t> data);
|
||||
//本末
|
||||
int bm_set_enable(uint8_t motor_id,uint8_t enable);
|
||||
int bm_set_enable(uint8_t enable);
|
||||
int bm_set_mode(uint8_t motor_id, uint8_t mode);
|
||||
int bm_set_pos(float angle1, float angle2);
|
||||
int bm_set_speed(float speed1, float speed2);
|
||||
int bm_set_speed(float speed1, float speed2,float speed3,float speed4);
|
||||
float bm_get_pos(uint8_t motor_id);
|
||||
float bm_get_speed(uint8_t motor_id);
|
||||
int bm_get_speed();
|
||||
int bm_query_id();
|
||||
int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);
|
||||
|
||||
|
||||
100
motor_dev/src/main.cpp
Normal file
100
motor_dev/src/main.cpp
Normal file
@@ -0,0 +1,100 @@
|
||||
#include"rclcpp/rclcpp.hpp"
|
||||
#include"motor_dev/rs485_driver.hpp"
|
||||
#include"interfaces/msg/motor_cmd.hpp"
|
||||
#include"interfaces/msg/motor_pos.hpp"
|
||||
#include"interfaces/srv/motor_param.hpp"
|
||||
using namespace motor_dev;
|
||||
RS485Driver rs485_driver_;
|
||||
|
||||
int main(int argc,char* argv[]){
|
||||
rclcpp::init(argc,argv);
|
||||
auto node=rclcpp::Node::make_shared("motor_dev_node");
|
||||
////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
|
||||
std::cout << "open RS485 port.." << std::endl;
|
||||
if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
|
||||
std::cout << "Failed to open RS485 port." << std::endl;
|
||||
}
|
||||
usleep(100000);
|
||||
rs485_driver_.speedMap.clear();
|
||||
rs485_driver_.bm_set_enable(1);
|
||||
|
||||
|
||||
|
||||
rs485_driver_.bm_set_param(1,28,3);
|
||||
rs485_driver_.bm_set_param(2,28,3);
|
||||
rs485_driver_.bm_set_param(3,28,3);
|
||||
rs485_driver_.bm_set_param(4,28,3);
|
||||
rs485_driver_.bm_set_enable(2);
|
||||
|
||||
rs485_driver_.bm_set_param(1,84,100);
|
||||
rs485_driver_.bm_set_param(2,84,100);
|
||||
rs485_driver_.bm_set_param(3,84,100);
|
||||
rs485_driver_.bm_set_param(4,84,100);
|
||||
|
||||
|
||||
//init
|
||||
/////rs485_driver_.bm_set_mode(1,3);//位置模式
|
||||
auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
|
||||
///
|
||||
if (msg->target == "rs485") {
|
||||
if(msg->type=="bm"){
|
||||
size_t n = msg->motor_id.size();
|
||||
if(n<2){
|
||||
printf("bm msg need four angle\n");
|
||||
return;
|
||||
}
|
||||
float speed1=msg->motor_speed[0];
|
||||
float speed2=msg->motor_speed[1];
|
||||
float speed3=msg->motor_speed[3];
|
||||
float speed4=msg->motor_speed[4];
|
||||
printf("###:%s,speed:%.1f,%.1f,%.1f,%.1f\n",msg->type.c_str(),speed1,speed2,speed3,speed4);
|
||||
rs485_driver_.bm_set_speed(speed1,speed2,speed3,speed4);
|
||||
}else if(msg->type=="mt"){
|
||||
uint8_t id=msg->motor_id[0];
|
||||
float angle =msg->motor_angle[0];
|
||||
rs485_driver_.mt_set_pos(id,angle);
|
||||
}
|
||||
}
|
||||
});
|
||||
auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
|
||||
//读取电机位置
|
||||
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() {
|
||||
static int pub_cnt=0;
|
||||
interfaces::msg::MotorPos motor_pos;
|
||||
rs485_driver_.bm_get_speed();
|
||||
|
||||
motor_pos.motor_id.clear();
|
||||
motor_pos.motor_angle.clear();
|
||||
motor_pos.motor_speed.clear();
|
||||
{
|
||||
motor_pos.source = "rs485";
|
||||
motor_pos.type = "bm";
|
||||
motor_pos.position="none";
|
||||
for(const auto& kv:rs485_driver_.speedMap){
|
||||
printf("%d:%.1f,",kv.first,kv.second);
|
||||
motor_pos.motor_id.push_back(kv.first);
|
||||
motor_pos.motor_speed.push_back(kv.second);
|
||||
}
|
||||
printf("\n");
|
||||
motor_pub->publish(motor_pos);
|
||||
|
||||
}
|
||||
pub_cnt+=1;
|
||||
});
|
||||
auto motor_param_srv=node->create_service<interfaces::srv::MotorParam>("motor_param",[=](const std::shared_ptr<interfaces::srv::MotorParam::Request> req,std::shared_ptr<interfaces::srv::MotorParam::Response> res){
|
||||
res->ret=0;
|
||||
printf("recv sv:%d,%d\n",req->motor_id,req->accel);
|
||||
if(req->accel<500){
|
||||
rs485_driver_.bm_set_param(1,84,300);
|
||||
res->ret=1;
|
||||
printf("svc:%d\n",res->ret);
|
||||
}
|
||||
});
|
||||
|
||||
|
||||
|
||||
rclcpp::spin(node);
|
||||
printf("now close rs485 port\n");
|
||||
rs485_driver_.closePort();
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
@@ -10,6 +10,52 @@
|
||||
namespace motor_dev
|
||||
{
|
||||
|
||||
std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
|
||||
std::vector<uint8_t> packet;
|
||||
packet.push_back(0x55); // 同步字节1
|
||||
packet.push_back(0xAA); // 同步字节2
|
||||
packet.push_back(0x1E); // 30字节
|
||||
|
||||
packet.push_back(0x01);
|
||||
packet.push_back(0x01);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x0a);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
|
||||
packet.push_back(cmd);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x08);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x00);
|
||||
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
packet.push_back(data[i]);
|
||||
}
|
||||
packet.push_back(0x00);
|
||||
bool ret = sendData(packet);
|
||||
std::vector<uint8_t> rx_data;
|
||||
if(ret){
|
||||
rx_data.clear();
|
||||
ret=receiveData(rx_data, 100, 1);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:rx_data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
return rx_data;
|
||||
}
|
||||
}
|
||||
return rx_data;
|
||||
}
|
||||
|
||||
const unsigned char CRC8Table[]={
|
||||
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
|
||||
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
|
||||
@@ -174,12 +220,10 @@ namespace motor_dev
|
||||
{
|
||||
static int idx=0;
|
||||
idx+=1;
|
||||
#ifdef hehe
|
||||
printf("[%03d]-->",idx);
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
#endif
|
||||
ssize_t bytes_written = write(com_fd_, data.data(), data.size());
|
||||
if (bytes_written != static_cast<ssize_t>(data.size()))
|
||||
{
|
||||
@@ -250,7 +294,7 @@ namespace motor_dev
|
||||
bool ret = sendData(command);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
bool ret=receiveData(data, 20, 20);
|
||||
bool ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
|
||||
}else{
|
||||
@@ -269,19 +313,19 @@ namespace motor_dev
|
||||
command.push_back(0x36);
|
||||
command.push_back(motor_id);
|
||||
command.push_back(0x1c);
|
||||
command.push_back(mode);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(2);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
uint8_t crc8 = CRC8_Table(command.data(), 10);
|
||||
command.push_back(crc8);
|
||||
//uint8_t crc8 = CRC8_Table(command.data(), 10);
|
||||
//command.push_back(crc8);
|
||||
bool ret = sendData(command);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 20);
|
||||
ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
@@ -293,65 +337,6 @@ namespace motor_dev
|
||||
}
|
||||
|
||||
|
||||
int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
|
||||
std::vector<uint8_t> packet;
|
||||
packet.push_back(0);
|
||||
packet.push_back(0x36);
|
||||
packet.push_back(motor_id);
|
||||
packet.push_back(param);
|
||||
|
||||
packet.push_back((uint8_t)(val));
|
||||
packet.push_back((uint8_t)(val>>8));
|
||||
packet.push_back((uint8_t)(val>>16));
|
||||
packet.push_back((uint8_t)(val>>24));
|
||||
packet.push_back(0);
|
||||
packet.push_back(0);
|
||||
uint8_t crc8 = CRC8_Table(packet.data(), 10);
|
||||
packet.push_back(crc8);
|
||||
bool ret = sendData(packet);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 20);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
int RS485Driver::bm_set_enable(uint8_t motor_id,uint8_t enable){
|
||||
std::vector<uint8_t> command;
|
||||
command.push_back(0);
|
||||
command.push_back(0x38);
|
||||
|
||||
command.push_back(motor_id);
|
||||
command.push_back(enable);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
|
||||
uint8_t crc8 = CRC8_Table(command.data(), 10);
|
||||
command.push_back(crc8);
|
||||
bool ret = sendData(command);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 20);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
int RS485Driver::bm_set_pos(float angle1,float angle2){
|
||||
std::vector<uint8_t> command;
|
||||
//if(angle>360.0)
|
||||
@@ -377,11 +362,10 @@ namespace motor_dev
|
||||
unsigned char crc8 = CRC8_Table(command.data(), 10);
|
||||
command.push_back(crc8);
|
||||
bool ret = sendData(command);
|
||||
#if 0
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 20);
|
||||
ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
@@ -389,47 +373,62 @@ namespace motor_dev
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
int RS485Driver::bm_set_speed(float speed1,float speed2){
|
||||
int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
|
||||
std::vector<uint8_t> command;
|
||||
//if(angle>360.0)
|
||||
// angle=360.0;
|
||||
int32_t pos=speed1*10;
|
||||
uint8_t high1 = (pos >> 8) & 0xFF; // 高8位
|
||||
uint8_t low1 = pos & 0xFF; // 低8位
|
||||
pos=speed2*10;
|
||||
uint8_t high2 = (pos >> 8) & 0xFF; // 高8位
|
||||
uint8_t low2 = pos & 0xFF; // 低8位
|
||||
//if(motor_id<5){
|
||||
command.push_back(0x00);
|
||||
command.push_back(0x32);
|
||||
command.push_back(high1);
|
||||
command.push_back(low1);
|
||||
command.push_back(high2);
|
||||
command.push_back(low2);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
//}
|
||||
unsigned char crc8 = CRC8_Table(command.data(), 10);
|
||||
command.push_back(crc8);
|
||||
bool ret = sendData(command);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 5);
|
||||
if(ret){
|
||||
#ifdef hehe
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
pos=speed3*10;
|
||||
uint8_t high3 = (pos >> 8) & 0xFF; // 高8位
|
||||
uint8_t low3 = pos & 0xFF; // 低8位
|
||||
pos=speed4*10;
|
||||
uint8_t high4 = (pos >> 8) & 0xFF; // 高8位
|
||||
uint8_t low4 = pos & 0xFF; // 低8位
|
||||
|
||||
command.push_back(high1);
|
||||
command.push_back(low1);
|
||||
command.push_back(high2);
|
||||
command.push_back(low2);
|
||||
command.push_back(high3);
|
||||
command.push_back(low3);
|
||||
command.push_back(high4);
|
||||
command.push_back(low4);
|
||||
printf("set_speed.%.1f\n",speed1);
|
||||
sendCan(0x32,command);
|
||||
return 0;
|
||||
}
|
||||
int RS485Driver::bm_set_enable(uint8_t enable){
|
||||
std::vector<uint8_t> command;
|
||||
command.push_back(enable);
|
||||
command.push_back(enable);
|
||||
command.push_back(enable);
|
||||
command.push_back(enable);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
printf("set_enable\n");
|
||||
sendCan(0x38,command);
|
||||
return 0;
|
||||
}
|
||||
int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
|
||||
std::vector<uint8_t> command;
|
||||
command.push_back(motor_id);
|
||||
command.push_back(param);
|
||||
|
||||
command.push_back((uint8_t)(val));
|
||||
command.push_back((uint8_t)(val>>8));
|
||||
command.push_back((uint8_t)(val>>16));
|
||||
command.push_back((uint8_t)(val>>24));
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
printf("set_param:%d,%d,%d\n",motor_id,param,val);
|
||||
sendCan(0x36,command);
|
||||
return 0;
|
||||
}
|
||||
float RS485Driver::bm_get_pos(uint8_t motor_id){
|
||||
@@ -450,14 +449,12 @@ namespace motor_dev
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 11, 1);
|
||||
ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
#ifdef hehe
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
#endif
|
||||
uint8_t nid=0x70+motor_id;
|
||||
if(data[0]==0&&data[1]==nid){
|
||||
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
|
||||
@@ -478,54 +475,26 @@ namespace motor_dev
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
float RS485Driver::bm_get_speed(uint8_t motor_id){
|
||||
int RS485Driver::bm_get_speed(){
|
||||
std::vector<uint8_t> command;
|
||||
command.push_back(0);
|
||||
command.push_back(0x35);
|
||||
command.push_back(motor_id);
|
||||
//command.push_back(0x0d);
|
||||
//command.push_back(4);
|
||||
//command.push_back(0x0b);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(1);
|
||||
command.push_back(5);
|
||||
command.push_back(11);
|
||||
command.push_back(13);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
command.push_back(0);
|
||||
unsigned char crc8 = CRC8_Table(command.data(), 10);
|
||||
command.push_back(crc8);
|
||||
bool ret = sendData(command);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 50);
|
||||
if(ret){
|
||||
printf("<---:");
|
||||
for(auto d:data)
|
||||
printf("%02x ",d);
|
||||
printf("\n");
|
||||
uint8_t nid=0x70+motor_id;
|
||||
if(data[0]==0&&data[1]==nid){
|
||||
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
|
||||
//float angle=360.0*pos/32768.0;
|
||||
int16_t pos=(data[6]<<8)+(data[7]&0xff);
|
||||
float angle=3.60f*pos;
|
||||
pos=(data[8]<<8)+(data[9]&0xff);
|
||||
float speed=pos/10.0f;
|
||||
//printf("id:%d,pos:%d\n",motor_id,pos);
|
||||
return speed;
|
||||
///printf("pos:%d,angle:%.1f\n",pos,angle);
|
||||
}
|
||||
else{
|
||||
|
||||
}
|
||||
//float angle = pos * 360.0 /255.0;
|
||||
///float angle=0;
|
||||
|
||||
command.push_back(0);
|
||||
std::vector<uint8_t> rx=sendCan(0x35,command);
|
||||
speedMap.clear();
|
||||
for(int x=0;x<rx.size();x++)
|
||||
if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
|
||||
uint8_t motor_id=rx[x+3]-0x70;
|
||||
int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
|
||||
printf("id:%d,speed:%d\n",motor_id,speed);
|
||||
speedMap[motor_id]=speed/10.0f;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
//mt
|
||||
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
|
||||
@@ -590,7 +559,7 @@ namespace motor_dev
|
||||
#if 1
|
||||
std::vector<uint8_t> data;
|
||||
data.clear();
|
||||
ret=receiveData(data, 20, 20);
|
||||
ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
|
||||
uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
|
||||
@@ -631,7 +600,7 @@ namespace motor_dev
|
||||
bool ret=sendData(packet);
|
||||
if(ret){
|
||||
std::vector<uint8_t> data;
|
||||
ret=receiveData(data, 20, 20);
|
||||
ret=receiveData(data, 20, 1);
|
||||
if(ret){
|
||||
uint8_t id= data[10];
|
||||
return id;
|
||||
|
||||
Reference in New Issue
Block a user