4 changed files with 335 additions and 1043 deletions
-
320components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
-
42components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
-
206components/step_motor_ctrl_module/step_motor_ctrl_module.hpp.bak
-
810components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak
@ -1,206 +0,0 @@ |
|||
#pragma once |
|||
// |
|||
#include "sdk/os/zos.hpp" |
|||
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" |
|||
#include "sdk\components\zcancmder\zcanreceiver.hpp" |
|||
namespace iflytop { |
|||
class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { |
|||
ENABLE_MODULE(StepMotorCtrlModule, 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<void(int32_t status)> action_cb_status_t; |
|||
|
|||
private: |
|||
IStepperMotor* 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, IStepperMotor* 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 0; }; |
|||
|
|||
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<int32_t()> action, function<int32_t(bool&, int periodcount)> 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<int32_t()> 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); |
|||
|
|||
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); |
|||
|
|||
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 |
|||
@ -1,810 +0,0 @@ |
|||
#include "step_motor_ctrl_module.hpp" |
|||
|
|||
#include <stdlib.h> |
|||
#include <string.h> |
|||
|
|||
#include "a8000_protocol\protocol.hpp" |
|||
#include "sdk\components\flash\znvs.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) { |
|||
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 StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } |
|||
|
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::_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 StepMotorCtrlModule::_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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::read_calculate_curpos_action_result(int32_t& pos) { |
|||
pos = m_calculate_curpos_result; |
|||
return 0; |
|||
} |
|||
|
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::enable(bool venable) { |
|||
zlock_guard l(m_lock); |
|||
ZLOGI(TAG, "m%d enable %d", m_id, venable); |
|||
m_stepM1->enable(venable); |
|||
// m_enable = venable; |
|||
creg.module_enable = venable; |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::read_pos(int32_t& pos) { |
|||
getnowpos(pos); |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::read_pos() { |
|||
int32_t pos; |
|||
getnowpos(pos); |
|||
return pos; |
|||
} |
|||
|
|||
bool StepMotorCtrlModule::read_zero_io_state() { |
|||
if (m_Xgpio) return m_Xgpio->getState(); |
|||
return false; |
|||
} |
|||
|
|||
void StepMotorCtrlModule::_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 StepMotorCtrlModule::getnowpos(int32_t& pos) { |
|||
int32_t motor_pos = m_stepM1->getXACTUAL(); |
|||
inverse_kinematics(motor_pos, pos); |
|||
} |
|||
void StepMotorCtrlModule::_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 StepMotorCtrlModule::_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 StepMotorCtrlModule::_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 StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } |
|||
void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { |
|||
// m_zero_motor_shift |
|||
x = motor_pos; |
|||
x += m_param.motor_shift; |
|||
} |
|||
void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { |
|||
x -= m_param.motor_shift; |
|||
motor_pos = x; |
|||
} |
|||
#if 0 |
|||
void StepMotorCtrlModule::updateStatus(uint8_t status) { |
|||
zlock_guard lock(m_statu_lock); |
|||
m_status = status; |
|||
} |
|||
#endif |
|||
|
|||
int32_t StepMotorCtrlModule::do_motor_action_block_2(function<int32_t()> action, function<int32_t(bool&, int periodcount)> 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 StepMotorCtrlModule::do_motor_action_block(function<int32_t()> action) { return do_motor_action_block_2(action, nullptr); } |
|||
int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int overtime) { |
|||
return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); |
|||
} |
|||
int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int overtime) { |
|||
return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); |
|||
} |
|||
int32_t StepMotorCtrlModule::move_to_zero_block(int overtime) { |
|||
return do_motor_action_block([this]() { return move_to_zero(nullptr); }); |
|||
} |
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) {} |
|||
void StepMotorCtrlModule::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 = 15; |
|||
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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::getid(int32_t* id) { |
|||
*id = m_id; |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::module_stop() { return stop(0); } |
|||
int32_t StepMotorCtrlModule::module_break() { return stop(0); } |
|||
int32_t StepMotorCtrlModule::module_get_last_exec_status(int32_t* status) { |
|||
*status = 0; |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::module_get_error(int32_t* iserror) { |
|||
*iserror = 0; |
|||
return 0; |
|||
} |
|||
|
|||
int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { |
|||
switch (param_id) { |
|||
MODULE_COMMON_PROCESS_REG_CB(); |
|||
PROCESS_REG(kreg_step_motor_pos, read_pos(val), ACTION_NONE); |
|||
PROCESS_REG(kreg_step_motor_shift, REG_GET(m_param.motor_shift), REG_SET(m_param.motor_shift)); |
|||
PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_param.motor_shaft), REG_SET(m_param.motor_shaft)); |
|||
PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_param.motor_one_circle_pulse), REG_SET(m_param.motor_one_circle_pulse)); |
|||
PROCESS_REG(kreg_step_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_step_motor_default_velocity, REG_GET(m_param.motor_default_velocity), REG_SET(m_param.motor_default_velocity)); |
|||
PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_param.motor_default_acc), REG_SET(m_param.motor_default_acc)); |
|||
PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_param.motor_default_dec), REG_SET(m_param.motor_default_dec)); |
|||
PROCESS_REG(kreg_step_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_step_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_step_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_step_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_step_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_step_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_step_motor_ihold, REG_GET(m_param.stepmotor_ihold), REG_SET(m_param.stepmotor_ihold)); |
|||
PROCESS_REG(kreg_step_motor_irun, REG_GET(m_param.stepmotor_irun), REG_SET(m_param.stepmotor_irun)); |
|||
PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_param.stepmotor_iholddelay), REG_SET(m_param.stepmotor_iholddelay)); |
|||
PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_param.stepmotor_iglobalscaler), REG_SET(m_param.stepmotor_iglobalscaler)); |
|||
PROCESS_REG(kreg_step_motor_max_d, REG_GET(m_param.max_d), REG_SET(m_param.max_d)); |
|||
PROCESS_REG(kreg_step_motor_min_d, REG_GET(m_param.min_d), REG_SET(m_param.min_d)); |
|||
PROCESS_REG(kreg_step_motor_io_state, module_readio(&val), ACTION_NONE); |
|||
|
|||
default: |
|||
return err::kmodule_not_find_config_index; |
|||
break; |
|||
} |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::module_clear_error() { return 0; } |
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::_read_io() { |
|||
int32_t iostate = 0; |
|||
module_readio(&iostate); |
|||
return iostate; |
|||
} |
|||
|
|||
int32_t StepMotorCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { |
|||
*adc = 0; |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::module_factory_reset() { return factory_reset(); } |
|||
int32_t StepMotorCtrlModule::module_flush_cfg() { return flush(); } |
|||
int32_t StepMotorCtrlModule::module_active_cfg() { |
|||
active_cfg(); |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::motor_read_pos(int32_t* pos) { |
|||
int32_t xnow = 0; |
|||
getnowpos(xnow); |
|||
*pos = xnow; |
|||
return 0; |
|||
} |
|||
void StepMotorCtrlModule::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4) {} |
|||
|
|||
int32_t StepMotorCtrlModule::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 { |
|||
set_last_exec_status(erro, -dx + m_param.motor_shift); |
|||
m_stepM1->setXACTUAL(0); |
|||
} |
|||
}, |
|||
[this]() { _motor_stop(); }); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::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 StepMotorCtrlModule::motor_easy_move_to_zero(int32_t direction) { |
|||
if (direction <= 0) { |
|||
return move_to_zero(nullptr); |
|||
} else { |
|||
return err::koperation_not_support; |
|||
} |
|||
} |
|||
int32_t StepMotorCtrlModule::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return _exec_move_to_io_task(ioindex, direction); } |
|||
|
|||
/******************************************************************************* |
|||
* EXEC_TASK * |
|||
*******************************************************************************/ |
|||
|
|||
int32_t StepMotorCtrlModule::_exec_move_to_io_task(int32_t ioindex, int32_t direction) { |
|||
m_thread.stop(); |
|||
m_thread.start([this, ioindex, direction]() { _exec_move_to_io_task_fn(ioindex, direction); }); |
|||
return 0; |
|||
} |
|||
int32_t StepMotorCtrlModule::_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 StepMotorCtrlModule::_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 StepMotorCtrlModule::_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; |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue