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

add meta_dev

Browse Source
This commit is contained in:
hehe
2025-11-25 11:05:03 +08:00
parent e5cc743baf
commit a03bb14e1e
8 changed files with 419 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
jz_dev/jz_dev/jz_dev_node.py
View File

@@ -4,56 +4,64 @@ 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.srv import JzCmd
from interfaces.action import JzCmd
#from interfaces.srv import JzCmd
from interfaces.action import JzCmd
import sys
import os
class JzDevNode(Node):
def __init__(self):
def __init__(self):
super().__init__('jz_dev_node')
# self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
self.action=ActionServer(self,JzCmd,'jz_cmd',self.jz_cmd_callback_action)
self.declare_parameter('port','/dev/ttyUSB0')
param = self.get_parameter('port')
print('input param:',param.value)
self.com_dev = param.value
self.declare_parameter('devid',9)
param = self.get_parameter('devid')
self.devid = param.value
self.declare_parameter('name','/jz_cmd0')
param = self.get_parameter('name')
self.srv_name = param.value
print('param:',self.com_dev,self.devid,self.srv_name)
self.action=ActionServer(self,JzCmd,self.srv_name,self.jz_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_id=0
self.cur_goal=None
self.timer_on=False
self.target_mode=0
print('jz_dev_node init:',self.com_dev)
#self.feedback_timer = self.create_timer(0.1, self.update_feedback)
def jz_cmd_callback_action(self,goal):
print("jz cmd callback")
self.cur_goal=goal
req=goal.request
self.cur_id=req.devid
self.target_loc=req.loc
self.target_speed=req.speed
self.target_torque=req.torque
self.target_mode=req.mode
print('recv goal',self.cur_id,self.target_loc,self.target_speed,self.target_torque,self.target_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.cur_id, True)
ret = self.clawControl.enableClamp(self.devid, True)
if ret!=1:
print('enble fail!')
print('start run...')
self.clawControl.runWithParam(self.cur_id,self.target_loc,self.target_speed,self.target_torque)
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.cur_id)
speed = self.clawControl.getClampCurrentSpeed(self.cur_id)
torque = self.clawControl.getClampCurrentTorque(self.cur_id)
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:
@@ -64,7 +72,6 @@ class JzDevNode(Node):
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 = JzCmd.Feedback()
feedback_msg.devid = self.cur_id
feedback_msg.loc=loc[0]
feedback_msg.speed=speed[0]
feedback_msg.torque=torque[0]
@@ -73,30 +80,29 @@ class JzDevNode(Node):
time.sleep(1)
else:
result=JzCmd.Result()
result.devid=self.cur_id
result.state="uart open fail!"
result.result=0
goal.abort()
print("uart open failed")
return result
#print('start timer')
self.timer_on=True
print('location diff:',self.cur_loc,self.target_loc)
state = self.clawControl.getClampCurrentState(self.cur_id)
state = self.clawControl.getClampCurrentState(self.devid)
result=JzCmd.Result()
result.devid=self.cur_id
result.state=state[0]
result.result=1
goal.succeed()
print('action finish',state)
return result
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)
print('recv jz 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(devid)
self.clawControl.serialOperation(self.com_dev, 115200, False)
@@ -107,6 +113,7 @@ class JzDevNode(Node):
def main():
rclpy.init()
node=JzDevNode()
node=JzDevNode()
rclpy.spin(node)
rclpy.shutdown()
pass

13
jz_dev/launch/jz_dev.launch.py
View File

@@ -1,15 +1,6 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='jz_dev',
executable='jz_dev_node',
name='imu_node',
parameters=[{
'port': '/dev/ttyUSB0',
}]
)
])
Node(package="meta_dev",executable="meta_dev_node",name="meta_dev_node",output="screen",parameters=[])
])

31
meta_dev/CMakeLists.txt Normal file
View 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
View 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
View 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
View 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;
}

145
motor_dev/src/main.cpp
View File

