From bc42f5a410170b36fd503b41eb6a39991fc8ca39 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Fri, 6 Oct 2023 10:12:07 +0800 Subject: [PATCH] update --- .../mini_servo_motor_ctrl_module.cpp | 10 +-- .../motor_laser_code_scanne.cpp | 85 ++++++++++++++++++++++ .../motor_laser_code_scanne.hpp | 37 ++++++++++ components/sensors/smtp2/smtp2.cpp | 12 +-- .../step_motor_ctrl_module.cpp | 10 +-- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 8 +- components/zcancmder/zcanreceiver.hpp | 2 +- .../zcan_motor_laser_code_scanner_scan_module.cpp | 63 ++++++++++++++++ .../zcan_motor_laser_code_scanner_scan_module.hpp | 19 +++++ components/zprotocols/errorcode | 2 +- components/zprotocols/zcancmder | 2 +- 11 files changed, 227 insertions(+), 23 deletions(-) create mode 100644 components/motor_laser_code_scanner/motor_laser_code_scanne.cpp create mode 100644 components/motor_laser_code_scanner/motor_laser_code_scanne.hpp create mode 100644 components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp create mode 100644 components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.hpp diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 9c9a299..a2a71f8 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -28,7 +28,7 @@ int32_t MiniRobotCtrlModule::stop(u8 stop_type) { return err::ksucc; } int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { - if (nowpos < 0 || nowpos > 4095) return err::kcommon_error_param_out_of_range; + if (nowpos < 0 || nowpos > 4095) return err::kce_param_out_of_range; if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime; return 0; @@ -47,7 +47,7 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_ while (true) { if (m_thread.getExitFlag() && run_time != 0) { stop(0); - call_status_cb(status_cb, err::kcommon_error_break_by_user); + call_status_cb(status_cb, err::kce_break_by_user); break; } @@ -94,7 +94,7 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s if (m_thread.getExitFlag()) { stop(0); - call_status_cb(status_cb, err::kcommon_error_break_by_user); + call_status_cb(status_cb, err::kce_break_by_user); break; } m_thread.sleep(10); @@ -143,7 +143,7 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb while (true) { if (m_thread.getExitFlag() && run_time != 0) { stop(0); - call_status_cb(status_cb, err::kcommon_error_break_by_user); + call_status_cb(status_cb, err::kce_break_by_user); break; } if (m_thread.getExitFlag() && run_time == 0) { @@ -165,7 +165,7 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb return 0; } -int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { return err::kcommon_error_operation_not_support; } +int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { return err::kce_operation_not_support; } int32_t MiniRobotCtrlModule::read_version(version_t& version) { uint8_t mainversion, subversion, miniserv_mainversion, miniserv_subversion; diff --git a/components/motor_laser_code_scanner/motor_laser_code_scanne.cpp b/components/motor_laser_code_scanner/motor_laser_code_scanne.cpp new file mode 100644 index 0000000..f0ef408 --- /dev/null +++ b/components/motor_laser_code_scanner/motor_laser_code_scanne.cpp @@ -0,0 +1,85 @@ +#include "motor_laser_code_scanne.hpp" + +#include +#include + +#include "sdk\components\zprotocols\errorcode\errorcode.hpp" +using namespace iflytop; +using namespace std; +#define TAG "MotorLaserCodeScanner" + +#define DO(infostr, ACTION) \ + { \ + int exec_ret = ACTION; \ + if (exec_ret != 0) { \ + ZLOGE(TAG, "do " infostr "(line:%d) fail, ret = %d", __LINE__, exec_ret); \ + m_lastexecstatus = exec_ret; \ + if (status_cb) status_cb(exec_ret); \ + return; \ + } \ + } + +void MotorLaserCodeScanner::initialize(int id, I_StepMotorCtrlModule* stepM, int max_scan_result_num) { + m_id = id; + m_stepM1 = stepM; + + m_mallocsize = sizeof(scan_result_t) + 2 * max_scan_result_num; + m_scan_result = (scan_result_t*)malloc(m_mallocsize); + ZASSERT(m_scan_result != nullptr); + memset(m_scan_result, 0, m_mallocsize); + m_max_scan_result_num = max_scan_result_num; + + m_lock.init(); + m_thread.init(TAG, 1024, osPriorityNormal); +} + +int32_t MotorLaserCodeScanner::start_scan(s32 moveby_distance, // + s32 scan_interval_distance, // + s32 each_sample_times, // + s32 transmitting_tube_amplification, // + s32 receiving_tube_amplification, // + // + action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "start_scan %d %d %d %d %d", moveby_distance, scan_interval_distance, each_sample_times, transmitting_tube_amplification, receiving_tube_amplification); + + m_thread.stop(); + memset(m_scan_result, 0, m_mallocsize); + m_thread.start([this, // + moveby_distance, // + scan_interval_distance, // + each_sample_times, // + transmitting_tube_amplification, // + receiving_tube_amplification, // + status_cb]() { + int32_t count = moveby_distance / scan_interval_distance; + int32_t each_move_by_distance = moveby_distance > 0 ? abs(scan_interval_distance) : -abs(scan_interval_distance); + + for (int32_t i = 0; i < count; i++) { + ZLOGI(TAG, "move to %d", each_move_by_distance); + DO("move_by", m_stepM1->move_by_block(each_move_by_distance)); + // read adc + } + m_scan_result->each_scan_result_len = 2; + m_scan_result->scan_reult_nums = count; + call_action_cb(status_cb, 0); + }); + return 0; +} +int32_t MotorLaserCodeScanner::stop_scan() { + zlock_guard lock(m_lock); + ZLOGI(TAG, "stop_scan"); + m_thread.stop(); + return 0; +}; + +int32_t MotorLaserCodeScanner::get_scan_result(scan_result_t** result) { + if (m_thread.isworking()) return err::kce_device_is_busy; + *result = m_scan_result; + return 0; +} + +void MotorLaserCodeScanner::call_action_cb(action_cb_status_t status_cb, int32_t status) { + m_lastexecstatus = status; + if (status_cb) status_cb(status); +} diff --git a/components/motor_laser_code_scanner/motor_laser_code_scanne.hpp b/components/motor_laser_code_scanner/motor_laser_code_scanne.hpp new file mode 100644 index 0000000..e51d2d3 --- /dev/null +++ b/components/motor_laser_code_scanner/motor_laser_code_scanne.hpp @@ -0,0 +1,37 @@ +#pragma once +// +#include "sdk/os/zos.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_motor_laser_code_scanner.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" +namespace iflytop { +class MotorLaserCodeScanner : public I_MotorLaserCodeScanner { + ZThread m_thread; + int m_id = 0; + I_StepMotorCtrlModule* m_stepM1 = nullptr; + zmutex m_lock; + + scan_result_t* m_scan_result = nullptr; + int m_max_scan_result_num = 0; + int m_mallocsize = 0; + + int32_t m_lastexecstatus = 0; + + public: + void initialize(int id, I_StepMotorCtrlModule* stepM, int max_scan_result_num); + + virtual int32_t start_scan(s32 moveby_distance, // + s32 scan_interval_distance, // + s32 each_sample_times, // + s32 transmitting_tube_amplification, // + s32 receiving_tube_amplification, // + // + action_cb_status_t status_cb) override; + // + virtual int32_t stop_scan() override; + // + virtual int32_t get_scan_result(scan_result_t** result) override; + + private: + void call_action_cb(action_cb_status_t status_cb, int32_t status); +}; +}; // namespace iflytop diff --git a/components/sensors/smtp2/smtp2.cpp b/components/sensors/smtp2/smtp2.cpp index 670c8cc..cf5c8c4 100644 --- a/components/sensors/smtp2/smtp2.cpp +++ b/components/sensors/smtp2/smtp2.cpp @@ -254,15 +254,15 @@ int32_t SMTP2::getState() { int ret = getState(isbusy, error); if (ret == 0) { if (isbusy) { - return err::kcommon_error_device_is_busy; + return err::kce_device_is_busy; } return error; } - return err::kcommon_error_device_is_offline; + return err::kce_device_is_offline; } bool SMTP2::isOnline() { - if (getState() != err::kcommon_error_device_is_offline) { + if (getState() != err::kce_device_is_offline) { return true; } return false; @@ -274,7 +274,7 @@ bool SMTP2::isOnline() { iflytop::err::error_t SMTP2::read_ack_error_code(char* rxbuf, size_t rxlen) { if (rxlen < 3) { - return err::kcommon_error_device_is_offline; + return err::kce_device_is_offline; } uint8_t errorcode = (uint8_t)rxbuf[2]; if (errorcode > '`') { @@ -408,7 +408,7 @@ int32_t SMTP2::doaction_block(function action) { // 等待移液枪复位完成 while (true) { int32_t state = getState(); - if (state != err::kcommon_error_device_is_busy) { + if (state != err::kce_device_is_busy) { if (state == 0) { stop(); return 0; @@ -419,7 +419,7 @@ int32_t SMTP2::doaction_block(function action) { } if (thisThread.getExitFlag()) { stop(); - return err::kcommon_error_break_by_user; + return err::kce_break_by_user; } thisThread.sleep(1000); } diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 4eec01a..9c646cc 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -12,7 +12,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g m_lock.init(); m_statu_lock.init(); - m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); + m_thread.init(TAG, 1024, osPriorityNormal); #if 0 run_param_t run_param; @@ -179,7 +179,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ if (lastforms < 0) { ZLOGW(TAG, "lastforms:%d < 0", lastforms); - return err::kcommon_error_param_out_of_range; + return err::kce_param_out_of_range; } if (abs(speed) > m_param.maxspeed) { @@ -304,7 +304,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { } if (m_Xgpio->getState()) { ZLOGE(TAG, "leave away zero failed"); - return err::kcommon_error_x_leave_away_zero_point_fail; + return err::kce_x_leave_away_zero_point_fail; } } /******************************************************************************* @@ -325,7 +325,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { if (!xreach_zero) { // 触发异常回调 ZLOGE(TAG, "x reach zero failed"); - return err::kcommon_error_not_found_x_zero_point; + return err::kce_not_found_x_zero_point; } if (m_thread.getExitFlag()) { @@ -422,7 +422,7 @@ int32_t StepMotorCtrlModule::do_motor_action_block_2(function action, if (isbusy()) { stop(0); - return err::kcommon_error_break_by_user; + return err::kce_break_by_user; } return get_last_exec_status(); diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index c1fc84b..6bd7f84 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -278,7 +278,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { } if (m_Xgpio->getState()) { ZLOGE(TAG, "leave away zero failed"); - return err::kcommon_error_x_leave_away_zero_point_fail; + return err::kce_x_leave_away_zero_point_fail; } } /******************************************************************************* @@ -299,7 +299,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { if (!xreach_zero) { // 触发异常回调 ZLOGE(TAG, "x reach zero failed"); - return err::kcommon_error_not_found_x_zero_point; + return err::kce_not_found_x_zero_point; } if (m_thread.getExitFlag()) { @@ -328,7 +328,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { } if (m_Ygpio->getState()) { ZLOGE(TAG, "leave away zero failed"); - return err::kcommon_error_y_leave_away_zero_point_fail; + return err::kce_y_leave_away_zero_point_fail; } } @@ -349,7 +349,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { if (!yreach_zero) { // 触发异常回调 ZLOGE(TAG, "y reach zero failed"); - return err::kcommon_error_not_found_y_zero_point; + return err::kce_not_found_y_zero_point; } if (m_thread.getExitFlag()) { ZLOGI(TAG, "break move to zero thread exit"); diff --git a/components/zcancmder/zcanreceiver.hpp b/components/zcancmder/zcanreceiver.hpp index ebd37c4..5ff04ee 100644 --- a/components/zcancmder/zcanreceiver.hpp +++ b/components/zcancmder/zcanreceiver.hpp @@ -73,7 +73,7 @@ class ZCanCmder : public ZCanIRQListener { int packetRxOvertime_ms; // }; - uint8_t txbuff[1000]; + uint8_t txbuff[2100]; public: class LoopJobContext { diff --git a/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp new file mode 100644 index 0000000..aa0f423 --- /dev/null +++ b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp @@ -0,0 +1,63 @@ +#include "zcan_motor_laser_code_scanner_scan_module.hpp" +using namespace iflytop; + +#define TAG "ZcanMotorLaserCodeScannerScanModule" +#if 1 +void ZcanMotorLaserCodeScannerScanModule::initialize(ZCanCmder* cancmder, int id, I_MotorLaserCodeScanner* module) { + m_cancmder = cancmder; + m_id = id; + m_module = module; + cancmder->registerListener(this); +} +void ZcanMotorLaserCodeScannerScanModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { +#if 0 +ZPACKET_CMD_ACK_AND_REPORT(kcmd_motor_laser_code_scanner_scan, + CMD(u8 id; // + s32 moveby_distance; // + s32 scan_interval_distance; // + s32 each_sample_times; // + s32 transmitting_tube_amplification; // 0.001 + s32 receiving_tube_amplification; // 0.001 + ), + ACK(u8 id;), REPORT(u8 id; int32_t exec_status;)); +ZPACKET_CMD_ACK(kcmd_motor_laser_code_scanner_stop_scan, CMD(u8 id;), ACK(u8 id;)); +ZPACKET_CMD_ACK(kcmd_motor_laser_code_scanner_get_scan_result, CMD(u8 id; u16 packetoff;), ACK(u8 id; u8 endpacket; u8 packetlen; u8 packet[];)); +#endif + + PROCESS_PACKET(kcmd_motor_laser_code_scanner_scan, m_id) { + errorcode = m_module->start_scan( // + cmd->moveby_distance, cmd->scan_interval_distance, cmd->each_sample_times, cmd->transmitting_tube_amplification, cmd->receiving_tube_amplification, // + [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_motor_laser_code_scanner_scan); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_motor_laser_code_scanner_stop_scan, m_id) { errorcode = m_module->stop_scan(); } + END_PP(); + + if (rxcmd->iscmd(kcmd_motor_laser_code_scanner_get_scan_result)) { + #if 0 + auto* cmd = rxcmd->get_data_as(); + auto cmdheader = rxcmd->get_cmdheader(); + uint32_t errorcode = 0; + if (cmd->id == m_id) { + + I_MotorLaserCodeScanner::scan_result_t* result = nullptr; + u8 packetoff = cmd->packetoff; + errorcode = m_module->get_scan_result(&result); + + + if (errorcode == 0) { + } + // scan_result_t* result; + if (errorcode == 0) { + m_cancmder->sendAck(rxcmd->get_cmdheader(), m_txbuf, sizeof(*ack)); + } else { + m_cancmder->sendErrorAck(rxcmd->get_cmdheader(), errorcode); + } + return; + + } + #endif + } +} +#endif \ No newline at end of file diff --git a/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.hpp b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.hpp new file mode 100644 index 0000000..43c3793 --- /dev/null +++ b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.hpp @@ -0,0 +1,19 @@ +#pragma once +// #include "sdk/components/zprotocols/zcancmder/api/i_step_motor_ctrl_module.hpp" +// #include "sdk/components/zprotocols/zcancmder/api/i_pipette_module.hpp" +#include "sdk/components/zprotocols/zcancmder/api/i_motor_laser_code_scanner.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" +namespace iflytop { + +class ZcanMotorLaserCodeScannerScanModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + I_MotorLaserCodeScanner* m_module; + + uint8_t m_txbuf[512] = {0}; + + public: + void initialize(ZCanCmder* cancmder, int id, I_MotorLaserCodeScanner* module); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop diff --git a/components/zprotocols/errorcode b/components/zprotocols/errorcode index fd9bffb..3ea1062 160000 --- a/components/zprotocols/errorcode +++ b/components/zprotocols/errorcode @@ -1 +1 @@ -Subproject commit fd9bffbcf3c637591eca6b5b72f7a5f9fe5670b3 +Subproject commit 3ea10620670f4d40e220ed2ac75df734065fe3a6 diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index b59413a..6ee2257 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit b59413a0e50fa100ab191dd0b2f7c9eb549e259c +Subproject commit 6ee22571832bbd90cdd5f4edb16e7a6d570e8baf