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

del main.cpp

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-25 15:38:37 +08:00
parent fac220ccf4
commit ee826f8042
1 changed files with 0 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
motor_dev/src/main.cpp
View File

@@ -1,174 +0,0 @@
#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"
#include"interfaces/srv/motor_info.hpp"
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)){
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);
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,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);//位置模式
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();
if(n<2){
printf("bm msg need two 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];
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];
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), [=]() {
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);
///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_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_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;
// log_printf("pub pos:%.1f,%.1f",angle1,angle2);
}
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
});
#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)
{
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
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");
rs485_driver_.closePort();
rclcpp::shutdown();
}