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

change speed mode

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-28 18:35:21 +08:00
parent 28cd3f03fa
commit 81ec425d21
3 changed files with 115 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -29,7 +29,9 @@ namespace motor_dev
int bm_set_enable(uint8_t motor_id,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);
float bm_get_pos(uint8_t motor_id);
float bm_get_speed(uint8_t motor_id);
int bm_query_id();
int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);
@@ -38,7 +40,7 @@ namespace motor_dev
void add_crc16(std::vector<uint8_t> &data);
int mt_set_pos(uint8_t motor_id, float angle);
float mt_get_pos(uint8_t motor_id);
private:
int com_fd_;
//bool is_open_;

39
motor_dev/src/main.cpp
View File

@@ -14,12 +14,15 @@ int main(int argc,char* argv[]){
if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
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);
usleep(500000);
rs485_driver_.bm_set_enable(1,1);
rs485_driver_.bm_set_enable(2,1);
rs485_driver_.bm_set_mode(1,3);
rs485_driver_.bm_set_enable(1,2);
rs485_driver_.bm_set_mode(2,3);
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);
@@ -39,10 +42,14 @@ int main(int argc,char* argv[]){
printf("bm msg need two angle\n");
return;
}
float angle1 =msg->motor_angle[0];
float angle2 =msg->motor_angle[1];
printf("##############:%s,%.1f,%.1f\n",msg->type.c_str(),angle1,angle2);
rs485_driver_.bm_set_pos(angle1,angle2);
//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);
}else if(msg->type=="mt"){
uint8_t id=msg->motor_id[0];
float angle =msg->motor_angle[0];
@@ -57,13 +64,17 @@ int main(int argc,char* argv[]){
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);
///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)
{
@@ -72,12 +83,12 @@ int main(int argc,char* argv[]){
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;
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,angle1,angle2);
printf("[%03d]=====%.1f,%.1f\n",pub_cnt,speed1,speed2);
}
pub_cnt+=1;
#if 0

88
motor_dev/src/rs485_driver.cpp
View File

@@ -388,7 +388,44 @@ namespace motor_dev
}
return 0;
}
int RS485Driver::bm_set_speed(float speed1,float speed2){
std::vector<uint8_t> command;
//if(angle>360.0)
// angle=360.0;
int32_t pos=speed1*10;
uint8_t high1 = (pos >> 8) & 0xFF; // 高8位
uint8_t low1 = pos & 0xFF; // 低8位
pos=speed2*10;
uint8_t high2 = (pos >> 8) & 0xFF; // 高8位
uint8_t low2 = pos & 0xFF; // 低8位
//if(motor_id<5){
command.push_back(0x00);
command.push_back(0x32);
command.push_back(high1);
command.push_back(low1);
command.push_back(high2);
command.push_back(low2);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
//}
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
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;
}
float RS485Driver::bm_get_pos(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(0);
@@ -433,6 +470,55 @@ namespace motor_dev
}
return -1;
}
float RS485Driver::bm_get_speed(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x35);
command.push_back(motor_id);
//command.push_back(0x0d);
//command.push_back(4);
//command.push_back(0x0b);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(1);
command.push_back(0);
command.push_back(0);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 50);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
uint8_t nid=0x70+motor_id;
if(data[0]==0&&data[1]==nid){
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
//float angle=360.0*pos/32768.0;
int16_t pos=(data[6]<<8)+(data[7]&0xff);
float angle=3.60f*pos;
pos=(data[8]<<8)+(data[9]&0xff);
float speed=pos/10.0f;
//printf("id:%d,pos:%d\n",motor_id,pos);
return speed;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}
else{
}
//float angle = pos * 360.0 /255.0;
///float angle=0;
}
}
return -1;
}
//mt
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
uint16_t crc = 0xFFFF;