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

del msg

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-30 18:35:57 +08:00
parent 67615b7e9d
commit 9f64856540
3 changed files with 43 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
motor_dev/src/main.cpp
View File

@@ -7,6 +7,31 @@
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");
@@ -24,13 +49,13 @@ int main(int argc,char* argv[]){
rs485_driver_.bm_set_mode(2,4);
rs485_driver_.bm_set_enable(2,2);
//rs485_driver_.bm_set_param(1,77,10);
//rs485_driver_.bm_set_param(1,78,2);
//rs485_driver_.bm_set_param(1,79,2);
rs485_driver_.bm_set_param(1,77,15);
rs485_driver_.bm_set_param(1,78,4);
rs485_driver_.bm_set_param(1,79,4);
//rs485_driver_.bm_set_param(1,28,4);
rs485_driver_.bm_set_param(2,77,10);
rs485_driver_.bm_set_param(2,78,2);
rs485_driver_.bm_set_param(2,79,2);
rs485_driver_.bm_set_param(2,77,15);
rs485_driver_.bm_set_param(2,78,4);
rs485_driver_.bm_set_param(2,79,4);
//rs485_driver_.mt_set_pos(1,-5);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
@@ -49,8 +74,8 @@ int main(int argc,char* argv[]){
// float speed1=msg->motor_speed[0];
//float speed2=msg->motor_speed[1];
printf("####vvv##########:%s,speed:%.1f,%.1f\n",msg->type.c_str(),angle1,angle2);
////rs485_driver_.bm_set_pos(angle1,angle2);
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];
@@ -61,11 +86,12 @@ int main(int argc,char* argv[]){
//TODO
}
});
#if 0
#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);
@@ -93,7 +119,7 @@ int main(int argc,char* argv[]){
motor_pub->publish(motor_pos);
//std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,angle1,angle2);
log_printf("pub pos:%.1f,%.1f",angle1,angle2);
}
pub_cnt+=1;
#if 0
@@ -112,8 +138,9 @@ int main(int argc,char* argv[]){
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){
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);
@@ -123,6 +150,7 @@ 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;
@@ -137,7 +165,7 @@ auto motor_info_srv=node->create_service<interfaces::srv::MotorInfo>("motor_info
}
});
#endif
rclcpp::spin(node);
printf("now close rs485 port\n");