HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update drivers @huiyu

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-04 17:53:35 +08:00
parent 1a6fe5df70
commit 83caf55dbe
6 changed files with 77 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -13,7 +13,7 @@ import os
class GripperDevNode(Node):
def __init__(self):
super().__init__('gripper_dev_node')
self.declare_parameter('port','/dev/ttyUSB0')
self.declare_parameter('port','/dev/ttyUSB1')
param = self.get_parameter('port')
self.com_dev = param.value
self.declare_parameter('devid',6)

49
imu_dev/launch/imu_dev.launch.py
View File

@@ -1,22 +1,39 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
imu_node = Node(
package='imu',
executable='imu_node',
name='imu_node',
parameters=[{
'port_name': '/dev/ttyUSB1',
}]
)
imu_dev_node = Node(
package='imu_dev',
executable='imu_dev_node',
name='imu_dev_node',
output='screen'
)
openzen_dev = FindPackageShare(package="openzen_driver")
openzen_dev_launch_path = PathJoinSubstitution(
[openzen_dev, "launch", "openzen_lpms.launch.py"]
)
include_openzen_dev = IncludeLaunchDescription(
PythonLaunchDescriptionSource(openzen_dev_launch_path),
)
return LaunchDescription([
Node(
package='imu',
executable='imu_node',
name='imu_node',
parameters=[{
'port_name': '/dev/ttyUSB0',
}]
),
Node(
package='imu_dev',
executable='imu_dev_node',
name='imu_dev_node',
output='screen'
)
#imu_node,
#imu_dev_node,
include_openzen_dev
])

70
motor_dev/src/main.cpp
View File

