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

code update

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-29 17:00:27 +08:00
parent 38a4c4a193
commit 67615b7e9d
12 changed files with 134 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
drv_test/CMakeLists.txt
View File

@@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(motor_dev REQUIRED)
find_package(interfaces REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(motor_test src/main.cpp)
ament_target_dependencies(motor_test rclcpp motor_dev interfaces)

23
drv_test/src/main.cpp
View File

@@ -2,6 +2,7 @@
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
@@ -14,15 +15,18 @@ public:
: Node("motor_test_node")
{
publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
//publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
timer_ = this->create_wall_timer(100ms, std::bind(&MotorCmdPublisher::timer_callback, this));
timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this));
}
private:
int angle1=9999;
int angle2=6666;
int xcnt=0;
void timer_callback()
{
#if 1
auto message = MotorCmd();
message.target = "rs485";
@@ -44,14 +48,25 @@ private:
message.motor_angle.push_back((float)angle1);
message.motor_angle.push_back((float)angle2);
publisher_->publish(message);
RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2);
timer_->cancel();
#endif
#if 0
auto message=std_msgs::msg::String();
xcnt+=1;
if(xcnt%2==0)
message.data="green,10";
else
message.data="blue,10";
publisher_->publish(message);
RCLCPP_INFO(this->get_logger(), "Published LedCmd message:%s",message.data.c_str());
#endif
//timer_->cancel();
}
int idx=0;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
//rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};
int main(int argc, char * argv[])

14
ethercat_dev/src/ethercat_control/CMakeLists.txt
View File

@@ -35,9 +35,9 @@ add_executable(ethercat_node src/ethercat_node.cpp)
target_link_libraries(ethercat_node ethercat_core)
ament_target_dependencies(ethercat_node rclcpp std_msgs sensor_msgs rcl_interfaces)
add_executable(ethercat_csp_test src/ethercat_csp_test.cpp)
ament_target_dependencies(ethercat_csp_test rclcpp std_msgs)
install(TARGETS ethercat_csp_test DESTINATION lib/${PROJECT_NAME})
# add_executable(ethercat_csp_test src/ethercat_csp_test.cpp)
# ament_target_dependencies(ethercat_csp_test rclcpp std_msgs)
# install(TARGETS ethercat_csp_test DESTINATION lib/${PROJECT_NAME})
install(TARGETS ethercat_node
@@ -45,11 +45,11 @@ install(TARGETS ethercat_node
install(DIRECTORY include/
DESTINATION include)
# 测试程序
add_executable(ethercat_topic_test src/ethercat_topic_test.cpp)
ament_target_dependencies(ethercat_topic_test rclcpp std_msgs)
# add_executable(ethercat_topic_test src/ethercat_topic_test.cpp)
# ament_target_dependencies(ethercat_topic_test rclcpp std_msgs)
install(TARGETS ethercat_topic_test
DESTINATION lib/${PROJECT_NAME})
# install(TARGETS ethercat_topic_test
# DESTINATION lib/${PROJECT_NAME})

2
ethercat_dev/src/ethercat_control/include/ethercat_control/motor_control.hpp
View File

@@ -4,7 +4,7 @@
#include <cstdint>
#include <algorithm>
constexpr int NUM_SLAVES = 6; //定义电机数量
constexpr int NUM_SLAVES = 2; //定义电机数量
//运行模式
enum class OpMode : int8_t {

30
ethercat_dev/src/ethercat_control/src/ethercat_core.cpp
View File

@@ -287,7 +287,7 @@ void cyclic_task()
// 读取从站状态字、当前位置、速度、扭矩、运行模式
for (int i = 0; i < NUM_SLAVES; ++i)
{
if (i == 2)
if (i == 0)
// if (false)
{
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
@@ -350,6 +350,7 @@ void cyclic_task()
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
std::cout << " Motor : " << i << " Ready to switch on" << std::endl;
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
@@ -359,9 +360,12 @@ void cyclic_task()
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
std::cout << " Motor : " << i << " Switched on" << std::endl;
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
std::cout << " Motor : " << i << " Operation enabled" << std::endl;
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
@@ -505,6 +509,7 @@ void cyclic_task()
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
std::cout << " Motor : " << i << " Ready to switch on" << std::endl;
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
@@ -513,9 +518,12 @@ void cyclic_task()
int32_t pv2 = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv2);
g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
std::cout << " Motor : " << i << " Switched on" << std::endl;
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
std::cout << " Motor : " << i << " Operation enabled" << std::endl;
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
@@ -671,12 +679,12 @@ bool start()
int position = i+1;
if (i > 1)
{
position = i+2;
}
// if (i > 0)
// {
// position = i+2;
// }
if (i == 2)
if (i == 0)
{
sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID);
if (!sc[i])
@@ -715,12 +723,12 @@ bool start()
int position = i+1;
if (i > 1)
{
position = i+2;
}
// if (i > 1)
// {
// position = i+2;
// }
if (i == 2)
if (i == 0)
{
// ---- CSP/CSV/CST PDO ----
const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]);

46
ethercat_dev/src/ethercat_control/src/ethercat_node.cpp
View File

@@ -196,29 +196,29 @@ private:
continue;
}
switch (idx)
{
case 0:
idx = 2;
break;
case 1:
idx = 3;
break;
case 2:
idx = 0;
break;
case 3:
idx = 1;
break;
case 4:
idx = 5;
break;
case 5:
idx = 4;
break;
default:
break;
}
// switch (idx)
// {
// case 0:
// idx = 2;
// break;
// case 1:
// idx = 3;
// break;
// case 2:
// idx = 0;
// break;
// case 3:
// idx = 1;
// break;
// case 4:
// idx = 5;
// break;
// case 5:
// idx = 4;
// break;
// default:
// break;
// }
mc_set_mode(idx, OpMode::CSP);
mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));

