project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
283569575d
commit
fcad3379ad
3 changed files with 101 additions and 100 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 197
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 2
      components/zprotocols/zcancmder
  3. 2
      components/zprotocols/zcancmder_v2

197
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -87,16 +87,16 @@ int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb)
m_thread.stop(); m_thread.stop();
m_status_cb = status_cb; m_status_cb = status_cb;
if (m_param.min_x != 0 && tox < m_param.min_x) {
tox = m_param.min_x;
if (m_param.min_d != 0 && tox < m_param.min_d) {
tox = m_param.min_d;
} }
if (m_param.max_x != 0 && tox > m_param.max_x) {
tox = m_param.max_x;
if (m_param.max_d != 0 && tox > m_param.max_d) {
tox = m_param.max_d;
} }
m_thread.start( m_thread.start(
[this, tox, status_cb]() { [this, tox, status_cb]() {
_motor_move_to(tox, m_default_speed, m_param.acc, m_param.dec);
_motor_move_to(tox, m_default_speed, m_param.motor_default_acc, m_param.motor_default_dec);
while (!_motor_is_reach_target()) { while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
vTaskDelay(10); vTaskDelay(10);
@ -124,28 +124,28 @@ int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb)
} }
int32_t StepMotorCtrlModule::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { int32_t StepMotorCtrlModule::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) {
if (m_default_speed > m_param.maxspeed) {
ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.maxspeed);
m_default_speed = m_param.maxspeed;
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; if (velocity != 0) m_default_speed = velocity;
return move_to(x, status_cb); return move_to(x, status_cb);
} }
int32_t StepMotorCtrlModule::move_by(int32_t dx, int32_t velocity, action_cb_status_t 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.maxspeed) {
ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.maxspeed);
m_default_speed = m_param.maxspeed;
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; if (velocity != 0) m_default_speed = velocity;
return move_by(dx, status_cb); return move_by(dx, status_cb);
} }
int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) { int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) {
m_default_speed = m_param.maxspeed;
m_default_speed = m_param.motor_default_velocity;
return _move_to(tox, status_cb); return _move_to(tox, status_cb);
} }
int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) { int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) {
m_default_speed = m_param.maxspeed;
m_default_speed = m_param.motor_default_velocity;
return _move_by(dx, status_cb); return _move_by(dx, status_cb);
} }
@ -160,7 +160,7 @@ int32_t StepMotorCtrlModule::calculate_curpos(action_cb_status_t status_cb) {
// //
int32_t dx; int32_t dx;
int32_t erro = exec_move_to_zero_task(dx); int32_t erro = exec_move_to_zero_task(dx);
int xstart = dx + m_param.shift_x;
int xstart = dx + m_param.motor_shift;
m_calculate_curpos_result = xstart; m_calculate_curpos_result = xstart;
call_exec_status_cb(erro, status_cb); call_exec_status_cb(erro, status_cb);
}, },
@ -212,8 +212,8 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb
} }
int32_t calibrate_x; int32_t calibrate_x;
calibrate_x = dx + nowx;
m_param.shift_x = calibrate_x;
calibrate_x = dx + nowx;
m_param.motor_shift = calibrate_x;
m_stepM1->setXACTUAL(0); m_stepM1->setXACTUAL(0);
call_exec_status_cb(erro, status_cb); call_exec_status_cb(erro, status_cb);
@ -247,21 +247,21 @@ int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) {
} }
int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { //
zlock_guard l(m_lock); zlock_guard l(m_lock);
ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec);
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) { if (lastforms < 0) {
ZLOGW(TAG, "lastforms:%d < 0", lastforms); ZLOGW(TAG, "lastforms:%d < 0", lastforms);
return err::kparam_out_of_range; return err::kparam_out_of_range;
} }
if (m_param.maxspeed != 0 && abs(speed) > m_param.maxspeed) {
ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.maxspeed);
speed = m_param.maxspeed;
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.stop();
m_thread.start([this, lastforms, speed, status_cb]() { 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.acc, m_param.dec);
_rotate(speed, m_param.acc, m_param.dec);
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(); int32_t startticket = zos_get_tick();
// bool reachtime = false; // bool reachtime = false;
@ -312,17 +312,17 @@ int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) {
m_param = param; m_param = param;
active_cfg(); active_cfg();
ZLOGI(TAG, "=========== base param ============"); ZLOGI(TAG, "=========== base param ============");
ZLOGI(TAG, "= x_shaft :%d", m_param.x_shaft);
ZLOGI(TAG, "= distance_scale :%f", m_param.distance_scale);
ZLOGI(TAG, "= ihold :%d", m_param.ihold);
ZLOGI(TAG, "= irun :%d", m_param.irun);
ZLOGI(TAG, "= iholddelay :%d", m_param.iholddelay);
ZLOGI(TAG, "= acc :%d", m_param.acc);
ZLOGI(TAG, "= dec :%d", m_param.dec);
ZLOGI(TAG, "= maxspeed :%d", m_param.maxspeed);
ZLOGI(TAG, "= min_x :%d", m_param.min_x);
ZLOGI(TAG, "= max_x :%d", m_param.max_x);
ZLOGI(TAG, "= shift_x :%d", m_param.shift_x);
ZLOGI(TAG, "= x_shaft :%d", m_param.motor_shaft);
ZLOGI(TAG, "= distance_scale :%f", m_param.motor_one_circle_pulse);
ZLOGI(TAG, "= ihold :%d", m_param.stepmotor_ihold);
ZLOGI(TAG, "= irun :%d", m_param.stepmotor_irun);
ZLOGI(TAG, "= iholddelay :%d", m_param.stepmotor_iholddelay);
ZLOGI(TAG, "= acc :%d", m_param.motor_default_acc);
ZLOGI(TAG, "= dec :%d", m_param.motor_default_dec);
ZLOGI(TAG, "= maxspeed :%d", m_param.motor_default_velocity);
ZLOGI(TAG, "= min_x :%d", m_param.min_d);
ZLOGI(TAG, "= max_x :%d", m_param.max_d);
ZLOGI(TAG, "= motor_shift :%d", m_param.motor_shift);
#if 0 #if 0
s32 run_to_zero_max_d; s32 run_to_zero_max_d;
@ -333,12 +333,12 @@ int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) {
s32 look_zero_edge_speed; s32 look_zero_edge_speed;
s32 look_zero_edge_dec; s32 look_zero_edge_dec;
#endif #endif
ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_max_d);
ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.run_to_zero_speed);
ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.run_to_zero_dec);
ZLOGI(TAG, "= look_zero_edge_max_d :%d", m_param.look_zero_edge_max_d);
ZLOGI(TAG, "= look_zero_edge_speed :%d", m_param.look_zero_edge_speed);
ZLOGI(TAG, "= look_zero_edge_dec :%d", m_param.look_zero_edge_dec);
ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.motor_run_to_zero_max_d);
ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.motor_run_to_zero_speed);
ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.motor_run_to_zero_dec);
ZLOGI(TAG, "= look_zero_edge_max_d :%d", m_param.motor_look_zero_edge_max_d);
ZLOGI(TAG, "= look_zero_edge_speed :%d", m_param.motor_look_zero_edge_speed);
ZLOGI(TAG, "= look_zero_edge_dec :%d", m_param.motor_look_zero_edge_dec);
// ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d); // ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d);
// ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d); // ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d);
@ -386,7 +386,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
if (!m_Xgpio->getState()) { if (!m_Xgpio->getState()) {
{ {
ZLOGI(TAG, "---------STEP1-------- move to zero first"); ZLOGI(TAG, "---------STEP1-------- move to zero first");
_rotate(-m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
_rotate(-m_param.motor_run_to_zero_speed, m_param.motor_default_acc, m_param.motor_run_to_zero_dec);
bool xreach_zero = false; bool xreach_zero = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
@ -420,7 +420,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
* @brief 如果设备已经在零点，则反向移动一定距离远离零点 * @brief 如果设备已经在零点，则反向移动一定距离远离零点
*/ */
if (m_Xgpio->getState()) { if (m_Xgpio->getState()) {
_rotate(m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec);
_rotate(m_param.motor_look_zero_edge_speed, m_param.motor_default_acc, m_param.motor_look_zero_edge_dec);
while (true) { while (true) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
if (!m_Xgpio->getState()) { if (!m_Xgpio->getState()) {
@ -496,12 +496,12 @@ void StepMotorCtrlModule::_motor_stop(int32_t dec) {
} }
bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); }
void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) {
// m_zero_shift_x
// m_zero_motor_shift
x = motor_pos; x = motor_pos;
x += m_param.shift_x;
x += m_param.motor_shift;
} }
void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) {
x -= m_param.shift_x;
x -= m_param.motor_shift;
motor_pos = x; motor_pos = x;
} }
#if 0 #if 0
@ -577,22 +577,22 @@ void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t
} }
void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) {
memset(&cfg, 0, sizeof(cfg)); memset(&cfg, 0, sizeof(cfg));
cfg.base_param.distance_scale = 10000;
cfg.base_param.distance_scale_denominator = 1;
cfg.base_param.ihold = 1;
cfg.base_param.irun = 3;
cfg.base_param.iholddelay = 100;
cfg.base_param.acc = 300;
cfg.base_param.dec = 300;
cfg.base_param.maxspeed = 500;
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 = 3;
cfg.base_param.stepmotor_iholddelay = 100;
cfg.base_param.motor_default_acc = 300;
cfg.base_param.motor_default_dec = 300;
cfg.base_param.motor_default_velocity = 500;
cfg.base_param.run_to_zero_max_d = 10000 * 100;
cfg.base_param.run_to_zero_speed = 100;
cfg.base_param.run_to_zero_dec = 600;
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.look_zero_edge_max_d = 10000 * 5;
cfg.base_param.look_zero_edge_speed = 100;
cfg.base_param.look_zero_edge_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() { int32_t StepMotorCtrlModule::flush() {
@ -615,10 +615,10 @@ int32_t StepMotorCtrlModule::factory_reset() {
return 0; return 0;
} }
void StepMotorCtrlModule::active_cfg() { void StepMotorCtrlModule::active_cfg() {
m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay);
m_stepM1->setScale(m_param.distance_scale);
m_stepM1->setScaleDenominator(m_param.distance_scale_denominator);
m_stepM1->setMotorShaft(m_param.x_shaft);
m_stepM1->setIHOLD_IRUN(m_param.stepmotor_ihold, m_param.stepmotor_irun, m_param.stepmotor_iholddelay);
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) { int32_t StepMotorCtrlModule::getid(int32_t* id) {
@ -679,7 +679,7 @@ int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) {
#if 0 #if 0
int32_t StepMotorCtrlModule::module_set_reg(int32_t param_id, int32_t val) { int32_t StepMotorCtrlModule::module_set_reg(int32_t param_id, int32_t val) {
switch (param_id) { switch (param_id) {
SET_REG(kreg_motor_shift, m_param.shift_x);
SET_REG(kreg_motor_shift, m_param.motor_shift);
SET_REG(kreg_motor_shaft, m_param.x_shaft); SET_REG(kreg_motor_shaft, m_param.x_shaft);
SET_REG(kreg_motor_one_circle_pulse, m_param.distance_scale); SET_REG(kreg_motor_one_circle_pulse, m_param.distance_scale);
SET_REG(kreg_motor_one_circle_pulse_denominator, m_param.distance_scale_denominator); SET_REG(kreg_motor_one_circle_pulse_denominator, m_param.distance_scale_denominator);
@ -706,7 +706,7 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t param_id, int32_t* val) {
read_pos(nowpos); read_pos(nowpos);
switch (param_id) { switch (param_id) {
GET_REG(kreg_motor_shift, m_param.shift_x);
GET_REG(kreg_motor_shift, m_param.motor_shift);
GET_REG(kreg_motor_shaft, m_param.x_shaft); GET_REG(kreg_motor_shaft, m_param.x_shaft);
GET_REG(kreg_motor_one_circle_pulse, m_param.distance_scale); GET_REG(kreg_motor_one_circle_pulse, m_param.distance_scale);
GET_REG(kreg_motor_one_circle_pulse_denominator, m_param.distance_scale_denominator); GET_REG(kreg_motor_one_circle_pulse_denominator, m_param.distance_scale_denominator);
@ -743,32 +743,33 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
switch (param_id) { switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB(); MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_robot_pos, read_pos(val), ACTION_NONE); PROCESS_REG(kreg_robot_pos, read_pos(val), ACTION_NONE);
PROCESS_REG(kreg_motor_shift, REG_GET(m_param.shift_x), REG_SET(m_param.shift_x));
PROCESS_REG(kreg_motor_shaft, REG_GET(m_param.x_shaft), REG_SET(m_param.x_shaft));
PROCESS_REG(kreg_motor_one_circle_pulse, REG_GET(m_param.distance_scale), REG_SET(m_param.distance_scale));
PROCESS_REG(kreg_motor_one_circle_pulse_denominator, REG_GET(m_param.distance_scale_denominator), REG_SET(m_param.distance_scale_denominator));
PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_param.maxspeed), REG_SET(m_param.maxspeed));
PROCESS_REG(kreg_motor_default_acc, REG_GET(m_param.acc), REG_SET(m_param.acc));
PROCESS_REG(kreg_motor_default_dec, REG_GET(m_param.dec), REG_SET(m_param.dec));
PROCESS_REG(kreg_motor_run_to_zero_max_d, REG_GET(m_param.run_to_zero_max_d), REG_SET(m_param.run_to_zero_max_d));
PROCESS_REG(kreg_motor_look_zero_edge_max_d, REG_GET(m_param.look_zero_edge_max_d), REG_SET(m_param.look_zero_edge_max_d));
PROCESS_REG(kreg_motor_run_to_zero_speed, REG_GET(m_param.run_to_zero_speed), REG_SET(m_param.run_to_zero_speed));
PROCESS_REG(kreg_motor_run_to_zero_dec, REG_GET(m_param.run_to_zero_dec), REG_SET(m_param.run_to_zero_dec));
PROCESS_REG(kreg_motor_look_zero_edge_speed, REG_GET(m_param.look_zero_edge_speed), REG_SET(m_param.look_zero_edge_speed));
PROCESS_REG(kreg_motor_look_zero_edge_dec, REG_GET(m_param.look_zero_edge_dec), REG_SET(m_param.look_zero_edge_dec));
PROCESS_REG(kreg_stepmotor_ihold, REG_GET(m_param.ihold), REG_SET(m_param.ihold));
PROCESS_REG(kreg_stepmotor_irun, REG_GET(m_param.irun), REG_SET(m_param.irun));
PROCESS_REG(kreg_stepmotor_iholddelay, REG_GET(m_param.iholddelay), REG_SET(m_param.iholddelay));
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_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); PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE);
default: default:
return err::kmodule_not_find_config_index; return err::kmodule_not_find_config_index;
break; break;
} }
return 0; return 0;
} }
int32_t StepMotorCtrlModule::do_action(int32_t actioncode) {
return 0;
}
int32_t StepMotorCtrlModule::do_action(int32_t actioncode) { return 0; }
int32_t StepMotorCtrlModule::module_clear_error() { return 0; } int32_t StepMotorCtrlModule::module_clear_error() { return 0; }
@ -810,7 +811,7 @@ int32_t StepMotorCtrlModule::motor_rotate(int32_t direction, int32_t motor_veloc
m_status_cb = nullptr; m_status_cb = nullptr;
m_thread.stop(); m_thread.stop();
if (acc == 0) { if (acc == 0) {
acc = m_param.acc;
acc = m_param.motor_default_acc;
} }
_rotate(direction * motor_velocity, acc, acc); _rotate(direction * motor_velocity, acc, acc);
return 0; return 0;
@ -828,18 +829,18 @@ int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity,
m_status_cb = nullptr; m_status_cb = nullptr;
m_thread.stop(); m_thread.stop();
if (m_param.min_x != 0 && tox < m_param.min_x) {
tox = m_param.min_x;
if (m_param.min_d != 0 && tox < m_param.min_d) {
tox = m_param.min_d;
} }
if (m_param.max_x != 0 && tox > m_param.max_x) {
tox = m_param.max_x;
if (m_param.max_d != 0 && tox > m_param.max_d) {
tox = m_param.max_d;
} }
if (acc == 0) { if (acc == 0) {
acc = m_param.acc;
acc = m_param.motor_default_acc;
} }
if (motor_velocity == 0) { if (motor_velocity == 0) {
motor_velocity = m_param.maxspeed;
motor_velocity = m_param.motor_default_velocity;
} }
m_thread.start( m_thread.start(
@ -865,14 +866,14 @@ int32_t StepMotorCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, i
int32_t StepMotorCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { 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"); ZLOGI(TAG, "motor_move_to_zero_backward");
if (findzerospeed != 0) { if (findzerospeed != 0) {
m_param.run_to_zero_speed = findzerospeed;
m_param.motor_run_to_zero_speed = findzerospeed;
} }
if (findzeroedge_speed != 0) { if (findzeroedge_speed != 0) {
m_param.look_zero_edge_speed = findzeroedge_speed;
m_param.motor_look_zero_edge_speed = findzeroedge_speed;
} }
if (acc != 0) { if (acc != 0) {
m_param.look_zero_edge_dec = acc;
m_param.run_to_zero_dec = acc;
m_param.motor_look_zero_edge_dec = acc;
m_param.motor_run_to_zero_dec = acc;
} }
return move_to_zero(nullptr); return move_to_zero(nullptr);
} }
@ -909,7 +910,7 @@ int32_t StepMotorCtrlModule::motor_calculated_pos_by_move_to_zero() {
set_last_exec_status(erro); set_last_exec_status(erro);
} else { } else {
m_com_reg.module_last_cmd_exec_status = erro; m_com_reg.module_last_cmd_exec_status = erro;
set_last_exec_status(erro, -dx + m_param.shift_x);
set_last_exec_status(erro, -dx + m_param.motor_shift);
m_stepM1->setXACTUAL(0); m_stepM1->setXACTUAL(0);
} }
}, },
@ -922,7 +923,7 @@ int32_t StepMotorCtrlModule::motor_easy_rotate(int32_t direction) {
ZLOGI(TAG, "m%d motor_easy_rotate %d", m_id, direction); ZLOGI(TAG, "m%d motor_easy_rotate %d", m_id, direction);
m_status_cb = nullptr; m_status_cb = nullptr;
m_thread.stop(); m_thread.stop();
_rotate(direction * m_param.maxspeed, m_param.acc, m_param.dec);
_rotate(direction * m_param.motor_default_velocity, m_param.motor_default_acc, m_param.motor_default_dec);
return 0; return 0;
}; };
@ -931,14 +932,14 @@ int32_t StepMotorCtrlModule::motor_easy_move_by(int32_t distance) {
ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance); ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance);
m_status_cb = nullptr; m_status_cb = nullptr;
m_thread.stop(); m_thread.stop();
return motor_move_by(distance, m_param.maxspeed, m_param.acc);
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) { int32_t StepMotorCtrlModule::motor_easy_move_to(int32_t position) {
zlock_guard lock(m_lock); zlock_guard lock(m_lock);
ZLOGI(TAG, "m%d motor_easy_move_to %d", m_id, position); ZLOGI(TAG, "m%d motor_easy_move_to %d", m_id, position);
m_status_cb = nullptr; m_status_cb = nullptr;
m_thread.stop(); m_thread.stop();
return motor_move_to(position, m_param.maxspeed, m_param.acc);
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) { int32_t StepMotorCtrlModule::motor_easy_move_to_zero(int32_t direction) {
if (direction <= 0) { if (direction <= 0) {

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 6ae11da71aeb4cb0e959b9dabb66c50043d20d03
Subproject commit 5f57d1520c908e86ea1db2f945eeda3bcec87f5d

2
components/zprotocols/zcancmder_v2

@ -1 +1 @@
Subproject commit 1273493bdf1b6770b3cd31e52bfaf84d62ada9cd
Subproject commit 03d55dfe5a8b9fbe0a070211c071e00460e184d0
Write Preview
Loading…
Cancel
Save