@@ -44,18 +44,18 @@ int main(int argc,char* argv[]){
rs485_driver_.bm_set_enable(1,1);
rs485_driver_.bm_set_enable(2,1);
rs485_driver_.bm_set_mode(1,3);//3应该是速度模式，我记得
rs485_driver_.bm_set_mode(1,4);
rs485_driver_.bm_set_enable(1,2);
rs485_driver_.bm_set_mode(2,3);
rs485_driver_.bm_set_mode(2,4);
rs485_driver_.bm_set_enable(2,2);
//下面这几个参数我记得是跟位置模式有关，也许可以不用
// rs485_driver_.bm_set_param(1,77,51);
// rs485_driver_.bm_set_param(1,78,8);
// rs485_driver_.bm_set_param(1,79,8);
// //rs485_driver_.bm_set_param(1,28,4);
// rs485_driver_.bm_set_param(2,77,51);
// rs485_driver_.bm_set_param(2,78,8);
// rs485_driver_.bm_set_param(2,79,8);
rs485_driver_.bm_set_param(1,77,51);
rs485_driver_.bm_set_param(1,78,8);
rs485_driver_.bm_set_param(1,79,8);
//rs485_driver_.bm_set_param(1,28,4);
rs485_driver_.bm_set_param(2,77,51);
rs485_driver_.bm_set_param(2,78,8);
rs485_driver_.bm_set_param(2,79,8);
//rs485_driver_.mt_set_pos(1,-5);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
@@ -69,15 +69,14 @@ int main(int argc,char* argv[]){
printf("bm msg need two angle\n");
return;
}
//float angle1 =msg->motor_angle[0];
//float angle2 =msg->motor_angle[1];
float angle1 =msg->motor_angle[0];
float angle2 =msg->motor_angle[1];
float speed1=msg->motor_speed[0];
float speed2=msg->motor_speed[1];
//log_printf("recv topic:%s,angle:%.1f,%.1f",msg->type.c_str(),angle1,angle2);
log_printf("recv topic:%s,speed:%.1f,%.1f",msg->type.c_str(),speed1,speed2);
///rs485_driver_.bm_set_pos(angle1,angle2);
rs485_driver_.bm_set_speed(speed1,speed2);
// float speed1=msg->motor_speed[0];
//float speed2=msg->motor_speed[1];
log_printf("recv topic:%s,angle:%.1f,%.1f",msg->type.c_str(),angle1,angle2);
rs485_driver_.bm_set_pos(angle1,angle2);
// rs485_driver_.bm_set_speed(speed1,speed2);
}else if(msg->type=="mt"){
uint8_t id=msg->motor_id[0];
float angle =msg->motor_angle[0];
@@ -94,16 +93,16 @@ int main(int argc,char* argv[]){
static int pub_cnt=0;
// log_printf("query motor pos");
interfaces::msg::MotorPos motor_pos;
//float angle1=rs485_driver_.bm_get_pos(1);
//float angle2=rs485_driver_.bm_get_pos(2);
float angle1=rs485_driver_.bm_get_pos(1);
float angle2=rs485_driver_.bm_get_pos(2);
float speed1=rs485_driver_.bm_get_speed(1);
float speed2=rs485_driver_.bm_get_speed(2);
//float speed1=rs485_driver_.bm_get_speed(1);
//float speed2=rs485_driver_.bm_get_speed(2);
///printf("bm cur angle:%.1f\n",angle);
///rs485_driver_.bm_query_id();
//rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95
motor_pos.motor_id.clear();
//motor_pos.motor_angle.clear();
motor_pos.motor_angle.clear();
motor_pos.motor_speed.clear();
////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
//if(angle1>-1&&angle2>-1)
@@ -113,10 +112,10 @@ int main(int argc,char* argv[]){
motor_pos.position="none";
motor_pos.motor_id.push_back(1);
motor_pos.motor_id.push_back(2);
//motor_pos.motor_angle.push_back(angle1);
//motor_pos.motor_angle.push_back(angle2);
motor_pos.motor_speed.push_back(speed1);
motor_pos.motor_speed.push_back(speed2);
motor_pos.motor_angle.push_back(angle1);
motor_pos.motor_angle.push_back(angle2);
//motor_pos.motor_speed.push_back(speed1);
//motor_pos.motor_speed.push_back(speed2);
motor_pub->publish(motor_pos);
//std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
@@ -141,15 +140,14 @@ int main(int argc,char* argv[]){
res->ret=-1;
log_printf("recv sv:%d,%d,%d,%d",req->motor_id,req->max_speed,req->add_acc,req->dec_acc);
//if(mid>-1&&mid<500)
{
// if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){
// rs485_driver_.bm_set_param(mid,77,req->max_speed);
// rs485_driver_.bm_set_param(mid,78,req->add_acc);
// rs485_driver_.bm_set_param(mid,79,req->dec_acc);
// res->ret=0;
// printf("svc:%d\n",res->ret);
// }
{
if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){
rs485_driver_.bm_set_param(mid,77,req->max_speed);
rs485_driver_.bm_set_param(mid,78,req->add_acc);
rs485_driver_.bm_set_param(mid,79,req->dec_acc);
res->ret=0;
printf("svc:%d\n",res->ret);
}
}
});
#if 0

14
openzen_dev/launch/openzen_lpms.launch.py
View File

@@ -13,11 +13,11 @@ def generate_launch_description():
# Note: topic tools not ported to ROS2 yet, so no easy conversion
# from quaternion to euler available (https://github.com/ros2/ros2/issues/857)
Node(
package="rqt_plot",
executable="rqt_plot",
name="ig1_data_plotter",
namespace="openzen",
arguments=["/openzen/data/angular_velocity"]
)
# Node(
# package="rqt_plot",
# executable="rqt_plot",
# name="ig1_data_plotter",
# namespace="openzen",
# arguments=["/openzen/data/angular_velocity"]
# )
])

2
waveshare/launch/imu_view.launch.py
View File

@@ -9,7 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
# Declare launch arguments
port_name_arg = DeclareLaunchArgument(
'port_name', default_value='/dev/ttyUSB0',
'port_name', default_value='/dev/ttyUSB1',
description='Choose which port to use'
)

2
waveshare/src/imu_node.cpp
View File

@@ -23,7 +23,7 @@ int main(int argc, char *argv[])
setlocale(LC_ALL,"");//Set encoding to prevent Chinese garbled characters
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("imu_node");
node->declare_parameter<std::string>("port_name", "/dev/ttyUSB0");
node->declare_parameter<std::string>("port_name", "/dev/ttyUSB0");//这个设备路径对不对？要确认夏
std::string port_name;
node->get_parameter<std::string>("port_name", port_name);//Get parameters