46
ethercat_dev/src/ethercat_control/src/motor_control.cpp
View File

@@ -116,29 +116,29 @@ void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t
// This should be done in HAL
// slave index transport into joint index.
switch (idx)
{
case 0:
idx = 2;
break;
case 1:
idx = 3;
break;
case 2:
idx = 0;
break;
case 3:
idx = 1;
break;
case 4:
idx = 5;
break;
case 5:
idx = 4;
break;
default:
break;
}
// switch (idx)
// {
// case 0:
// idx = 2;
// break;
// case 1:
// idx = 3;
// break;
// case 2:
// idx = 0;
// break;
// case 3:
// idx = 1;
// break;
// case 4:
// idx = 5;
// break;
// case 5:
// idx = 4;
// break;
// default:
// break;
// }
status = g_state.status_word[idx].load(std::memory_order_relaxed);
pos = g_state.pos_act[idx].load(std::memory_order_relaxed);

15
imu_dev/src/main.cpp
View File

@@ -18,6 +18,21 @@ int main(int argc, char **argv)
imu_msg.imu=*msg;
imu_pub->publish(imu_msg);
});
///livox/imu
auto imu_sub_1=node->create_subscription<sensor_msgs::msg::Imu>("/livox/imu",10,[=](const sensor_msgs::msg::Imu::SharedPtr msg){
interfaces::msg::ImuMsg imu_msg;
imu_msg.source="livox";
imu_msg.position="front";
imu_msg.type="None";
imu_msg.imu=*msg;
imu_pub->publish(imu_msg);
});
rclcpp::spin(node);
rclcpp::shutdown();
}

BIN
jz_dev/resource/JodellTool-0.1.5-py3-none-any.whl Normal file
View File

Binary file not shown.

22
motor_dev/src/main.cpp
View File

@@ -3,6 +3,7 @@
#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_;
@@ -35,7 +36,7 @@ int main(int argc,char* argv[]){
/////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("##############-----------------------");
/// printf("##############-----------------------");
if (msg->target == "rs485") {
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
@@ -48,8 +49,8 @@ int main(int argc,char* argv[]){
// 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);
printf("####vvv##########:%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];
@@ -60,6 +61,7 @@ int main(int argc,char* argv[]){
//TODO
}
});
#if 0
auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() {
@@ -106,6 +108,7 @@ int main(int argc,char* argv[]){
}
#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;
@@ -120,6 +123,19 @@ int main(int argc,char* argv[]){
}
}
});
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;
}
});

6
motor_dev/src/rs485_driver.cpp
View File

@@ -375,6 +375,7 @@ namespace motor_dev
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
#if 0
if(ret){
std::vector<uint8_t> data;
data.clear();
@@ -386,6 +387,7 @@ namespace motor_dev
printf("\n");
}
}
#endif
return 0;
}
int RS485Driver::bm_set_speed(float speed1,float speed2){
@@ -416,7 +418,7 @@ namespace motor_dev
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
ret=receiveData(data, 20, 5);
if(ret){
printf("<---:");
for(auto d:data)
@@ -444,7 +446,7 @@ namespace motor_dev
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 50);
ret=receiveData(data, 11, 1);
if(ret){
printf("<---:");
for(auto d:data)

6
voice_control/src/iat_publish.cpp
View File

@@ -67,7 +67,7 @@ static void demo_mic(const char* session_begin_params)
printf("start listen failed %d\n", errcode);
}
/* demo 8 seconds recording */
while(i++ < 8)
while(i++ < 3)
sleep(1);
errcode = sr_stop_listening(&iat);
if (errcode) {
@@ -127,11 +127,11 @@ int main(int argc, char* argv[])
{
// 语音识别唤醒
RCLCPP_INFO(node->get_logger(), "开始从麦克风识别语音");
RCLCPP_INFO(node->get_logger(), "8秒内请说话...");
RCLCPP_INFO(node->get_logger(), "3秒内请说话...");
demo_mic(session_begin_params);
RCLCPP_INFO(node->get_logger(), "8秒已过");
RCLCPP_INFO(node->get_logger(), "3秒已过");
// 语音识别完成，发布结果
if (resultFlag) {