From 095f1035ecc7ec96044d72254102d54d0f475665 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 22 Oct 2023 17:27:10 +0800 Subject: [PATCH] update --- sdk | 2 +- usrc/intelligent_winding_robot_ctrl.cpp | 118 +++++++++++++++++++++++ usrc/intelligent_winding_robot_ctrl.hpp | 161 ++++++++++++++++++++++++++++++++ usrc/main.cpp | 38 ++++++-- 4 files changed, 311 insertions(+), 8 deletions(-) create mode 100644 usrc/intelligent_winding_robot_ctrl.cpp create mode 100644 usrc/intelligent_winding_robot_ctrl.hpp diff --git a/sdk b/sdk index 089069f..e06be65 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 089069f6ef3ae015b315835f83f520e800b785b1 +Subproject commit e06be65d256688d5ab0018e4d195352654081205 diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp new file mode 100644 index 0000000..eca9d05 --- /dev/null +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -0,0 +1,118 @@ +#include "intelligent_winding_robot_ctrl.hpp" +#include +using namespace std; +using namespace iflytop; + +#define TAG "IntelligentWindingRobotCtrl" + +int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; } +// 排废舵机 +int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() { return 0; } +int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() { return 0; } +/** + * @brief 绕线探测舵机 + */ +int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() { return 0; } +int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness() { return 0; } +/** + * @brief 压线舵机 + */ +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() { return 0; } +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { return 0; } +/** + * @brief 线拉紧舵机 + */ +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() { return 0; } // 零位 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() { return 0; } // 入线位 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() { return 0; } // 拉线位置 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { return 0; } // 放线位置 +/** + * @brief 夹线舵机 + */ +int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { return 0; } +/** + * @brief 剪刀 + */ +int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() { return 0; } // block +int32_t IntelligentWindingRobotCtrl::scissors_cut() { return 0; } // block +/** + * @brief 机械臂夹线舵机 + */ +int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { return 0; } + +/** + * @brief 机械臂钩爪 + */ +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() { return 0; } +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() { return 0; } +/** + * @brief XY平台 + */ +int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; } +/** + * @brief Z轴 + */ +int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; } +int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; } + +int32_t IntelligentWindingRobotCtrl::xy_reset() { return 0; } // 复位 +int32_t IntelligentWindingRobotCtrl::xy_move_to_zero() { return 0; } // 移动到零位 +int32_t IntelligentWindingRobotCtrl::xy_take_clip(int32_t index) { return 0; } // 取弹夹 +int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线 +int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹 +int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } // 移除线 +/** + * @brief + */ +int32_t IntelligentWindingRobotCtrl::start_winding() { return 0; } +int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; } +int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; } + +int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; } +int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } + +int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) { + m_dm = dm; + m_cmdparse = cmdparse; + + cmdparse->regCMD("paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); }); + cmdparse->regCMD("paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); }); + cmdparse->regCMD("raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); }); + cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_get_thickness(); }); + cmdparse->regCMD("yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); }); + cmdparse->regCMD("yaxian_duoji_move_to_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_press_pos(); }); + cmdparse->regCMD("yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_wait_for_press_pos(); }); + cmdparse->regCMD("xianlajin_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_reset(); }); + cmdparse->regCMD("xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_line_entry_pos(); }); + cmdparse->regCMD("xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_tight_line_pos(); }); + cmdparse->regCMD("xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_loose_line_pos(); }); + cmdparse->regCMD("jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_reset_pos(); }); + cmdparse->regCMD("jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_clamp_pos(); }); + cmdparse->regCMD("scissors_move_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_move_reset_pos(); }); + cmdparse->regCMD("scissors_cut", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_cut(); }); + cmdparse->regCMD("arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_reset_pos(); }); + cmdparse->regCMD("arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_clamp_pos(); }); + cmdparse->regCMD("arm_hook_claws_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_reset(); }); + cmdparse->regCMD("arm_hook_claws_move_to_half_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_half_pos(); }); + cmdparse->regCMD("arm_hook_claws_move_to_full_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_full_pos(); }); + cmdparse->regCMD("xy_platform_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_platform_reset(); }); + cmdparse->regCMD("z_axis_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_reset(); }); + cmdparse->regCMD("z_axis_move_to", "(int32_t pos)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_move_to(atoi(paraV[0])); }); + cmdparse->regCMD("xy_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_reset(); }); + cmdparse->regCMD("xy_move_to_zero", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_move_to_zero(); }); + cmdparse->regCMD("xy_take_clip", "(int32_t index)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_clip(atoi(paraV[0])); }); + cmdparse->regCMD("xy_take_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_line(); }); + cmdparse->regCMD("xy_take_back_clip", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_back_clip(); }); + cmdparse->regCMD("xy_remove_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_remove_line(); }); + cmdparse->regCMD("start_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return start_winding(); }); + cmdparse->regCMD("stop_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return stop_winding(); }); + cmdparse->regCMD("reset_and_check_device", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return reset_and_check_device(); }); + cmdparse->regCMD("setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return setcfg(paraV[0], atoi(paraV[1])); }); + cmdparse->regCMD("dumpcfg", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return dumpcfg(); }); + + return 0; +} diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp new file mode 100644 index 0000000..20aa28b --- /dev/null +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -0,0 +1,161 @@ +#pragma once +#include "sdk/os/zos.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" + +namespace iflytop { +using namespace std; +class IntelligentWindingRobotCtrl { + public: + typedef struct { + /** + * @brief 排废舵机 + */ + int32_t paifei_duoji_reset_pos; + int32_t paifei_duoji_press_pos; + int32_t paifei_duoji_press_torque; + + /** + * @brief 绕线探测舵机 + */ + int32_t raoxiantance_duoji_reset_pos; + int32_t raoxiantance_duoji_tance_zero_pos; + + /** + * @brief 压线舵机 + */ + int32_t yaxian_duoji_reset_pos; + int32_t yaxian_duoji_press_pos; + int32_t yaxian_duoji_press_torque; + int32_t yaxian_duoji_wait_for_press_pos; + + /** + * @brief 线拉紧舵机 + */ + int32_t xianlajin_duoji_reset_pos; + int32_t xianlajin_duoji_line_entry_pos; + int32_t xianlajin_duoji_tight_line_pos; + int32_t xianlajin_duoji_loose_line_pos; + + /** + * @brief 线夹紧舵机 + */ + int32_t jiaxian_duoji_reset_pos; + int32_t jiaxian_duoji_clamp_pos; + int32_t jiaxian_duoji_clamp_torque; + + /** + * @brief 机械臂夹线舵机 + */ + int32_t arm_jiaxian_duoji_reset_pos; + int32_t arm_jiaxian_duoji_clamp_pos; + int32_t arm_jiaxian_duoji_clamp_torque; + + /** + * @brief 剪刀 + */ + int32_t scissors_reset_pos; + int32_t scissors_cut_pos; + + /** + * @brief 钩爪 + */ + int32_t arm_hook_claws_half_pos; + int32_t arm_hook_claws_full_pos; + + /** + * @brief Z轴定位 + */ + int32_t z_axis_take_line_pos; + int32_t z_axis_take_clip_pos; + int32_t z_axis_winding_hight; + + /** + * @brief XY平台 + */ + + int32_t xy_platform_winding_pos_x; + int32_t xy_platform_winding_pos_y; + + int32_t xy_platform_takeline_pos_x; + int32_t xy_platform_takeline_pos_y; + + } config_t; + + ZModuleDeviceManager* m_dm; + ICmdParser* m_cmdparse; + + public: + int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); + int32_t initialize_device(); + // 排废舵机 + int32_t paifei_duoji_moveto_reset(); + int32_t paifei_duoji_moveto_press(); + /** + * @brief 绕线探测舵机 + */ + int32_t raoxiantance_duoji_move_to_reset(); + int32_t raoxiantance_duoji_move_to_get_thickness(); + /** + * @brief 压线舵机 + */ + int32_t yaxian_duoji_move_to_reset(); + int32_t yaxian_duoji_move_to_press_pos(); + int32_t yaxian_duoji_move_to_wait_for_press_pos(); + /** + * @brief 线拉紧舵机 + */ + int32_t xianlajin_duoji_move_to_reset(); // 零位 + int32_t xianlajin_duoji_move_to_line_entry_pos(); // 入线位 + int32_t xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置 + int32_t xianlajin_duoji_move_to_loose_line_pos(); // 放线位置 + /** + * @brief 夹线舵机 + */ + int32_t jiaxian_duoji_move_to_reset_pos(); + int32_t jiaxian_duoji_move_to_clamp_pos(); + /** + * @brief 剪刀 + */ + int32_t scissors_move_reset_pos(); // block + int32_t scissors_cut(); // block + /** + * @brief 机械臂夹线舵机 + */ + int32_t arm_jiaxian_duoji_move_to_reset_pos(); + int32_t arm_jiaxian_duoji_move_to_clamp_pos(); + + /** + * @brief 机械臂钩爪 + */ + int32_t arm_hook_claws_reset(); + int32_t arm_hook_claws_move_to_half_pos(); + int32_t arm_hook_claws_move_to_full_pos(); + /** + * @brief XY平台 + */ + int32_t xy_platform_reset(); + /** + * @brief Z轴 + */ + int32_t z_axis_reset(); + int32_t z_axis_move_to(int32_t pos); + + int32_t xy_reset(); // 复位 + int32_t xy_move_to_zero(); // 移动到零位 + int32_t xy_take_clip(int32_t index); // 取弹夹 + int32_t xy_take_line(); // 取线 + int32_t xy_take_back_clip(); // 放弹夹 + int32_t xy_remove_line(); // 移除线 + /** + * @brief + */ + int32_t start_winding(); + int32_t stop_winding(); + int32_t reset_and_check_device(); + + int32_t setcfg(const char* cfgname, int32_t cfgvalue); + int32_t dumpcfg(); +}; + +} // namespace iflytop diff --git a/usrc/main.cpp b/usrc/main.cpp index 0545025..2a9b391 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -26,6 +26,8 @@ // #include "sdk\components\zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp" // #include "sdk\components\zcancmder_master_module\zcan_xy_robot_master_module.hpp" +#include "intelligent_winding_robot_ctrl.hpp" +#include "sdk\components\taojingchi_screen\taojingchi_screen_service.hpp" #include "sdk\components\zprotocol_helper\micro_computer_module_device_script_cmder_paser.hpp" #include "sdk\components\zprotocols\zcancmder_v2\protocol_proxy.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" @@ -141,13 +143,15 @@ namespace iflytop { /******************************************************************************* * 基础组件 * *******************************************************************************/ +ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线 +ModbusBlockHost g_modbusblockhost; // modbus总线 +FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线 +ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备 +StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器 + CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线 -ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线 -ModbusBlockHost g_modbusblockhost; // modbus总线 -FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线 -ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备 +TaoJingChiScreenService g_taojingchi_screen_service; // 淘精驰屏幕服务 MicroComputerModuleDeviceScriptCmderPaser g_zmodule_device_script_cmder_paser; // 用于解析所有的设备指令 -StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器 /******************************************************************************* * 本地设备 * @@ -161,6 +165,8 @@ MiniRobotCtrlModule g_mini_servo[6]; ZIProtocolProxy g_xyrobotctrlmodule; ZIProtocolProxy g_z_step_motor; +IntelligentWindingRobotCtrl g_intelligent_winding_robot_ctrl; + } // namespace iflytop extern "C" { @@ -185,7 +191,6 @@ void Main::run() { /******************************************************************************* * 总线初始化 * *******************************************************************************/ - g_cmdScheduler.initialize(&DEBUG_UART, 1000); // 串口字符串指令总线 auto* cfg = m_zcanCommnaderMaster.createCFG(); // can总线配置 m_zcanCommnaderMaster.init(cfg); // can总线 g_modbusblockhost.initialize(&huart2, &hdma_usart2_tx, &hdma_usart2_rx); // modbus总线 @@ -194,7 +199,6 @@ void Main::run() { // 设备管理器初始化 g_zmodule_device_manager.initialize(&m_zcanCommnaderMaster); // 设备指令解析器初始化 - g_zmodule_device_script_cmder_paser.initialize(&g_cmdScheduler, &g_zmodule_device_manager); /******************************************************************************* * 设备构造和初始化 * @@ -228,6 +232,26 @@ void Main::run() { g_zmodule_device_manager.registerModule(&g_step_motor45[1]); g_zmodule_device_manager.registerModule(&g_step_motor45[2]); + g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler); + + /******************************************************************************* + * 串口字符串指令总线 * + *******************************************************************************/ + g_cmdScheduler.initialize(&DEBUG_UART, 1000); // + g_zmodule_device_script_cmder_paser.initialize(&g_cmdScheduler, &g_zmodule_device_manager); + g_taojingchi_screen_service.initialize( + &huart5, 1, + /** + * @brief 私有协议解析 + */ + [this](const char* cmd, int32_t paramN, const char** paraV) { // + ZLOGI(TAG, "process cmd:%s", cmd); + }, + /** + * @brief 陶晶驰消息解析 + */ + [this](uint8_t* data, size_t len) {}); + #if 0 step_motor_cmd_reg(); #endif