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

change speed to angle

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-29 15:25:37 +08:00
parent 862d685e06
commit 38a4c4a193
1 changed files with 23 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
motor_dev/src/main.cpp
View File

@@ -16,16 +16,16 @@ int main(int argc,char* argv[]){
}
usleep(500000);
rs485_driver_.bm_set_enable(1,1);
rs485_driver_.bm_set_enable(2,1);
rs485_driver_.bm_set_enable(2,1);
rs485_driver_.bm_set_mode(1,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,10);
rs485_driver_.bm_set_param(1,78,2);
rs485_driver_.bm_set_param(1,79,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,28,4);
rs485_driver_.bm_set_param(2,77,10);
rs485_driver_.bm_set_param(2,78,2);
@@ -35,6 +35,7 @@ rs485_driver_.bm_set_enable(2,1);
/////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();
@@ -42,14 +43,14 @@ rs485_driver_.bm_set_enable(2,1);
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];
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);
// float speed1=msg->motor_speed[0];
//float speed2=msg->motor_speed[1];
printf("##############:%s,speed:%.1f,%.1f\n",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];
@@ -64,11 +65,11 @@ rs485_driver_.bm_set_enable(2,1);
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() {
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);
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
@@ -83,12 +84,14 @@ rs485_driver_.bm_set_enable(2,1);
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);
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;
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,speed1,speed2);
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,angle1,angle2);
}
pub_cnt+=1;
#if 0