@@ -3,68 +3,58 @@
#include"interfaces/msg/motor_cmd.hpp"
#include"interfaces/msg/motor_pos.hpp"
#include"interfaces/srv/motor_param.hpp"
#include"interfaces/srv/motor_info.hpp"
#include <unistd.h>
using namespace motor_dev;
RS485Driver rs485_driver_;
static auto g_program_start_time = std::chrono::steady_clock::now();
// 带时间戳的 printf 风格日志函数
void log_printf(const char* format, ...) {
auto now = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
double seconds = elapsed.count();
char buffer[1024];
va_list args;
va_start(args, format);
int n = vsnprintf(buffer, sizeof(buffer), format, args);
va_end(args);
if (n < 0) {
std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
return;
}
std::fprintf(stdout, "[%.3f] %s\n", seconds, buffer);
std::fflush(stdout); // 确保立即输出（对调试很重要）
}
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/ttyS1", 115200)){
if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
std::cout << "Failed to open RS485 port." << std::endl;
}
usleep(500000);
rs485_driver_.bm_set_enable(1,1);
rs485_driver_.bm_set_enable(2,1);
usleep(100000);
rs485_driver_.bm_set_enable(1);
rs485_driver_.bm_set_param(1,28,2);
rs485_driver_.bm_set_param(2,28,2);
rs485_driver_.bm_set_param(3,28,2);
rs485_driver_.bm_set_param(4,28,2);
rs485_driver_.bm_set_enable(2);
rs485_driver_.bm_set_param(1,84,1);
rs485_driver_.bm_set_param(2,84,1);
rs485_driver_.bm_set_param(3,84,1);
rs485_driver_.bm_set_param(4,84,1);
usleep(1000000);
rs485_driver_.bm_set_speed(8.0f,10,10,10);
int idx=0;
while(idx<20){
rs485_driver_.bm_get_speed();
usleep(100000);
idx+=1;
}
///usleep(20000000);
rs485_driver_.bm_set_speed(0,0,0,0);
idx=0;
while(idx<10){
rs485_driver_.bm_get_speed();
usleep(100000);
idx+=1;
}
//rs485_driver_.bm_set_enable(2,1);
//rs485_driver_.bm_set_enable(3,1);
//rs485_driver_.bm_set_enable(4,1);
return 0;
rs485_driver_.bm_set_param(1,84,10);
rs485_driver_.bm_set_param(2,84,10);
rs485_driver_.bm_set_mode(1,4);
rs485_driver_.bm_set_enable(1,2);
rs485_driver_.bm_set_mode(2,4);
rs485_driver_.bm_set_enable(2,2);
rs485_driver_.bm_set_param(1,77,40);
rs485_driver_.bm_set_param(1,78,10);
rs485_driver_.bm_set_param(1,79,10);
//rs485_driver_.bm_set_param(1,28,4);
rs485_driver_.bm_set_param(2,77,40);
rs485_driver_.bm_set_param(2,78,10);
rs485_driver_.bm_set_param(2,79,10);
//rs485_driver_.mt_set_pos(1,-5);
//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){
///
/// printf("##############-----------------------");
if (msg->target == "rs485") {
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
@@ -72,35 +62,31 @@ 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);
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];
printf("##############:%s,speed:%.1f,%.1f\n",msg->type.c_str(),speed1,speed2);
//rs485_driver_.bm_set_pos(angle1,angle2);
rs485_driver_.bm_set_speed(speed1,speed2,0,0);
}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);
}
}else if(msg->target=="ethercat"){
//TODO
}
});
#if 1
auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() {
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() {
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 speed1=rs485_driver_.bm_get_speed(1);
//float speed2=rs485_driver_.bm_get_speed(2);
//float angle1=rs485_driver_.bm_get_pos(1);
//float angle2=rs485_driver_.bm_get_pos(2);
#if 0
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
@@ -115,15 +101,14 @@ 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_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;
log_printf("pub pos:%.1f,%.1f",angle1,angle2);
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,speed1,speed2);
}
#endif
pub_cnt+=1;
#if 0
motor_pos.motor_id.clear();
@@ -137,13 +122,11 @@ int main(int argc,char* argv[]){
}
#endif
});
#endif
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){
uint8_t mid=req->motor_id;
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)
{
printf("recv sv:%d,%d,%d,%d\n",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);
@@ -153,22 +136,8 @@ int main(int argc,char* argv[]){
}
}
});
#if 0
auto motor_info_srv=node->create_service<interfaces::srv::MotorInfo>("motor_info",[=](const std::shared_ptr<interfaces::srv::MotorInfo::Request> req,std::shared_ptr<interfaces::srv::MotorInfo::Response> res){
printf("recv motor_info:%s,%d\n",req->type.c_str(),req->motor_ids[0]);
res->ret=false;
if(req->type=="bm"){
res->motor_angles.clear();
for(auto d:req->motor_ids){
float angle=rs485_driver_.bm_get_pos(d);
res->motor_angles.push_back(angle);
printf("motor_info %d, angle:%.1f\n",d,angle);
}
res->ret=true;
}
});
#endif
rclcpp::spin(node);
printf("now close rs485 port\n");

261
motor_dev/src/rs485_driver.cpp
View File

@@ -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, 40, 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\n");
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,24 @@ 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);
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);
}
}
return -1;
return 0;
}
//mt
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
@@ -590,7 +557,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 +598,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;