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

motor param

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-22 20:49:55 +08:00
parent 28f9f7edd7
commit 59eac59814
3 changed files with 49 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -31,6 +31,7 @@ namespace motor_dev
int bm_set_pos(float angle1, float angle2);
float bm_get_pos(uint8_t motor_id);
int bm_query_id();
int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);
//脉塔
int mt_query_id();

15
motor_dev/src/main.cpp
View File

@@ -18,6 +18,14 @@ int main(int argc,char* argv[]){
rs485_driver_.bm_set_enable(1,2);
rs485_driver_.bm_set_mode(2,4);
rs485_driver_.bm_set_enable(2,2);
usleep(500000);
rs485_driver_.bm_set_param(1,77,10);
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,10);
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);//位置模式
@@ -55,7 +63,7 @@ int main(int argc,char* argv[]){
//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();
printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
if(angle1>-1&&angle2>-1)
{
motor_pos.source = "rs485";
@@ -67,9 +75,11 @@ int main(int argc,char* argv[]){
motor_pos.motor_angle.push_back(angle2);
motor_pub->publish(motor_pos);
//std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,angle1,angle2);
}
pub_cnt+=1;
#if 1
#if 0
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
float angle=rs485_driver_.mt_get_pos(1);
@@ -78,7 +88,6 @@ int main(int argc,char* argv[]){
motor_pos.motor_id.push_back(1);
motor_pos.motor_angle.push_back(angle);
motor_pub->publish(motor_pos);
std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle<<std::endl;
}
#endif
});

38
motor_dev/src/rs485_driver.cpp
View File

@@ -289,6 +289,37 @@ namespace motor_dev
}
return 0;
}
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);
@@ -364,8 +395,8 @@ namespace motor_dev
command.push_back(0x35);
command.push_back(motor_id);
command.push_back(0x0d);
command.push_back(1);
command.push_back(2);
command.push_back(4);
command.push_back(0x0b);
command.push_back(3);
command.push_back(0);
command.push_back(0);
@@ -386,6 +417,9 @@ namespace motor_dev
if(data[0]==0&&data[1]==nid){
uint16_t pos = (data[2]<<8)+(data[3]&0xff);
float angle=360.0*pos/32768.0;
pos=(data[6]<<8)+(data[7]&0xff);
angle=3.60f*pos;
//printf("id:%d,pos:%d\n",motor_id,pos);
return angle;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}