diff --git a/a8000_protocol b/a8000_protocol index 590c891..ffd9442 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 590c89179513d49d36fd9ad9cb678e3d9e34be7a +Subproject commit ffd944277ed3f6f63e01d3ab2ea1c14d01fec752 diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 28460d8..10920db 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -108,6 +108,9 @@ int32_t StepMotorCtrlModule::module_set_reg(int32_t regindex, int32_t val) { MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_one_circle_pulse, (m_cfg.motor_one_circle_pulse)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_one_circle_pulse_denominator, (m_cfg.motor_one_circle_pulse_denominator)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_default_velocity, (m_cfg.motor_default_velocity)); + MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_low_velocity, (m_cfg.motor_low_velocity)); + MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_mid_velocity, (m_cfg.motor_mid_velocity)); + MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_high_velocity, (m_cfg.motor_high_velocity)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_run_to_zero_speed, (m_cfg.motor_run_to_zero_speed)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_look_zero_edge_speed, (m_cfg.motor_look_zero_edge_speed)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_ihold, (m_cfg.stepmotor_ihold)); @@ -183,6 +186,9 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t regindex, int32_t* val) { MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_one_circle_pulse, m_cfg.motor_one_circle_pulse); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_one_circle_pulse_denominator, m_cfg.motor_one_circle_pulse_denominator); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_default_velocity, m_cfg.motor_default_velocity); + MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_low_velocity, (m_cfg.motor_low_velocity)); + MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_mid_velocity, (m_cfg.motor_mid_velocity)); + MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_high_velocity, (m_cfg.motor_high_velocity)); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_run_to_zero_speed, m_cfg.motor_run_to_zero_speed); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_look_zero_edge_speed, m_cfg.motor_look_zero_edge_speed); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_ihold, m_cfg.stepmotor_ihold); @@ -573,21 +579,26 @@ int32_t StepMotorCtrlModule::check_befor_run() { return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { +int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { return step_motor_rotate(direction, 0); } +int32_t StepMotorCtrlModule::step_motor_rotate(int32_t direction, int32_t speedlevel) { ZLOGI(TAG, "m%d motor_rotate %d", module_id, direction); int32_t ecode = check_befor_run(); if (ecode != 0) return ecode; - return do_step_motor_easy_rotate(direction); + return do_step_motor_easy_rotate(direction, speedlevel); } -int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { + +int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { return step_motor_move_by(distance, 0); } +int32_t StepMotorCtrlModule::step_motor_move_by(int32_t distance, int32_t speedlevel) { ZLOGI(TAG, "m%d motor_move_by %d", module_id, distance); int32_t ecode = check_befor_run(); if (ecode != 0) return ecode; - return do_step_motor_easy_move_by(distance); + return do_step_motor_easy_move_by(distance, speedlevel); } -int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { + +int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { return step_motor_move_to(tox, 0); } +int32_t StepMotorCtrlModule::step_motor_move_to(int32_t tox, int32_t speedlevel) { ZLOGI(TAG, "m%d motor_move_to %d", module_id, tox); int32_t ecode = check_befor_run(); if (ecode != 0) return ecode; @@ -597,8 +608,9 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { } if (m_cfg.min_d != 0 && tox < m_cfg.min_d) tox = m_cfg.min_d; if (m_cfg.max_d != 0 && tox > m_cfg.max_d) tox = m_cfg.max_d; - return do_step_motor_easy_move_to(tox); + return do_step_motor_easy_move_to(tox, speedlevel); } + int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { ZLOGI(TAG, "m%d motor_move_to_zero", module_id); int32_t ecode = check_befor_run(); @@ -696,15 +708,15 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp return 0; } -int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { +int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction, int32_t speedlevel) { m_thread.stop(); module_status = 1; m_thread.start( - [this, direction]() { + [this, direction, speedlevel]() { befor_motor_move(); { - // rotate(direction, m_cfg.motor_default_velocity); - rotateInVelocityMode(direction >= 0 ? m_cfg.motor_default_velocity : -m_cfg.motor_default_velocity); + int32_t speed = getspeed(speedlevel); + rotateInVelocityMode(direction >= 0 ? speed : -speed); while (true) { if (m_thread.getExitFlag()) break; @@ -718,7 +730,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { [this]() { m_stepM1->stop(); }); return 0; } -int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) { +int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance, int32_t speedlevel) { int32_t motor_pos = 0; step_motor_read_pos(&motor_pos); motor_pos += distance; @@ -726,18 +738,18 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) { int32_t ecode = check_befor_run(); if (ecode != 0) return ecode; - return do_step_motor_easy_move_to(motor_pos); + return do_step_motor_easy_move_to(motor_pos, speedlevel); } -int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) { +int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox, int32_t speedlevel) { m_thread.stop(); module_status = 1; m_thread.start( - [this, tox]() { + [this, tox, speedlevel]() { befor_motor_move(); do { int32_t expectation_dpos = tox - getnowpos(); - moveTo(tox, m_cfg.motor_default_velocity); + moveTo(tox, getspeed(speedlevel)); if (!waitforstop(nullptr)) break; checkdpos(expectation_dpos); @@ -807,8 +819,8 @@ void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) { ZLOGI(TAG, "dpos%d,expect_dpos:%d", dpos, expect_dpos); } } - -int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { +int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { return step_motor_move_to_zero_point_quick(0); } +int32_t StepMotorCtrlModule::step_motor_move_to_zero_point_quick(int32_t speedlevel) { // ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", module_id); int32_t ecode = check_befor_run(); @@ -821,20 +833,21 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { m_thread.stop(); module_status = 1; m_thread.start( - [this]() { + [this, speedlevel]() { befor_motor_move(); - bool moveToZero = false; + bool moveToZero = false; + int32_t speed = getspeed(speedlevel); do { // 期望偏差 int32_t expectation_dpos = -m_cfg.io_trigger_append_distance + 0 + m_cfg.motor_dzero - getnowpos(); if (!m_iotable[0].getState()) { // 快速移动到零点 - moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); + moveTo(0 + m_cfg.motor_dzero, speed); // 等待移动到目标点 if (!waitforstop([this]() { return m_iotable[0].getState(); })) break; } else { // 快速移动到零点 - moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); + moveTo(0 + m_cfg.motor_dzero, speed); // 等待移动到目标点 if (!waitforstop([this]() { return !m_iotable[0].getState(); })) break; } @@ -853,6 +866,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { return 0; } + // int32_t StepMotorCtrlModule::getnowpos(int32_t& x) { if (m_cfg.motor_enable_enc == 0) { @@ -933,3 +947,19 @@ bool StepMotorCtrlModule::waitforstop(function checkstop, int checkperio return suc; } + +int32_t StepMotorCtrlModule::getspeed(int32_t speedlevel) { + int32_t speed = m_cfg.motor_default_velocity; + if (speedlevel == 0) { + speed = m_cfg.motor_default_velocity; + } else if (speedlevel == 1) { + speed = m_cfg.motor_low_velocity; + } else if (speedlevel == 2) { + speed = m_cfg.motor_mid_velocity; + } else if (speedlevel == 3) { + speed = m_cfg.motor_high_velocity; + } else { + speed = m_cfg.motor_default_velocity; + } + return speed; +} diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index e7701c1..d578d74 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -22,6 +22,9 @@ class StepMotorCtrlModule : public ZIModule { int32_t stepmotor_iglobalscaler; int32_t stepmotor_mres; int32_t motor_default_velocity; + int32_t motor_low_velocity; + int32_t motor_mid_velocity; + int32_t motor_high_velocity; int32_t min_d; int32_t max_d; int32_t motor_run_to_zero_speed; @@ -72,8 +75,6 @@ class StepMotorCtrlModule : public ZIModule { * ZIModule * ***********************************************************************************************************************/ - - virtual int32_t module_set_reg(int32_t regindex, int32_t val) override; virtual int32_t module_get_reg(int32_t regindex, int32_t* val) override; virtual int32_t module_reset_reg() override; @@ -93,6 +94,13 @@ class StepMotorCtrlModule : public ZIModule { virtual int32_t step_motor_easy_rotate(int32_t direction); virtual int32_t step_motor_easy_move_by(int32_t distance); virtual int32_t step_motor_easy_move_to(int32_t position); + virtual int32_t step_motor_easy_move_to_zero_point_quick(); + + virtual int32_t step_motor_move_by(int32_t distance, int32_t speedlevel); + virtual int32_t step_motor_move_to(int32_t position, int32_t speedlevel); + virtual int32_t step_motor_move_to_zero_point_quick(int32_t speedlevel); + virtual int32_t step_motor_rotate(int32_t direction, int32_t speedlevel); + virtual int32_t step_motor_easy_move_to_zero(); virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction); virtual int32_t step_motor_easy_move_to_end_point(); @@ -110,8 +118,6 @@ class StepMotorCtrlModule : public ZIModule { virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32); - virtual int32_t step_motor_easy_move_to_zero_point_quick(); - public: TMC51X0* getMotor() { return m_stepM1; } config_t* getCfg() { return &m_cfg; } @@ -119,9 +125,9 @@ class StepMotorCtrlModule : public ZIModule { private: int32_t check_befor_run(); - int32_t do_step_motor_easy_rotate(int32_t direction); - int32_t do_step_motor_easy_move_by(int32_t distance); - int32_t do_step_motor_easy_move_to(int32_t position); + int32_t do_step_motor_easy_rotate(int32_t direction, int32_t speedlevel); + int32_t do_step_motor_easy_move_by(int32_t distance, int32_t speedlevel); + int32_t do_step_motor_easy_move_to(int32_t position, int32_t speedlevel); int32_t do_step_motor_easy_move_to_zero(); int32_t do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction); int32_t do_step_motor_easy_move_to_end_point(); @@ -137,7 +143,6 @@ class StepMotorCtrlModule : public ZIModule { // void _rotate(int32_t vel, int32_t acc, int32_t dec); - private: void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status); void setErrorFlag(int32_t ecode); @@ -155,6 +160,8 @@ class StepMotorCtrlModule : public ZIModule { bool waitforstop(function checkstop, int checkperiod = 20); void checkdpos(int32_t expect_dpos); + private: + int32_t getspeed(int32_t speedlevel); // int32_t read_enc_val(int32_t& enc_val); // int32_t write_enc_val(int32_t enc_val); diff --git a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp index ca1a116..a23261a 100644 --- a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp +++ b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp @@ -50,8 +50,6 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) { REGFN(module_get_type); REGFN(module_get_status); - - REGFN(step_motor_enable); REGFN(step_motor_read_pos); REGFN(step_motor_read_enc_pos); @@ -73,6 +71,10 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) { REGFN(step_motor_set_subdevice_reg); REGFN(step_motor_get_subdevice_reg); REGFN(step_motor_easy_move_to_zero_point_quick); + REGFN(step_motor_move_by); + REGFN(step_motor_move_to); + REGFN(step_motor_move_to_zero_point_quick); + REGFN(step_motor_rotate); REGFN(mini_servo_enable); REGFN(mini_servo_read_pos); @@ -176,9 +178,7 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) { REGFN(pipette_zmotor_read_dev_status_cache); } void ZCanProtocolParser::_registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; } -void ZCanProtocolParser::registerModule(ZIModule* module) { - _registerModule(module->getid(), module); -} +void ZCanProtocolParser::registerModule(ZIModule* module) { _registerModule(module->getid(), module); } void ZCanProtocolParser::processRxPacket(ZcanRxframe* rxframe) { // printf("processRxPacket cmdid:%d moduleid:%d\n", rxcmd->cmdid, rxcmd->moduleId); @@ -285,9 +285,6 @@ void ZCanProtocolParser::_processRxPacket(ZIModule* module, zcr_cmd_header_t* rx #define MODULE_CLASS ZIModule - - - int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); return module->module_ping(); @@ -351,7 +348,6 @@ int32_t ZCanProtocolParser::module_get_type(cmdcontxt_t* cxt) { return module->module_get_type(&ack[0]); } - // int32_t ZCanProtocolParser::module_active_cfg(cmdcontxt_t* cxt) { // CHECK_AND_GET_MODULE(0); // return module->module_active_cfg(); @@ -470,6 +466,22 @@ int32_t ZCanProtocolParser::step_motor_easy_move_to_zero_point_quick(cmdcontxt_t return module->step_motor_easy_move_to_zero_point_quick(); } +int32_t ZCanProtocolParser::step_motor_move_by(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->step_motor_move_by(cxt->params[0], cxt->params[1]); +} +int32_t ZCanProtocolParser::step_motor_move_to(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->step_motor_move_to(cxt->params[0], cxt->params[1]); +} +int32_t ZCanProtocolParser::step_motor_move_to_zero_point_quick(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->step_motor_move_to_zero_point_quick(cxt->params[0]); +} +int32_t ZCanProtocolParser::step_motor_rotate(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->step_motor_rotate(cxt->params[0], cxt->params[1]); +} #undef MODULE_CLASS #define MODULE_CLASS MiniServoCtrlModule // virtual int32_t mini_servo_enable(int32_t enable) = 0; diff --git a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp index 332a6b1..87b5fda 100644 --- a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp +++ b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp @@ -88,6 +88,11 @@ class ZCanProtocolParser : public IZCanRxProcesser { CMDFN(step_motor_easy_reciprocating_motion); CMDFN(step_motor_easy_move_to_zero_point_quick); + CMDFN(step_motor_move_by); + CMDFN(step_motor_move_to); + CMDFN(step_motor_move_to_zero_point_quick); + CMDFN(step_motor_rotate); + CMDFN(mini_servo_enable); CMDFN(mini_servo_read_pos); CMDFN(mini_servo_active_cfg); diff --git a/usrc/version.h b/usrc/version.h index bcd51e4..df0e943 100644 --- a/usrc/version.h +++ b/usrc/version.h @@ -1,2 +1,2 @@ #pragma once -#define APP_VERSION 1100 +#define APP_VERSION 1101