From 4589e9842532fa397c23d637c5d285aa07ea0715 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 5 Jun 2024 18:28:33 +0800 Subject: [PATCH] update --- .../mini_servo_motor_ctrl_module.cpp | 1 - .../step_motor_ctrl_module.cpp | 377 +++++++++++++-------- .../step_motor_ctrl_module.hpp | 33 +- components/tmc/ic/ztmc5130.cpp | 54 ++- components/tmc/ic/ztmc5130.hpp | 101 +++--- .../water_cooling_temperature_control_module.cpp | 23 +- components/zcancmder/zcan_protocol_parser.cpp | 8 + components/zcancmder/zcan_protocol_parser.hpp | 2 + 8 files changed, 366 insertions(+), 233 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 8739299..fd9745e 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -176,7 +176,6 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t * void MiniServoCtrlModule::befor_motor_move() { creg.m_module_status = 1; creg.module_errorcode = 0; - creg.module_errorbitflag0 = 0; } void MiniServoCtrlModule::after_motor_move() { if (creg.module_errorcode != 0) { 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 3734a00..11d4d6d 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -5,6 +5,7 @@ #include "a8000_protocol\protocol.hpp" #include "sdk\components\flash\znvs.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" using namespace iflytop; #define TAG "SMCM" @@ -23,6 +24,11 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i step_motor_active_cfg(); step_motor_enable(true); + + TMC5130* tmcmotor = dynamic_cast(m_stepM1); + if (tmcmotor) { + tmcmotor->getGState(); // 读取状态，清空下复位标识 + } } void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); @@ -64,7 +70,7 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t case kreg_step_motor_iholddelay: case kreg_step_motor_max_d: case kreg_step_motor_min_d: - step_motor_active_cfg(); + return 0; } return suc; } @@ -113,125 +119,6 @@ int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) { m_stepM1->stop(); return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { - ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); - m_thread.stop(); - if (!m_state.enable) { - return err::kstep_motor_not_enable; - } - - m_thread.start( - [this, direction]() { - befor_motor_move(); - - { - m_stepM1->setAcceleration(m_cfg.motor_default_acc); - m_stepM1->setDeceleration(m_cfg.motor_default_dec); - - int32_t vel = m_cfg.motor_default_velocity; - if (direction <= 0) vel = -vel; - m_stepM1->rotate(vel); - - while (true) { - if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; - vTaskDelay(5); - } - } - - after_motor_move(); - }, - [this]() { m_stepM1->stop(); }); - - return 0; -} -int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { - ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox); - m_thread.stop(); - if (!m_state.enable) { - return err::kstep_motor_not_enable; - } - - if (m_cfg.min_d != 0 && tox < m_cfg.min_d) tox = m_cfg.min_d; - if (m_cfg.max_d != 0 && tox > m_cfg.max_d) tox = m_cfg.max_d; - - m_thread.start( - [this, tox]() { - befor_motor_move(); - - { - m_stepM1->setAcceleration(m_cfg.motor_default_acc); - m_stepM1->setDeceleration(m_cfg.motor_default_dec); - int32_t motor_pos = 0; - inter_forward_kinematics(tox, motor_pos); - m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity); - - while (!m_stepM1->isReachTarget()) { - if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; - - vTaskDelay(5); - } - } - - after_motor_move(); - }, - [this, tox]() { m_stepM1->stop(); }); - - return 0; -} - -int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { - ZLOGI(TAG, "m%d motor_move_by %d", m_id, distance); - int32_t motor_pos = 0; - step_motor_read_pos(&motor_pos); - motor_pos += distance; - return step_motor_easy_move_to(motor_pos); -} - -int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { - ZLOGI(TAG, "m%d motor_move_to_zero", m_id); - m_thread.stop(); - if (!m_state.enable) { - return err::kstep_motor_not_enable; - } - - if (m_nio == 0) { - return err::kstep_motor_ioindex_out_of_range; - } - - m_thread.start( - [this]() { - befor_motor_move(); - exec_move_to_io_task(0, -1); - after_motor_move(); - m_stepM1->setXACTUAL(0); - }, - [this]() { m_stepM1->stop(); }); - return 0; -} - -int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) { - ZLOGI(TAG, "m%d motor_move_to_io %d %d", m_id, ioindex, direction); - m_thread.stop(); - if (!m_state.enable) { - return err::kstep_motor_not_enable; - } - - if (ioindex < 0 || ioindex >= m_nio) { - return err::kstep_motor_ioindex_out_of_range; - } - - m_thread.start( - [this, ioindex, direction]() { - befor_motor_move(); - exec_move_to_io_task(ioindex, direction); - after_motor_move(); - }, - [this]() { m_stepM1->stop(); }); - - return 0; -} int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { int32_t motor_pos = 0; @@ -263,6 +150,11 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t* return 0; } +int32_t StepMotorCtrlModule::step_motor_read_motoric_status(int32_t* errorflag) { + *errorflag = m_state.motorStatus; + return 0; +} + /*********************************************************************************************************************** * INTER * ***********************************************************************************************************************/ @@ -279,10 +171,12 @@ int StepMotorCtrlModule::inter_get_pos(int32_t& x) { return 0; } void StepMotorCtrlModule::befor_motor_move() { // - m_state.before_move_pos = m_stepM1->getXACTUAL(); - creg.m_module_status = 1; - creg.module_errorcode = 0; - creg.module_errorbitflag0 = 0; + m_state.before_move_pos = m_stepM1->getXACTUAL(); + creg.m_module_status = 1; + creg.module_errorcode = 0; + + step_motor_active_cfg(); + m_stepM1->enable(1); } void StepMotorCtrlModule::after_motor_move() { m_state.after_move_pos = m_stepM1->getXACTUAL(); @@ -292,9 +186,53 @@ void StepMotorCtrlModule::after_motor_move() { } creg.m_module_status = 0; } -bool StepMotorCtrlModule::check_when_run() { return true; } -int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) { +void StepMotorCtrlModule::dumpTMC5130Status(TMC5130::DevStatusReg_t* status) { + ZLOGE(TAG, "sg_result: %d", status->sg_result); + ZLOGE(TAG, "fsactive: %d", status->fsactive); + ZLOGE(TAG, "cs_actual: %d", status->cs_actual); + ZLOGE(TAG, "stallguard: %d", status->stallguard); + ZLOGE(TAG, "ot: %d", status->ot); + ZLOGE(TAG, "otpw: %d", status->otpw); + ZLOGE(TAG, "s2ga: %d", status->s2ga); + ZLOGE(TAG, "s2gb: %d", status->s2gb); + ZLOGE(TAG, "ola: %d", status->ola); + ZLOGE(TAG, "olb: %d", status->olb); + ZLOGE(TAG, "stst: %d", status->stst); +} + +void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) { + creg.module_errorcode = ecode; + m_state.motorStatus = motorStatue; +} + +bool StepMotorCtrlModule::check_when_run() { + // + TMC5130* tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + auto state = tmc5130->getGState(); + auto devStatus = tmc5130->getDevStatus(); + if (state.reset) { + ZLOGE(TAG, "motor reset when run"); + setErrorFlag(err::kstep_motor_subic_reset, (*(uint32_t*)&devStatus)); + return false; + } else if (state.uv_cp) { + ZLOGE(TAG, "motor uv_cp when run"); + dumpTMC5130Status(&devStatus); + setErrorFlag(err::kstep_motor_uv_cp, (*(uint32_t*)&devStatus)); + return false; + } else if (state.drv_err) { + ZLOGE(TAG, "motor drv_err when run"); + setErrorFlag(err::kstep_motor_drv_err, (*(uint32_t*)&devStatus)); + dumpTMC5130Status(&devStatus); + return false; + } + } + + return true; +} + +bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) { int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed; int32_t runToPointDec = m_cfg.motor_run_to_zero_dec; @@ -304,6 +242,7 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc direction = direction >= 0 ? 1 : -1; ZGPIO* gpio = &m_iotable[ioindex]; + if (!check_when_run()) return false; if (!gpio->getState()) { ZLOGI(TAG, "---------STEP1-------- move to point"); @@ -328,10 +267,13 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc // 失败返回 if (!xreach_io) { ZLOGE(TAG, "x reach io failed"); - return err::kxymotor_not_found_x_zero_point; + setErrorFlag(err::kstep_motor_not_found_zero_point, 0); + return false; } } + if (!check_when_run()) return false; + // 如果设备已经在零点，则反向移动一定距离远离零点 if (gpio->getState()) { ZLOGI(TAG, "---------STEP2-------- find edge"); @@ -354,17 +296,20 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc if (!reach_edge) { ZLOGE(TAG, "leave away zero failed"); - return err::kxymotor_x_find_zero_edge_fail; + setErrorFlag(err::kstep_motor_not_found_point_edge, 0); + return false; } } + if (!check_when_run()) return false; + if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return err::kmodule_opeation_break_by_user; + ZLOGW(TAG, "break move to zero thread exit"); + return true; } ZLOGI(TAG, "_exec_move_to_io_task_fn success"); - return 0; + return true; } void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { @@ -372,4 +317,172 @@ void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { m_stepM1->setAcceleration(acc); m_stepM1->setDeceleration(dec); m_stepM1->rotate(vel); -} \ No newline at end of file +} + +/*********************************************************************************************************************** + * 电机控制接口 * + ***********************************************************************************************************************/ + +int32_t StepMotorCtrlModule::module_clear_error() { + /** + * @brief + * 驱动器大部分的错误，在不重启的情况下，是无法清除的。module_clear_error这里不生效 + */ + return 0; +} + +int32_t StepMotorCtrlModule::check_befor_run() { + if (!m_state.enable) { + return err::kstep_motor_not_enable; + } + + if (creg.module_errorcode == err::kstep_motor_subic_reset) { + return creg.module_errorcode; + } else if (creg.module_errorcode == err::kstep_motor_drv_err) { + return creg.module_errorcode; + } else if (creg.module_errorcode == err::kstep_motor_uv_cp) { + return creg.module_errorcode; + } else { + /** + * @brief 其他错误允许设备继续运行 + */ + return 0; + } + + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { + ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); + + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + + return do_step_motor_easy_rotate(direction); +} +int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { + ZLOGI(TAG, "m%d motor_move_by %d", m_id, distance); + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + return do_step_motor_easy_move_by(distance); +} +int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { + ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox); + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + + return do_step_motor_easy_move_to(tox); +} +int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { + ZLOGI(TAG, "m%d motor_move_to_zero", m_id); + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + + if (m_nio == 0) { + return err::kstep_motor_ioindex_out_of_range; + } + return do_step_motor_easy_move_to_zero(); +} +int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) { + ZLOGI(TAG, "m%d motor_move_to_io %d %d", m_id, ioindex, direction); + + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + + if (ioindex < 0 || ioindex >= m_nio) { + return err::kstep_motor_ioindex_out_of_range; + } + return do_step_motor_easy_move_to_io(ioindex, direction); +} + +// +// +// + +int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { + m_thread.stop(); + m_thread.start( + [this, direction]() { + befor_motor_move(); + { + m_stepM1->setAcceleration(m_cfg.motor_default_acc); + m_stepM1->setDeceleration(m_cfg.motor_default_dec); + + int32_t vel = m_cfg.motor_default_velocity; + if (direction <= 0) vel = -vel; + m_stepM1->rotate(vel); + + while (true) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + vTaskDelay(5); + } + } + + after_motor_move(); + }, + [this]() { m_stepM1->stop(); }); + return 0; +} +int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) { + if (m_cfg.min_d != 0 && tox < m_cfg.min_d) tox = m_cfg.min_d; + if (m_cfg.max_d != 0 && tox > m_cfg.max_d) tox = m_cfg.max_d; + m_thread.stop(); + m_thread.start( + [this, tox]() { + befor_motor_move(); + + { + m_stepM1->setAcceleration(m_cfg.motor_default_acc); + m_stepM1->setDeceleration(m_cfg.motor_default_dec); + int32_t motor_pos = 0; + inter_forward_kinematics(tox, motor_pos); + m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity); + + while (!m_stepM1->isReachTarget()) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + + vTaskDelay(5); + } + } + + after_motor_move(); + }, + [this, tox]() { m_stepM1->stop(); }); + + return 0; +} + +int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) { + int32_t motor_pos = 0; + step_motor_read_pos(&motor_pos); + motor_pos += distance; + return step_motor_easy_move_to(motor_pos); +} + +int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() { + m_thread.stop(); + m_thread.start( + [this]() { + befor_motor_move(); + exec_move_to_io_task(0, -1); + after_motor_move(); + m_stepM1->setXACTUAL(0); + }, + [this]() { m_stepM1->stop(); }); + return 0; +} + +int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) { + m_thread.stop(); + m_thread.start( + [this, ioindex, direction]() { + befor_motor_move(); + exec_move_to_io_task(ioindex, direction); + after_motor_move(); + }, + [this]() { m_stepM1->stop(); }); + + return 0; +} 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 f758b9e..c1c0de3 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -3,6 +3,7 @@ #include "sdk/os/zos.hpp" #include "sdk\components\tmc\basic\tmc_ic_interface.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" namespace iflytop { class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { ENABLE_MODULE(StepMotorCtrlModule, ktmc_step_motor, 0x0001); @@ -30,10 +31,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { } config_t; typedef struct { - int32_t dpos; - int32_t after_move_pos; - int32_t before_move_pos; - int32_t enable; + int32_t dpos; + int32_t after_move_pos; + int32_t before_move_pos; + int32_t enable; + uint32_t motorStatus; } state_t; private: @@ -63,6 +65,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override; virtual int32_t module_stop() override { return step_motor_stop(0); } virtual int32_t module_active_cfg() override { return step_motor_active_cfg(); } + virtual int32_t module_clear_error() override; /*********************************************************************************************************************** * ZIStepMotor * ***********************************************************************************************************************/ @@ -74,16 +77,28 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_easy_move_by(int32_t distance) override; virtual int32_t step_motor_easy_move_to(int32_t position) override; virtual int32_t step_motor_easy_move_to_zero() override; - virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override; virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; + + virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override; virtual int32_t step_motor_active_cfg() override; virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override; + virtual int32_t step_motor_read_motoric_status(int32_t* errorflag) override; + public: IStepperMotor* getMotor() { return m_stepM1; } config_t* getCfg() { return &m_cfg; } private: + int32_t check_befor_run(); + + int32_t do_step_motor_easy_rotate(int32_t direction); + int32_t do_step_motor_easy_move_by(int32_t distance); + int32_t do_step_motor_easy_move_to(int32_t position); + int32_t do_step_motor_easy_move_to_zero(); + int32_t do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction); + + private: void inter_inverse_kinematics(int32_t motor_pos, int32_t& x); void inter_forward_kinematics(int32_t x, int32_t& motor_pos); @@ -94,9 +109,13 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { void after_motor_move(); bool check_when_run(); - int32_t exec_move_to_io_task(int32_t ioindex, int32_t direction); - void _rotate(int32_t vel, int32_t acc, int32_t dec); + bool exec_move_to_io_task(int32_t ioindex, int32_t direction); + void _rotate(int32_t vel, int32_t acc, int32_t dec); int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val); + + private: + void dumpTMC5130Status(TMC5130::DevStatusReg_t* status); + void setErrorFlag(int32_t ecode, uint32_t motorStatue); }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 465785e..0867e3a 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -46,39 +46,8 @@ void TMC5130::initialize(cfg_t *cfg) { } m_hspi = cfg->spi; - enableIC(false); - // tmc5130_init(&m_TMC5130, channel, &m_tmc_api_config, &tmc5130_defaultRegisterResetState[0]); - // tmc5130_setCallback(&m_TMC5130, pri_tmc4361A_callback); - + enable(false); reset(); - - writeInt(TMC5130_PWMCONF, 0x000500C8); - // writeInt( TMC5130_GCONF, 0x00000004); - writeInt(TMC5130_CHOPCONF, 0x000100c3); - writeInt(TMC5130_IHOLD_IRUN, 0x00051A00); - writeInt(TMC5130_PWMCONF, 0x000401c8); - - writeInt(TMC5130_XTARGET, 0); - writeInt(TMC5130_XACTUAL, 0x00000000); - writeInt(TMC5130_VACTUAL, 0x00000000); - - writeInt(TMC5130_VSTART, 100); - writeInt(TMC5130_A1, 1000); - writeInt(TMC5130_V1, 0); - - writeInt(TMC5130_D1, 1000); - writeInt(TMC5130_VSTOP, 100); - writeInt(TMC5130_TZEROWAIT, 0); - - // writeInt(TMC5130_VSTART, 100); - // writeInt(TMC5130_V1, 0); - // writeInt(TMC5130_VSTOP, 100); - // writeInt(TMC5130_TZEROWAIT, 0); - - setAcceleration(100); - setDeceleration(100); - setIHOLD_IRUN(2, 10, 1); - enableIC(true); } void TMC5130::setMRES(mres_type_t value) { @@ -118,7 +87,7 @@ void TMC5130::setNoAccLimit(bool enable) { } } -void TMC5130::enableIC(bool enable) { +void TMC5130::enable(bool enable) { // m_port->TMC5130Port_setENNPinState(m_channel, !enable); SET_PIN(m_ennpin, !enable); } @@ -137,6 +106,23 @@ uint8_t TMC5130::reset() { } writeInt(add, m_defaultRegisterResetState[add]); } + writeInt(TMC5130_PWMCONF, 0x000500C8); + writeInt(TMC5130_CHOPCONF, 0x000100c3); + writeInt(TMC5130_IHOLD_IRUN, 0x00051A00); + writeInt(TMC5130_PWMCONF, 0x000401c8); + writeInt(TMC5130_XTARGET, 0); + writeInt(TMC5130_XACTUAL, 0x00000000); + writeInt(TMC5130_VACTUAL, 0x00000000); + writeInt(TMC5130_VSTART, 100); + writeInt(TMC5130_A1, 1000); + writeInt(TMC5130_V1, 0); + writeInt(TMC5130_D1, 1000); + writeInt(TMC5130_VSTOP, 100); + writeInt(TMC5130_TZEROWAIT, 0); + setAcceleration(100); + setDeceleration(100); + setIHOLD_IRUN(2, 10, 1); + enable(false); return 0; } int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } @@ -181,8 +167,6 @@ void TMC5130::rotate(int32_t velocity) { writeInt(TMC5130_VMAX, abs(velocity)); writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); } -void TMC5130::right(int32_t velocity) { rotate(velocity); } -void TMC5130::left(int32_t velocity) { rotate(-velocity); } void TMC5130::moveTo(int32_t position, uint32_t velocityMax) { // position *= m_scale; // velocityMax *= m_scale; diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index a3194f1..47075bd 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -46,6 +46,28 @@ class Tmc5130RampStat { class TMC5130 : public IStepperMotor { public: typedef struct { + uint32_t sg_result : 10; + uint32_t reserved0 : 5; + uint32_t fsactive : 1; + uint32_t cs_actual : 5; + uint32_t reserved1 : 3; + uint32_t stallguard : 1; + uint32_t ot : 1; + uint32_t otpw : 1; + uint32_t s2ga : 1; + uint32_t s2gb : 1; + uint32_t ola : 1; + uint32_t olb : 1; + uint32_t stst : 1; + } DevStatusReg_t; + + typedef struct { + uint32_t reset : 1; + uint32_t drv_err : 1; // + uint32_t uv_cp : 1; + } GState_t; + + typedef struct { SPI_HandleTypeDef *spi; Pin_t csgpio = PinNull; // Pin_t resetPin = PinNull; // @@ -55,9 +77,6 @@ class TMC5130 : public IStepperMotor { } cfg_t; protected: - // TMC5130Port *m_port; - // TMC5130Config_t *m_config; - cfg_t m_cfg; ZGPIO *m_csnpin = NULL; @@ -81,69 +100,59 @@ class TMC5130 : public IStepperMotor { public: TMC5130(/* args */); - // static void createDeafultTMC5130Config(TMC5130Config_t *config, TMC5130Port *m_port); void initialize(cfg_t *cfg); - void enableIC(bool enable); - virtual void enable(bool enable) { enableIC(enable); } - - uint8_t reset(); + virtual void enable(bool enable); - /******************************************************************************* - * 常用寄存器读写方法 * - *******************************************************************************/ - - uint32_t readICVersion(); // 5130:0x11 5160:0x30 - virtual int32_t getXACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置 - virtual void setXACTUAL(int32_t value); // 设置电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置 + virtual void rotate(int32_t velocity); + virtual void moveTo(int32_t position, uint32_t velocityMax); + virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); + virtual void stop(); - virtual int32_t getVACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置 - uint32_t readXTARGET(); // 读取驱动器目标位置 + virtual void setXACTUAL(int32_t value); // 设置当前位置 + virtual int32_t getXACTUAL(); // 当前位置 + virtual int32_t getVACTUAL(); // 当前速度 + virtual void setAcceleration(float accelerationpps2); // 设置最大加速度 + virtual void setDeceleration(float accelerationpps2); // 设置最大减速度 + virtual void setMotorShaft(bool reverse); + virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay); + virtual void setGlobalScale(uint8_t globalscale); // ONLY for 5160 virtual void setScale(int32_t scale); virtual void setScaleDenominator(int32_t scale); - virtual void setAcceleration(float accelerationpps2); - virtual void setDeceleration(float accelerationpps2); - - void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay); - void setGlobalScale(uint8_t globalscale); - uint8_t getGlobalScale(); - // void setSubdivision(uint8_t subdivision); + virtual bool isReachTarget(); // 是否到达目标位置 + virtual bool isStoped() { return isReachTarget(); } + virtual void setNoAccLimit(bool enable) override; - void setMotorShaft(bool reverse); // 设置电机旋转方向 + public: + DevStatusReg_t getDevStatus() { + static_assert(sizeof(DevStatusReg_t) == 4); + uint32_t value = readInt(TMC5130_DRVSTATUS); + return *(DevStatusReg_t *)&value; + } + GState_t getGState() { + static_assert(sizeof(GState_t) == 4); + uint32_t value = readInt(TMC5130_GSTAT); + return *(GState_t *)&value; + } + public: + uint8_t reset(); + uint32_t readICVersion(); // 5130:0x11 5160:0x30 + uint32_t readXTARGET(); // 读取驱动器目标位置 + uint8_t getGlobalScale(); // uint32_t getTMC5130_RAMPSTAT(); // 这个寄存器读取的是TMC5130的状态寄存器 Tmc5130RampStat getTMC5130_RAMPSTAT2(); - virtual void rotate(int32_t velocity); - virtual void right(int32_t velocity); - virtual void left(int32_t velocity); - - virtual void moveTo(int32_t position, uint32_t velocityMax); - virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); - - virtual void stop(); - virtual bool isReachTarget(); // 是否到达目标位置 - virtual bool isStoped() { return isReachTarget(); } - /******************************************************************************* * 驱动器寄存器读写方法 * *******************************************************************************/ - // void writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value); void readWriteArray(uint8_t *data, size_t length); void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); void writeInt(uint8_t address, int32_t value); int32_t readInt(uint8_t address); - - void setNoAccLimit(bool enable) override; - - /** - * @brief 设置电机细分 - * - * @param value - */ - void setMRES(mres_type_t value); + void setMRES(mres_type_t value); private: uint32_t haspassedms(uint32_t now, uint32_t last); diff --git a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp index b2b6cf7..333dc85 100644 --- a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp +++ b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp @@ -1,7 +1,7 @@ #include "water_cooling_temperature_control_module.hpp" -#include "sdk\components\flash\znvs.hpp" #include "a8000_protocol\protocol.hpp" +#include "sdk\components\flash\znvs.hpp" using namespace iflytop; @@ -14,10 +14,10 @@ using namespace iflytop; #define ACTION_TEST_ENABLE_LOG 5 #define ACTION_TEST_DISABLE_LOG 6 -static int32_t prvabs(int32_t v){ - if(v<0){ +static int32_t prvabs(int32_t v) { + if (v < 0) { return -v; - }else{ + } else { return v; } } @@ -53,8 +53,7 @@ int32_t WaterCoolingTemperatureControlModule::getid(int32_t* id) { return 0; } int32_t WaterCoolingTemperatureControlModule::module_clear_error() { - creg.module_errorcode = 0; - creg.module_errorbitflag0 = 0; + creg.module_errorcode = 0; m_fan_ctrl->clearError(); m_pelter_ctrl->clearError(); m_pump_ctrl->clearError(); @@ -195,13 +194,13 @@ void WaterCoolingTemperatureControlModule::workloop() { #define BIT(x, n) ((x >> n) & 0x01) int32_t WaterCoolingTemperatureControlModule::checkdevice() { - creg.module_errorbitflag0 = geterrorbitflag0(); - if (creg.module_errorbitflag0 != 0) { + uint32_t module_errorbitflag0 = geterrorbitflag0(); + if (module_errorbitflag0 != 0) { ZLOGE(TAG, "checkdevice errorbitflag0:%d %d %d %d", // - BIT(creg.module_errorbitflag0, 0), // - BIT(creg.module_errorbitflag0, 1), // - BIT(creg.module_errorbitflag0, 2), // - BIT(creg.module_errorbitflag0, 3)); + BIT(module_errorbitflag0, 0), // + BIT(module_errorbitflag0, 1), // + BIT(module_errorbitflag0, 2), // + BIT(module_errorbitflag0, 3)); m_pelter_ctrl->dumpErrorInfo(); creg.module_errorcode = err::khwardware_error; return 1; diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index 7305d76..8f1ac89 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -33,6 +33,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_active_cfg); REGFN(step_motor_stop); REGFN(step_motor_read_io_state); + REGFN(step_motor_read_motoric_status); REGFN(mini_servo_enable); REGFN(mini_servo_read_pos); @@ -246,6 +247,13 @@ int32_t ZCanProtocolParser::step_motor_read_io_state(cmdcontxt_t* cxt) { return module->step_motor_read_io_state(cxt->params[0], ack); } +int32_t ZCanProtocolParser::step_motor_read_motoric_status(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->step_motor_read_motoric_status(&ack[0]); +} + #undef MODULE_CLASS #define MODULE_CLASS ZIMiniServo // virtual int32_t mini_servo_enable(int32_t enable) = 0; diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index 4c7a4a7..f26e7ae 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -68,6 +68,8 @@ class ZCanProtocolParser : public IZCanReceiverListener { static int32_t step_motor_easy_move_to_io(cmdcontxt_t* cxt); static int32_t step_motor_active_cfg(cmdcontxt_t* cxt); static int32_t step_motor_read_io_state(cmdcontxt_t* cxt); + static int32_t step_motor_read_motoric_status(cmdcontxt_t* cxt); + static int32_t mini_servo_enable(cmdcontxt_t* cxt); static int32_t mini_servo_read_pos(cmdcontxt_t* cxt);