5 changed files with 1052 additions and 1 deletions
-
2components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
-
824components/step_motor_ctrl_module/tmc51x0_motor.cpp
-
213components/step_motor_ctrl_module/tmc51x0_motor.hpp
-
7components/zprotocols/zcancmder_v2/api/zi_motor.hpp
-
7components/zprotocols/zcancmder_v2/cmdid.hpp
@ -0,0 +1,824 @@ |
|||
#include "tmc51x0_motor.hpp"
|
|||
|
|||
#include <stdlib.h>
|
|||
#include <string.h>
|
|||
|
|||
#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<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 TMC51X0Motor::do_motor_action_block(function<int32_t()> 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; |
|||
} |
|||
@ -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<void(int32_t status)> 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<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); |
|||
|
|||
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
|
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue