From 1c4b548d5b62a2b0ad37b0a70411347955103712 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 8 Apr 2024 14:11:40 +0800 Subject: [PATCH] add tmc51x0_motor --- .../step_motor_ctrl_module.cpp | 2 +- .../step_motor_ctrl_module/tmc51x0_motor.cpp | 824 +++++++++++++++++++++ .../step_motor_ctrl_module/tmc51x0_motor.hpp | 213 ++++++ .../zprotocols/zcancmder_v2/api/zi_motor.hpp | 7 + components/zprotocols/zcancmder_v2/cmdid.hpp | 7 + 5 files changed, 1052 insertions(+), 1 deletion(-) create mode 100644 components/step_motor_ctrl_module/tmc51x0_motor.cpp create mode 100644 components/step_motor_ctrl_module/tmc51x0_motor.hpp 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 690ac89..8bea04a 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -3,8 +3,8 @@ #include #include -#include "sdk\components\zprotocols\zcancmder_v2\api\errorcode.hpp" #include "sdk\components\flash\znvs.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\api\errorcode.hpp" using namespace iflytop; #define TAG "SMCM" void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg) { diff --git a/components/step_motor_ctrl_module/tmc51x0_motor.cpp b/components/step_motor_ctrl_module/tmc51x0_motor.cpp new file mode 100644 index 0000000..16c5e9b --- /dev/null +++ b/components/step_motor_ctrl_module/tmc51x0_motor.cpp @@ -0,0 +1,824 @@ +#include "tmc51x0_motor.hpp" + +#include +#include + +#include "sdk\components\flash\znvs.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\api\errorcode.hpp" +using namespace iflytop; +#define TAG "SMCM" + +void TMC51X0Motor::initialize(int moduleid, TMC5130* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg) { + m_id = moduleid; + m_stepM1 = stepM; + m_iotable = iotable; + m_nio = nio; + if (m_nio >= 8) m_nio = 8; + if (m_nio >= 1) m_Xgpio = &m_iotable[0]; + if (m_nio >= 2) m_end_gpio = &m_iotable[1]; + + m_lock.init(); + m_statu_lock.init(); + m_thread.init(TAG, 1024, osPriorityNormal); + m_flashmark = flashmark; + m_factory_config = *defaultcfg; + + if (m_flashmark) { + ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg))); + if (!m_cfg.configInited) { + m_cfg.configInited = true; + m_cfg = m_factory_config; + flush(); + } + } else { + m_cfg = m_factory_config; + } + + active_cfg(); +} + +bool TMC51X0Motor::isbusy() { return m_thread.isworking(); } + +int32_t TMC51X0Motor::move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { + ZLOGI(TAG, "move_to_logic_point %d", logic_point_num); + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { + return err::kparam_out_of_range; + } + logic_point_t logic_point = m_cfg.logic_point[logic_point_num]; + return move_to(logic_point.x, logic_point.velocity, status_cb); +} +int32_t TMC51X0Motor::set_logic_point(int logic_point_num, int32_t x, int32_t vel, int32_t acc, int32_t dec) { + ZLOGI(TAG, "set_logic_point %d %d %d %d %d", logic_point_num, x, vel, acc, dec); + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) return err::kparam_out_of_range; + if (logic_point_num < 0) logic_point_num = 0; + // memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.logic_point[logic_point_num].x = x; + m_cfg.logic_point[logic_point_num].velocity = vel; + m_cfg.logic_point[logic_point_num].acc = acc; + m_cfg.logic_point[logic_point_num].dec = dec; + return 0; +} +int32_t TMC51X0Motor::get_logic_point(int logic_point_num, logic_point_t& logic_point) { + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { + return err::kparam_out_of_range; + } + logic_point = m_cfg.logic_point[logic_point_num]; + return 0; +} + +int32_t TMC51X0Motor::_move_to(int32_t tox, action_cb_status_t status_cb) { // + + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_to %d", m_id, tox); + m_thread.stop(); + + if (m_param.min_d != 0 && tox < m_param.min_d) { + tox = m_param.min_d; + } + if (m_param.max_d != 0 && tox > m_param.max_d) { + tox = m_param.max_d; + } + + m_thread.start( + [this, tox, status_cb]() { + _move_to_nolimit(tox, m_default_speed, m_param.motor_default_acc, m_param.motor_default_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + call_exec_status_cb(0, status_cb); + }, + [this, tox, status_cb]() { _motor_stop(); }); + return 0; +} +int32_t TMC51X0Motor::_move_by(int32_t dx, action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_by %d", m_id, dx); + m_thread.stop(); + + int32_t xnow = 0; + getnowpos(xnow); + + int32_t toxnow = xnow + dx; + // ZLOGI(TAG, "m%d move_by %d xnow:%d toxnow:%d", m_id, dx, xnow, toxnow); + return move_to(toxnow, [this, status_cb, xnow](int32_t status) { + if (status_cb) status_cb(status); + }); +} + +int32_t TMC51X0Motor::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { + if (m_default_speed > m_param.motor_default_velocity) { + ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); + m_default_speed = m_param.motor_default_velocity; + } + if (velocity != 0) m_default_speed = velocity; + return move_to(x, status_cb); +} +int32_t TMC51X0Motor::move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { + if (m_default_speed > m_param.motor_default_velocity) { + ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); + m_default_speed = m_param.motor_default_velocity; + } + if (velocity != 0) m_default_speed = velocity; + return move_by(dx, status_cb); +} + +int32_t TMC51X0Motor::move_to(int32_t tox, action_cb_status_t status_cb) { + m_default_speed = m_param.motor_default_velocity; + return _move_to(tox, status_cb); +} +int32_t TMC51X0Motor::move_by(int32_t dx, action_cb_status_t status_cb) { + m_default_speed = m_param.motor_default_velocity; + return _move_by(dx, status_cb); +} + +int32_t TMC51X0Motor::calculate_curpos(action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "read_curpos_by_move2zero"); + m_thread.stop(); + m_thread.start( + [this, status_cb]() { + // + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + int xstart = dx + m_param.motor_shift; + m_calculate_curpos_result = xstart; + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); + return 0; +} +int32_t TMC51X0Motor::read_calculate_curpos_action_result(int32_t& pos) { + pos = m_calculate_curpos_result; + return 0; +} + +int32_t TMC51X0Motor::move_to_zero(action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_to_zero"); + m_thread.stop(); + + m_thread.start( + [this, status_cb]() { + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + if (erro == 0) { + m_stepM1->setXACTUAL(0); + } + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); + + return 0; +} +int32_t TMC51X0Motor::move_to_zero_with_calibrate(int32_t nowx, action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx); + m_thread.stop(); + + m_thread.start([this, nowx, status_cb]() { + int32_t dx; + + int32_t erro = _exec_move_to_zero_task(dx); + if (erro != 0) { + ZLOGI(TAG, "_exec_move_to_zero_task failed:%d", erro); + _motor_stop(); + call_exec_status_cb(erro, status_cb); + + return; + } + + int32_t calibrate_x; + calibrate_x = dx + nowx; + m_param.motor_shift = calibrate_x; + + m_stepM1->setXACTUAL(0); + call_exec_status_cb(erro, status_cb); + return; + }); + + return 0; +} +int32_t TMC51X0Motor::enable(bool venable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d enable %d", m_id, venable); + m_stepM1->enable(venable); + // m_enable = venable; + m_com_reg.module_enable = venable; + return 0; +} +int32_t TMC51X0Motor::stop(uint8_t stopType) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d stop", m_id); + m_thread.stop(); + m_stepM1->stop(); + return 0; +} +int32_t TMC51X0Motor::force_change_current_pos(int32_t xpos) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d force_change_current_pos %d", m_id, xpos); + int32_t motor_pos = 0; + forward_kinematics(xpos, motor_pos); + m_stepM1->setXACTUAL(motor_pos); + return 0; +} +int32_t TMC51X0Motor::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // + zlock_guard l(m_lock); + ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.motor_default_acc, m_param.motor_default_dec); + + if (lastforms < 0) { + ZLOGW(TAG, "lastforms:%d < 0", lastforms); + return err::kparam_out_of_range; + } + + if (m_param.motor_default_velocity != 0 && abs(speed) > m_param.motor_default_velocity) { + ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.motor_default_velocity); + speed = m_param.motor_default_velocity; + } + m_thread.stop(); + m_thread.start([this, lastforms, speed, status_cb]() { + ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.motor_default_acc, m_param.motor_default_dec); + _rotate(speed, m_param.motor_default_acc, m_param.motor_default_dec); + + int32_t startticket = zos_get_tick(); + // bool reachtime = false; + + while (!m_thread.getExitFlag()) { + if ((int32_t)zos_haspassedms(startticket) > lastforms && lastforms != 0) { + // reachtime = true; + m_stepM1->stop(); + break; + } + // ZLOGI(TAG,"..... state %d",m_thread.getExitFlag()); + osDelay(100); + } + call_exec_status_cb(0, status_cb); + m_stepM1->stop(); + return; + }); + return 0; +} + +int32_t TMC51X0Motor::read_pos(int32_t& pos) { + getnowpos(pos); + return 0; +} +int32_t TMC51X0Motor::read_pos() { + int32_t pos; + getnowpos(pos); + return pos; +} + +bool TMC51X0Motor::read_zero_io_state() { + if (m_Xgpio) return m_Xgpio->getState(); + return false; +} + +void TMC51X0Motor::_rotate(int32_t vel, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + m_stepM1->rotate(vel); +} + +void TMC51X0Motor::getnowpos(int32_t& pos) { + int32_t motor_pos = m_stepM1->getXACTUAL(); + inverse_kinematics(motor_pos, pos); +} +void TMC51X0Motor::_move_to_nolimit(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _move_to_nolimit %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + int32_t motor_pos = 0; + forward_kinematics(pos, motor_pos); + m_stepM1->moveTo(motor_pos, maxv); +} +void TMC51X0Motor::_move_by_nolimit(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _motor_move_by %d maxv:%d acc:%d dec:%d", m_id, dpos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + + int32_t nowpos = 0; + getnowpos(nowpos); + + int32_t topos = nowpos + dpos; + int32_t motorpos = 0; + forward_kinematics(topos, motorpos); + ZLOGI(TAG, "motor moveTo %d %d", motorpos, maxv); + m_stepM1->moveTo(motorpos, maxv); +} +void TMC51X0Motor::_motor_stop(int32_t dec) { + ZLOGI(TAG, "m%d _motor_stop %d", m_id, dec); + if (dec > 0) { + m_stepM1->setDeceleration(dec); + } + m_stepM1->stop(); +} +bool TMC51X0Motor::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } +void TMC51X0Motor::inverse_kinematics(int32_t motor_pos, int32_t& x) { + // m_zero_motor_shift + x = motor_pos; + x += m_param.motor_shift; +} +void TMC51X0Motor::forward_kinematics(int32_t x, int32_t& motor_pos) { + x -= m_param.motor_shift; + motor_pos = x; +} +#if 0 +void TMC51X0Motor::updateStatus(uint8_t status) { + zlock_guard lock(m_statu_lock); + m_status = status; +} +#endif + +int32_t TMC51X0Motor::do_motor_action_block_2(function action, function break_condition) { + if (action == nullptr) return -1; + int32_t ret = action(); + if (ret != 0) { + stop(0); + return ret; + } + + ThisThread thisThread; + int count = 0; + while (!thisThread.getExitFlag()) { + if (!isbusy()) break; + thisThread.sleep(1); + count += 1; + + if (break_condition) { + bool break_flag = false; + int32_t err = break_condition(break_flag, count); + if (err != 0) { + stop(0); + return err; + } + + if (break_flag) { + stop(0); + return 0; + } + } + } + + if (isbusy()) { + stop(0); + return err::kmodule_opeation_break_by_user; + } + + return get_last_exec_status(); +} + +int32_t TMC51X0Motor::do_motor_action_block(function action) { return do_motor_action_block_2(action, nullptr); } +int32_t TMC51X0Motor::move_to_block(int32_t tox, int overtime) { + return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); +} +int32_t TMC51X0Motor::move_by_block(int32_t dx, int overtime) { + return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); +} +int32_t TMC51X0Motor::move_to_zero_block(int overtime) { + return do_motor_action_block([this]() { return move_to_zero(nullptr); }); +} +int32_t TMC51X0Motor::move_to_zero_with_calibrate_block(int32_t x, int overtime) { + return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); }); +} +int32_t TMC51X0Motor::move_to_block(int32_t tox, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, tox]() { return move_to(tox, velocity, nullptr); }); +} +int32_t TMC51X0Motor::move_by_block(int32_t dx, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, dx]() { return move_by(dx, velocity, nullptr); }); +} + +void TMC51X0Motor::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) { m_com_reg.module_last_cmd_exec_status = status; } +void TMC51X0Motor::create_default_cfg(flash_config_t& cfg) { + memset(&cfg, 0, sizeof(cfg)); + cfg.base_param.motor_one_circle_pulse = 10000; + cfg.base_param.motor_one_circle_pulse_denominator = 1; + cfg.base_param.stepmotor_ihold = 1; + cfg.base_param.stepmotor_irun = 31; + cfg.base_param.stepmotor_iholddelay = 100; + cfg.base_param.stepmotor_iglobalscaler = 1; + cfg.base_param.motor_default_acc = 300; + cfg.base_param.motor_default_dec = 300; + cfg.base_param.motor_default_velocity = 500; + + cfg.base_param.motor_run_to_zero_max_d = 10000 * 100; + cfg.base_param.motor_run_to_zero_speed = 100; + cfg.base_param.motor_run_to_zero_dec = 600; + + cfg.base_param.motor_look_zero_edge_max_d = 10000 * 5; + cfg.base_param.motor_look_zero_edge_speed = 100; + cfg.base_param.motor_look_zero_edge_dec = 600; +} + +int32_t TMC51X0Motor::flush() { + ZLOGI(TAG, "flush"); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + return 0; +} +int32_t TMC51X0Motor::factory_reset() { + // + ZLOGI(TAG, "factory_reset"); + m_cfg = m_factory_config; + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + active_cfg(); + return 0; +} +void TMC51X0Motor::active_cfg() { + m_stepM1->setIHOLD_IRUN(m_param.stepmotor_ihold, m_param.stepmotor_irun, m_param.stepmotor_iholddelay); + // stepmotor_iglobalscaler + m_stepM1->setScale(m_param.motor_one_circle_pulse); + m_stepM1->setScaleDenominator(m_param.motor_one_circle_pulse_denominator); + m_stepM1->setMotorShaft(m_param.motor_shaft); +} + +int32_t TMC51X0Motor::getid(int32_t* id) { + *id = m_id; + return 0; +} +int32_t TMC51X0Motor::module_stop() { return stop(0); } +int32_t TMC51X0Motor::module_break() { return stop(0); } +int32_t TMC51X0Motor::module_get_last_exec_status(int32_t* status) { + *status = m_com_reg.module_last_cmd_exec_status; + return 0; +} +int32_t TMC51X0Motor::module_get_status(int32_t* status) { + // ZLOGI(TAG, "[%d]-module_get_status %d", m_id, m_thread.isworking()); + *status = m_thread.isworking() ? 1 : 0; + return 0; +} +int32_t TMC51X0Motor::module_get_error(int32_t* iserror) { + *iserror = 0; + return 0; +} + +int32_t TMC51X0Motor::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { + switch (param_id) { + MODULE_COMMON_PROCESS_REG_CB(); + PROCESS_REG(kreg_robot_pos, read_pos(val), ACTION_NONE); + PROCESS_REG(kreg_motor_shift, REG_GET(m_param.motor_shift), REG_SET(m_param.motor_shift)); + PROCESS_REG(kreg_motor_shaft, REG_GET(m_param.motor_shaft), REG_SET(m_param.motor_shaft)); + PROCESS_REG(kreg_motor_one_circle_pulse, REG_GET(m_param.motor_one_circle_pulse), REG_SET(m_param.motor_one_circle_pulse)); + PROCESS_REG(kreg_motor_one_circle_pulse_denominator, REG_GET(m_param.motor_one_circle_pulse_denominator), REG_SET(m_param.motor_one_circle_pulse_denominator)); + PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_param.motor_default_velocity), REG_SET(m_param.motor_default_velocity)); + PROCESS_REG(kreg_motor_default_acc, REG_GET(m_param.motor_default_acc), REG_SET(m_param.motor_default_acc)); + PROCESS_REG(kreg_motor_default_dec, REG_GET(m_param.motor_default_dec), REG_SET(m_param.motor_default_dec)); + PROCESS_REG(kreg_motor_run_to_zero_max_d, REG_GET(m_param.motor_run_to_zero_max_d), REG_SET(m_param.motor_run_to_zero_max_d)); + PROCESS_REG(kreg_motor_look_zero_edge_max_d, REG_GET(m_param.motor_look_zero_edge_max_d), REG_SET(m_param.motor_look_zero_edge_max_d)); + PROCESS_REG(kreg_motor_run_to_zero_speed, REG_GET(m_param.motor_run_to_zero_speed), REG_SET(m_param.motor_run_to_zero_speed)); + PROCESS_REG(kreg_motor_run_to_zero_dec, REG_GET(m_param.motor_run_to_zero_dec), REG_SET(m_param.motor_run_to_zero_dec)); + PROCESS_REG(kreg_motor_look_zero_edge_speed, REG_GET(m_param.motor_look_zero_edge_speed), REG_SET(m_param.motor_look_zero_edge_speed)); + PROCESS_REG(kreg_motor_look_zero_edge_dec, REG_GET(m_param.motor_look_zero_edge_dec), REG_SET(m_param.motor_look_zero_edge_dec)); + PROCESS_REG(kreg_stepmotor_ihold, REG_GET(m_param.stepmotor_ihold), REG_SET(m_param.stepmotor_ihold)); + PROCESS_REG(kreg_stepmotor_irun, REG_GET(m_param.stepmotor_irun), REG_SET(m_param.stepmotor_irun)); + PROCESS_REG(kreg_stepmotor_iholddelay, REG_GET(m_param.stepmotor_iholddelay), REG_SET(m_param.stepmotor_iholddelay)); + PROCESS_REG(kreg_stepmotor_iglobalscaler, REG_GET(m_param.stepmotor_iglobalscaler), REG_SET(m_param.stepmotor_iglobalscaler)); + PROCESS_REG(kreg_motor_max_d, REG_GET(m_param.max_d), REG_SET(m_param.max_d)); + PROCESS_REG(kreg_motor_min_d, REG_GET(m_param.min_d), REG_SET(m_param.min_d)); + PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE); + + default: + return err::kmodule_not_find_config_index; + break; + } + return 0; +} +int32_t TMC51X0Motor::do_action(int32_t actioncode) { return 0; } +int32_t TMC51X0Motor::module_clear_error() { return 0; } +int32_t TMC51X0Motor::module_readio(int32_t* io) { + *io = 0; + for (int32_t i = 0; i < m_nio; i++) { + if (m_iotable[i].getState()) { + *io |= (0x01 << i); + } + } + return 0; +} +int32_t TMC51X0Motor::_read_io() { + int32_t iostate = 0; + module_readio(&iostate); + return iostate; +} + +int32_t TMC51X0Motor::module_read_adc(int32_t adcindex, int32_t* adc) { + *adc = 0; + return 0; +} +int32_t TMC51X0Motor::module_factory_reset() { return factory_reset(); } +int32_t TMC51X0Motor::module_flush_cfg() { return flush(); } +int32_t TMC51X0Motor::module_active_cfg() { + active_cfg(); + return 0; +} +int32_t TMC51X0Motor::motor_enable(int32_t varenable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d motor_enable %ld", m_id, varenable); + enable(varenable); + return 0; +} +int32_t TMC51X0Motor::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { + ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); + m_thread.stop(); + if (acc == 0) { + acc = m_param.motor_default_acc; + } + _rotate(direction * motor_velocity, acc, acc); + return 0; +} +int32_t TMC51X0Motor::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) { + zlock_guard lock(m_lock); + int32_t xnow = 0; + getnowpos(xnow); + int32_t toxnow = xnow + dx; + return motor_move_to(toxnow, motor_velocity, acc); +} +int32_t TMC51X0Motor::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) { + zlock_guard lock(m_lock); + m_thread.stop(); + + if (m_param.min_d != 0 && tox < m_param.min_d) { + tox = m_param.min_d; + } + if (m_param.max_d != 0 && tox > m_param.max_d) { + tox = m_param.max_d; + } + + if (acc == 0) { + acc = m_param.motor_default_acc; + } + if (motor_velocity == 0) { + motor_velocity = m_param.motor_default_velocity; + } + + m_thread.start( + [this, tox, motor_velocity, acc]() { + _move_to_nolimit(tox, motor_velocity, acc, acc); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + call_exec_status_cb(0, nullptr); + }, + [this, tox]() { _motor_stop(); }); + // ZLOGI(TAG, "m%d motor_move_to %d end", m_id, tox); + return 0; +} +int32_t TMC51X0Motor::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { + ZLOGI(TAG, "motor_move_to_zero_backward"); + if (findzerospeed != 0) { + m_param.motor_run_to_zero_speed = findzerospeed; + } + if (findzeroedge_speed != 0) { + m_param.motor_look_zero_edge_speed = findzeroedge_speed; + } + if (acc != 0) { + m_param.motor_look_zero_edge_dec = acc; + m_param.motor_run_to_zero_dec = acc; + } + return move_to_zero(nullptr); +} + +int32_t TMC51X0Motor::motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_with_calibrate(0, nullptr); } +int32_t TMC51X0Motor::motor_read_pos(int32_t* pos) { + int32_t xnow = 0; + getnowpos(xnow); + *pos = xnow; + return 0; +} +void TMC51X0Motor::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4) { + m_com_reg.module_last_cmd_exec_status = ecode; + m_com_reg.module_last_cmd_exec_val0 = val0; + m_com_reg.module_last_cmd_exec_val1 = val1; + m_com_reg.module_last_cmd_exec_val2 = val2; + m_com_reg.module_last_cmd_exec_val3 = val3; + m_com_reg.module_last_cmd_exec_val4 = val4; +} + +int32_t TMC51X0Motor::motor_calculated_pos_by_move_to_zero() { + zlock_guard lock(m_lock); + ZLOGI(TAG, "motor_calculated_pos_by_move_to_zero"); + m_thread.stop(); + m_thread.start( + [this]() { + // + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + if (erro != 0) { + set_last_exec_status(erro); + } else { + m_com_reg.module_last_cmd_exec_status = erro; + set_last_exec_status(erro, -dx + m_param.motor_shift); + m_stepM1->setXACTUAL(0); + } + }, + [this]() { _motor_stop(); }); + return 0; +} + +int32_t TMC51X0Motor::motor_easy_rotate(int32_t direction) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_rotate %d", m_id, direction); + m_thread.stop(); + _rotate(direction * m_param.motor_default_velocity, m_param.motor_default_acc, m_param.motor_default_dec); + return 0; +} +int32_t TMC51X0Motor::motor_easy_move_by(int32_t distance) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance); + m_thread.stop(); + return motor_move_by(distance, m_param.motor_default_velocity, m_param.motor_default_acc); +} +int32_t TMC51X0Motor::motor_easy_move_to(int32_t position) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_move_to %d", m_id, position); + m_thread.stop(); + return motor_move_to(position, m_param.motor_default_velocity, m_param.motor_default_acc); +} +int32_t TMC51X0Motor::motor_easy_move_to_zero(int32_t direction) { + if (direction <= 0) { + return move_to_zero(nullptr); + } else { + return err::koperation_not_support; + } +} +int32_t TMC51X0Motor::motor_read_enc_val(int32_t* enc_val) {} +int32_t TMC51X0Motor::motor_set_enc_resolution(int32_t enc_resolution) {} +int32_t TMC51X0Motor::motor_get_enc_resolution(int32_t* enc_resolution) {} + +int32_t TMC51X0Motor::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return _exec_move_to_io_task(ioindex, direction); } + +/******************************************************************************* + * EXEC_TASK * + *******************************************************************************/ + +int32_t TMC51X0Motor::_exec_move_to_io_task(int32_t ioindex, int32_t direction) { + m_thread.stop(); + m_thread.start([this, ioindex, direction]() { m_com_reg.module_last_cmd_exec_status = _exec_move_to_io_task_fn(ioindex, direction); }); + return 0; +} +int32_t TMC51X0Motor::_exec_move_to_io_task_fn(int32_t ioindex, int32_t direction) { + int32_t runToPointSpeed = m_param.motor_run_to_zero_speed; + int32_t runToPointDec = m_param.motor_run_to_zero_dec; + // int32_t runToPointMaxD = m_param.motor_run_to_zero_max_d; + + int32_t lookPointVelocity = m_param.motor_look_zero_edge_speed; + int32_t lookPointDec = m_param.motor_look_zero_edge_dec; + // int32_t lookPointMaxD = m_param.motor_look_zero_edge_max_d; + + direction = direction >= 0 ? 1 : -1; + + ZGPIO* gpio = &m_iotable[ioindex]; + + if (!gpio->getState()) { + ZLOGI(TAG, "---------STEP1-------- move to point"); + _rotate(runToPointSpeed * direction, runToPointDec, runToPointDec); + bool xreach_io = false; + + while (!m_thread.getExitFlag()) { + if (gpio->getState()) { + xreach_io = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + // 等待电机停止 + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { + _motor_stop(-1); + vTaskDelay(100); + } + + // 失败返回 + if (!xreach_io) { + ZLOGE(TAG, "x reach io failed"); + return err::kxymotor_not_found_x_zero_point; + } + } + + // 如果设备已经在零点，则反向移动一定距离远离零点 + if (gpio->getState()) { + ZLOGI(TAG, "---------STEP2-------- find edge"); + _rotate(-direction * lookPointVelocity, lookPointDec, lookPointDec); + bool reach_edge = false; + + while (!m_thread.getExitFlag()) { + if (!gpio->getState()) { + reach_edge = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + _motor_stop(-1); + vTaskDelay(100); + } + + if (!reach_edge) { + ZLOGE(TAG, "leave away zero failed"); + return err::kxymotor_x_find_zero_edge_fail; + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return err::kmodule_opeation_break_by_user; + } + + ZLOGI(TAG, "_exec_move_to_io_task_fn success"); + return 0; +} + +int32_t TMC51X0Motor::_exec_move_to_zero_task(int32_t& dx) { + int32_t pos1 = 0; + int32_t pos2 = 0; + int32_t ret = 0; + getnowpos(pos1); + ret = _exec_move_to_zero_task(); + getnowpos(pos2); + dx = pos2 - pos1; + return ret; +} +int32_t TMC51X0Motor::_exec_move_to_zero_task() { + /******************************************************************************* + * 第一次移动到零点 * + *******************************************************************************/ + if (!m_Xgpio->getState()) { + { + ZLOGI(TAG, "---------STEP1-------- move to zero first"); + _rotate(-m_param.motor_run_to_zero_speed, m_param.motor_default_acc, m_param.motor_run_to_zero_dec); + bool xreach_zero = false; + while (!m_thread.getExitFlag()) { + // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); + if (m_Xgpio->getState()) { + xreach_zero = true; + _motor_stop(-1); + + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + _motor_stop(-1); + vTaskDelay(10); + } + + break; + } + vTaskDelay(1); + } + if (!xreach_zero) { + // 触发异常回调 + ZLOGE(TAG, "x reach zero failed"); + return err::kxymotor_not_found_x_zero_point; + } + } + } + + /******************************************************************************* + * 远离X零点 * + *******************************************************************************/ + { + ZLOGI(TAG, "---------STEP2-------- leave away zero"); + /** + * @brief 如果设备已经在零点，则反向移动一定距离远离零点 + */ + if (m_Xgpio->getState()) { + _rotate(m_param.motor_look_zero_edge_speed, m_param.motor_default_acc, m_param.motor_look_zero_edge_dec); + while (true) { + if (m_thread.getExitFlag()) break; + if (!m_Xgpio->getState()) { + _motor_stop(-1); + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + vTaskDelay(1); + } + break; + } + vTaskDelay(1); + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return err::kmodule_opeation_break_by_user; + } + if (m_Xgpio->getState()) { + ZLOGE(TAG, "leave away zero failed"); + return err::kxymotor_x_find_zero_edge_fail; + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + + ZLOGI(TAG, "x reach zero"); + ZLOGI(TAG, "move_to_zero success"); + + return 0; +} \ No newline at end of file diff --git a/components/step_motor_ctrl_module/tmc51x0_motor.hpp b/components/step_motor_ctrl_module/tmc51x0_motor.hpp new file mode 100644 index 0000000..5887bd2 --- /dev/null +++ b/components/step_motor_ctrl_module/tmc51x0_motor.hpp @@ -0,0 +1,213 @@ +#pragma once +// +#include "sdk/os/zos.hpp" +#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" + +namespace iflytop { +class TMC51X0Motor : public ZIModule, public ZIMotor { + ENABLE_MODULE(TMC51X0Motor, kmotor_module, 0x0001); + + public: + typedef struct { + int32_t motor_shaft; + int32_t motor_one_circle_pulse; // + int32_t motor_one_circle_pulse_denominator; // + int32_t stepmotor_ihold; + int32_t stepmotor_irun; + int32_t stepmotor_iholddelay; + int32_t stepmotor_iglobalscaler; + int32_t motor_default_acc; + int32_t motor_default_dec; + int32_t motor_default_velocity; + int32_t min_d; + int32_t max_d; + // + int32_t motor_shift; + + int32_t motor_run_to_zero_max_d; + int32_t motor_run_to_zero_speed; + int32_t motor_run_to_zero_dec; + + int32_t motor_look_zero_edge_max_d; + int32_t motor_look_zero_edge_speed; + int32_t motor_look_zero_edge_dec; + } base_param_t; + + typedef struct { + uint8_t index; + int32_t acc; + int32_t dec; + int32_t velocity; + int32_t x; + } logic_point_t; + + typedef struct { + bool configInited; + base_param_t base_param; + logic_point_t logic_point[5]; + } flash_config_t; + typedef std::function action_cb_status_t; + + private: + TMC5130* m_stepM1; + int m_id = 0; + + ZGPIO* m_Xgpio; + ZGPIO* m_end_gpio; + + ZThread m_thread; + + zmutex m_lock; + zmutex m_statu_lock; + + flash_config_t m_cfg; + flash_config_t m_factory_config; + base_param_t& m_param = m_cfg.base_param; + int32_t m_default_speed = 0; + + ZGPIO* m_iotable; + int m_nio; + + int32_t m_calculate_curpos_result = 0; + const char* m_flashmark = nullptr; + // action_cb_status_t m_status_cb; + + // int32_t m_enable = 0; + + public: + void initialize(int moduleid, TMC5130* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg); + static void create_default_cfg(flash_config_t& cfg); + static uint32_t get_flash_cfg_size() { return sizeof(flash_config_t); } + + virtual bool isbusy(); + virtual int32_t get_last_exec_status() { return m_com_reg.module_last_cmd_exec_status; }; + + virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb); + virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, int32_t acc, int32_t dec); + virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point); + + virtual int32_t move_to_block(int32_t tox, int overtime = 0); + virtual int32_t move_by_block(int32_t dx, int overtime = 0); + + virtual int32_t move_to_block(int32_t tox, int32_t velocity, int overtime = 0); + virtual int32_t move_by_block(int32_t dx, int32_t velocity, int overtime = 0); + + virtual int32_t move_to_zero_block(int overtime = 0); + virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime = 0); + + virtual int32_t enable(bool venable); + virtual int32_t stop(uint8_t stopType); + virtual int32_t force_change_current_pos(int32_t x); + + virtual int32_t read_pos(int32_t& pos); + virtual int32_t read_pos(); + virtual bool read_zero_io_state(); + + int32_t do_motor_action_block_2(function action, function break_condition); + + /******************************************************************************* + * OVERRIDE MODULE * + *******************************************************************************/ + virtual int32_t module_ping() { return 0; }; + + virtual int32_t getid(int32_t* id) override; + virtual int32_t module_stop() override; + virtual int32_t module_break() override; + virtual int32_t module_get_last_exec_status(int32_t* status) override; + virtual int32_t module_get_status(int32_t* status) override; + virtual int32_t module_get_error(int32_t* iserror) override; + virtual int32_t module_clear_error() override; + + virtual int32_t module_readio(int32_t* io) override; + + virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc) override; + + virtual int32_t module_factory_reset() override; + virtual int32_t module_flush_cfg() override; + virtual int32_t module_active_cfg() override; + + virtual int32_t module_enable(int32_t enable) override { return motor_enable(enable); } + + /******************************************************************************* + * Motor * + *******************************************************************************/ + virtual int32_t motor_enable(int32_t enable) override; + virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; + + virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_read_pos(int32_t* pos) override; + + virtual int32_t motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_calculated_pos_by_move_to_zero() override; + + virtual int32_t motor_easy_rotate(int32_t direction) override; + virtual int32_t motor_easy_move_by(int32_t distance) override; + virtual int32_t motor_easy_move_to(int32_t position) override; + virtual int32_t motor_easy_move_to_zero(int32_t direction) override; + virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; + + IStepperMotor* _getStepMotor() { return m_stepM1; } + base_param_t& _get_base_param() { return m_param; } + + private: + void active_cfg(); + + int32_t do_motor_action_block(function action); + // void updateStatus(uint8_t status); + + void getnowpos(int32_t& pos); + + void _move_to_nolimit(int32_t pos, int32_t maxv, int32_t acc, int32_t dec); + void _move_by_nolimit(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec); + void _motor_stop(int32_t dec = -1); + bool _motor_is_reach_target(); + void _rotate(int32_t vel, int32_t acc, int32_t dec); + + void inverse_kinematics(int32_t motor_pos, int32_t& x); + void forward_kinematics(int32_t x, int32_t& motor_pos); + + int32_t _move_to(int32_t tox, action_cb_status_t status_cb); + int32_t _move_by(int32_t dx, action_cb_status_t status_cb); + + void call_exec_status_cb(int32_t status, action_cb_status_t status_cb); + void set_last_exec_status(int32_t ecode, int32_t val0 = 0, int32_t val1 = 0, int32_t val2 = 0, int32_t val3 = 0, int32_t val4 = 0); + + virtual int32_t motor_read_enc_val(int32_t* enc_val); + virtual int32_t motor_set_enc_resolution(int32_t enc_resolution); + virtual int32_t motor_get_enc_resolution(int32_t* enc_resolution); + + private: + int32_t _read_io(); + +#if 1 + int32_t move_to(int32_t tox, action_cb_status_t status_cb); + int32_t move_by(int32_t dx, action_cb_status_t status_cb); + int32_t move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb); + int32_t move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb); + int32_t calculate_curpos(action_cb_status_t status_cb); + int32_t move_to_zero(action_cb_status_t status_cb); + int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb); + int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb); + int32_t read_calculate_curpos_action_result(int32_t& pos); + int32_t flush(); + int32_t factory_reset(); +#endif + + private: + int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val); + int32_t do_action(int32_t actioncode); + + private: + /******************************************************************************* + * EXEC_TASK * + *******************************************************************************/ + int32_t _exec_move_to_zero_task(int32_t& dx); + int32_t _exec_move_to_zero_task(); + int32_t _exec_move_to_io_task(int32_t ioindex, int32_t direction); + int32_t _exec_move_to_io_task_fn(int32_t ioindex, int32_t direction); +}; +} // namespace iflytop \ No newline at end of file diff --git a/components/zprotocols/zcancmder_v2/api/zi_motor.hpp b/components/zprotocols/zcancmder_v2/api/zi_motor.hpp index d5fe9f4..b3ec038 100644 --- a/components/zprotocols/zcancmder_v2/api/zi_motor.hpp +++ b/components/zprotocols/zcancmder_v2/api/zi_motor.hpp @@ -41,6 +41,13 @@ class ZIMotor { virtual int32_t motor_easy_set_current_pos(int32_t pos) { return err::koperation_not_support; }; virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return err::koperation_not_support; }; + virtual int32_t motor_read_enc_val(int32_t* enc_val) { return err::koperation_not_support; } + virtual int32_t motor_set_enc_resolution(int32_t enc_resolution) { return err::koperation_not_support; } + virtual int32_t motor_get_enc_resolution(int32_t* enc_resolution) { return err::koperation_not_support; } + + virtual int32_t motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) { return err::koperation_not_support; } + virtual int32_t motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) { return err::koperation_not_support; } + // virtual int32_t motor_set_shaft(); // s32 pos, s32 speed, s32 torque, diff --git a/components/zprotocols/zcancmder_v2/cmdid.hpp b/components/zprotocols/zcancmder_v2/cmdid.hpp index d58a4d5..59101cf 100644 --- a/components/zprotocols/zcancmder_v2/cmdid.hpp +++ b/components/zprotocols/zcancmder_v2/cmdid.hpp @@ -93,6 +93,13 @@ typedef enum { kmotor_easy_set_current_pos = CMDID(2, 21), // para:{4}, ack:{} kmotor_easy_move_to_io = CMDID(2, 22), // para:{4,4}, ack:{} + kmotor_set_subdevice_reg = CMDID(2, 23), // para:{4,4}, ack:{} + kmotor_get_subdevice_reg = CMDID(2, 24), // para:{4,4}, ack:{} + + kmotor_read_enc_val = CMDID(2, 25), // para:{}, ack:{4} + kmotor_set_enc_resolution = CMDID(2, 26), // para:{4}, ack:{} + kmotor_get_enc_resolution = CMDID(2, 27), // para:{}, ack:{4} + #if 0 virtual ~ZIXYMotor() {} virtual int32_t xymotor_enable(int32_t enable) { return err::koperation_not_support; }