From 7196f541b79596766a4ce7a46180d1e66a76b934 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 30 Sep 2023 19:19:14 +0800 Subject: [PATCH] update --- .../mini_servo_motor_ctrl_module.cpp | 28 ++++++++++++++++++---- .../mini_servo_motor_ctrl_module.hpp | 7 ++++-- .../step_motor_ctrl_module.cpp | 2 +- .../step_motor_ctrl_module.hpp | 2 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 2 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 2 +- .../zcan_step_motor_ctrl_module.cpp | 2 +- .../zcancmder_module/zcan_xy_robot_module.cpp | 2 +- components/zprotocols/zcancmder | 2 +- os/zthread.cpp | 25 +++++++++---------- os/zthread.hpp | 13 ++++++++++ 11 files changed, 62 insertions(+), 25 deletions(-) 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 4f4fa39..2e6101f 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -55,10 +55,29 @@ int32_t MiniRobotCtrlModule::read_version(version_t& version) { return 0; } int32_t MiniRobotCtrlModule::read_status(status_t& status) { - - - return 0; } -int32_t MiniRobotCtrlModule::read_debug_info(debug_info_t& debug_info) { return 0; } + if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; + + FeiTeServoMotor::detailed_status_t feitestatus; + if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::kce_subdevice_overtime; + status.torque = feitestatus.torque; + status.speed = feitestatus.vel; + status.pos = feitestatus.pos; + status.io_state = 0; + if (feitestatus.state != 0) { + status.status = 3; + } else { + status.status = m_thread.isworking(); + } + return 0; +} +int32_t MiniRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) { + if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; + FeiTeServoMotor::status_t feitestatus; + if (!m_bus->read_status(m_id, &feitestatus)) return err::kce_subdevice_overtime; + + + return 0; +} int32_t MiniRobotCtrlModule::set_run_param(run_param_t& param) { return 0; } int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return 0; } @@ -67,3 +86,4 @@ int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& para int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; } int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; } int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; } +uint8_t MiniRobotCtrlModule::get_module_status() { ; } diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index 4a004df..f67f4bc 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -10,7 +10,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule { FeiTeServoMotor* m_bus; uint8_t m_id; ZThread m_thread; - s32 m_pos_shift; + s32 m_pos_shift; public: void initialize(FeiTeServoMotor* bus, uint8_t id); @@ -27,7 +27,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule { virtual int32_t read_version(version_t& version) override; virtual int32_t read_status(status_t& status) override; - virtual int32_t read_debug_info(debug_info_t& debug_info) override; + virtual int32_t read_debug_info(detailed_status_t& debug_info) override; virtual int32_t set_run_param(run_param_t& param) override; virtual int32_t set_basic_param(basic_param_t& param) override; @@ -36,5 +36,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule { virtual int32_t get_run_param(run_param_t& param) override; virtual int32_t get_basic_param(basic_param_t& param) override; virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override; + + private: + uint8_t get_module_status(); }; } // namespace iflytop \ No newline at end of file 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 e42ef3a..ae8bb11 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -208,7 +208,7 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) { getnowpos(status.x); return 0; } -int32_t StepMotorCtrlModule::read_debug_info(debug_info_t& debug_info) { +int32_t StepMotorCtrlModule::read_debug_info(detailed_status_t& debug_info) { debug_info.status = m_status; if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 31fa999..a3f16b5 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -58,7 +58,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual int32_t read_version(version_t& version) override; virtual int32_t read_status(status_t& status) override; - virtual int32_t read_debug_info(debug_info_t& debug_info) override; + virtual int32_t read_debug_info(detailed_status_t& debug_info) override; virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; 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 3cf77a4..00b9492 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -199,7 +199,7 @@ int32_t XYRobotCtrlModule::read_status(status_t& status) { getnowpos(status.x, status.y); return 0; } -int32_t XYRobotCtrlModule::read_debug_info(debug_info_t& debug_info) { +int32_t XYRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) { zlock_guard lock(m_lock); debug_info = {0}; if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 8542565..302a158 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -70,7 +70,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { virtual int32_t read_version(version_t& version) override; virtual int32_t read_status(status_t& status) override; - virtual int32_t read_debug_info(debug_info_t& debug_info) override; + virtual int32_t read_debug_info(detailed_status_t& debug_info) override; virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp index 23459ca..e5bdb28 100644 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp @@ -95,7 +95,7 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); - PROCESS_PACKET(kcmd_step_motor_ctrl_read_debug_info, m_id) { // + PROCESS_PACKET(kcmd_step_motor_ctrl_read_detailed_status, m_id) { // errorcode = m_module->read_debug_info(ack->ack); } END_PP(); diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index 6c18229..a4d5094 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -86,7 +86,7 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_read_debug_info, m_id) { // + PROCESS_PACKET(kcmd_xy_robot_ctrl_read_detailed_status, m_id) { // errorcode = m_xyRobotCtrlModule->read_debug_info(ack->ack); } END_PP(); diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 8a33d6e..2393d97 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 8a33d6eeef9b51f409f6364235c7dbc26556fab5 +Subproject commit 2393d97dec8c2e235087f1eaf239e5ad4ebb9f2f diff --git a/os/zthread.cpp b/os/zthread.cpp index 6d5f041..2edca3b 100644 --- a/os/zthread.cpp +++ b/os/zthread.cpp @@ -7,24 +7,25 @@ using namespace std; static void zosthread_default_task(void const *argument) { ZThread *thread = (ZThread *)argument; ZASSERT(thread); - thread->m_threadisworkingFlag = false; - thread->m_threadisWaitingForStopFlag = false; + thread->threadcb(); +}; +void ZThread::threadcb() { + m_status = kidle; while (true) { - if (thread->m_threadisworkingFlagCallSide) { - thread->m_threadisworkingFlag = true; - thread->m_taskfunction(); + if (m_threadisworkingFlagCallSide) { + m_status = kworking; + m_taskfunction(); + m_status = kdead; - thread->m_threadisWaitingForStopFlag = true; - while (thread->m_threadisworkingFlagCallSide) { + while (m_threadisworkingFlagCallSide) { vTaskDelay(10); } - thread->m_threadisworkingFlag = false; - thread->m_threadisWaitingForStopFlag = false; + m_status = kidle; } vTaskDelay(10); } -}; +} void ZThread::init(const char *threadname, int stack_size, osPriority priority) { int r_task_create = 0; @@ -52,7 +53,7 @@ void ZThread::start(zosthread_cb_t cb) { xSemaphoreTake(m_lock, portMAX_DELAY); m_threadisworkingFlagCallSide = true; // xEventGroupSetBits(m_zthreadstartworkevent, 0x01); - while (!m_threadisworkingFlag) { + while (m_status != kidle) { xTaskNotifyGive(m_defaultTaskHandle); vTaskDelay(1); } @@ -63,7 +64,7 @@ void ZThread::stop() { xSemaphoreTake(m_lock, portMAX_DELAY); m_threadisworkingFlagCallSide = false; // xEventGroupSetBits(m_zthreadstartworkevent, 0x01); - while (m_threadisworkingFlag) { + while (m_status != kidle) { xTaskNotifyGive(m_defaultTaskHandle); vTaskDelay(1); } diff --git a/os/zthread.hpp b/os/zthread.hpp index 00acfb9..db4a7e2 100644 --- a/os/zthread.hpp +++ b/os/zthread.hpp @@ -6,12 +6,21 @@ typedef function zosthread_cb_t; class ZThread { public: + typedef enum { + kidle, + kworking, + kdead, + } status_t; + const char* m_name; size_t m_stacksize; osPriority m_uxPriority; + status_t m_status = kidle; +#if 0 bool m_threadisworkingFlag = false; bool m_threadisWaitingForStopFlag = false; +#endif bool m_threadisworkingFlagCallSide = false; zosthread_cb_t m_taskfunction; @@ -27,9 +36,13 @@ class ZThread { void stop(); bool getExitFlag() { return !m_threadisworkingFlagCallSide; } + bool isworking() { return m_status == kworking; } void sleep(uint32_t ms); void wake(); + + public: + void threadcb(); }; } // namespace iflytop