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

can speed mode

Browse Source
This commit is contained in:
hehe
2025-11-25 18:14:00 +08:00
parent 6b6552ebea
commit 78c8894335
3 changed files with 41 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
motor_dev/include/motor_dev/rs485_driver.hpp
View File

@@ -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);

108
motor_dev/src/main.cpp
View File

@@ -15,42 +15,23 @@ int main(int argc,char* argv[]){
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,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_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,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,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);
rs485_driver_.bm_set_param(1,84,10);
rs485_driver_.bm_set_param(2,84,10);
//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){
@@ -59,17 +40,15 @@ int main(int argc,char* argv[]){
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
if(n<2){
printf("bm msg need two angle\n");
printf("bm msg need four angle\n");
return;
}
//float angle1 =msg->motor_angle[0];
//float angle2 =msg->motor_angle[1];
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);
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];
@@ -82,58 +61,33 @@ int main(int argc,char* argv[]){
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() {
static int pub_cnt=0;
interfaces::msg::MotorPos motor_pos;
//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
rs485_driver_.bm_get_speed();
motor_pos.motor_id.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)
{
motor_pos.source = "rs485";
motor_pos.type = "bm";
motor_pos.position="none";
motor_pos.motor_id.push_back(1);
motor_pos.motor_id.push_back(2);
motor_pos.motor_speed.push_back(speed1);
motor_pos.motor_speed.push_back(speed2);
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);
//std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,speed1,speed2);
}
#endif
pub_cnt+=1;
#if 0
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
float angle=rs485_driver_.mt_get_pos(1);
if(angle!=9999){
motor_pos.type = "mt";
motor_pos.motor_id.push_back(1);
motor_pos.motor_angle.push_back(angle);
motor_pub->publish(motor_pos);
}
#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;
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);
rs485_driver_.bm_set_param(mid,79,req->dec_acc);
res->ret=0;
printf("svc:%d\n",res->ret);
}
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);
}
});

6
motor_dev/src/rs485_driver.cpp
View File

@@ -44,7 +44,7 @@ namespace motor_dev
std::vector<uint8_t> rx_data;
if(ret){
rx_data.clear();
ret=receiveData(rx_data, 40, 1);
ret=receiveData(rx_data, 100, 1);
if(ret){
printf("<---:");
for(auto d:rx_data)
@@ -398,7 +398,7 @@ namespace motor_dev
command.push_back(low3);
command.push_back(high4);
command.push_back(low4);
printf("set_speed\n");
printf("set_speed.%.1f\n",speed1);
sendCan(0x32,command);
return 0;
}
@@ -486,11 +486,13 @@ namespace motor_dev
command.push_back(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 0;
}