diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 39b675b..ba6da18 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -394,7 +394,7 @@ void StepMotorCtrlModule::create_default_cfg(flash_config_t& 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_irun = 15; cfg.base_param.stepmotor_iholddelay = 100; cfg.base_param.stepmotor_iglobalscaler = 1; cfg.base_param.motor_default_acc = 300; diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp new file mode 100644 index 0000000..20e2432 --- /dev/null +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -0,0 +1,414 @@ +#include "zcan_protocol_parser.hpp" +#include "sdk/os/zos.hpp" + +#include + +using namespace iflytop; +using namespace std; +#define TAG "PROTO" + +#define REGFN(fn) regCmdFn(k##fn, fn); + +void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { + m_cancmder = cancmder; + m_cancmder->registerListener(this); + + REGFN(module_ping); + REGFN(module_stop); + REGFN(module_break); + REGFN(module_get_last_exec_status); + REGFN(module_get_status); + REGFN(module_set_reg); + REGFN(module_get_reg); + REGFN(module_readio); + REGFN(module_writeio); + REGFN(module_read_adc); + REGFN(module_get_error); + REGFN(module_clear_error); + REGFN(module_set_inited_flag); + REGFN(module_get_inited_flag); + REGFN(module_factory_reset); + REGFN(module_flush_cfg); + REGFN(module_active_cfg); + REGFN(module_read_raw); + REGFN(module_enable); + REGFN(module_start); + + REGFN(motor_enable); + REGFN(motor_rotate_with_torque); + REGFN(motor_calculated_pos_by_move_to_zero); + REGFN(motor_read_pos); + REGFN(motor_set_current_pos_by_change_shift); + REGFN(motor_easy_rotate); + REGFN(motor_easy_move_by); + REGFN(motor_easy_move_to); + REGFN(motor_easy_move_to_zero); + REGFN(motor_easy_set_current_pos); + REGFN(motor_easy_move_to_io); + + REGFN(xymotor_enable); + REGFN(xymotor_move_by); + REGFN(xymotor_move_to); + REGFN(xymotor_move_to_zero); + REGFN(xymotor_move_to_zero_and_calculated_shift); + REGFN(xymotor_read_pos); + REGFN(xymotor_calculated_pos_by_move_to_zero); + + REGFN(pipette_ctrl_init_device); + REGFN(pipette_ctrl_put_tip); + REGFN(pipette_ctrl_move_to_ul); + + REGFN(a8000_optical_module_power_ctrl); + REGFN(a8000_optical_open_laser); + REGFN(a8000_optical_close_laser); + REGFN(a8000_optical_set_laster_gain); + REGFN(a8000_optical_set_scan_amp_gain); + REGFN(a8000_optical_read_scanner_adc_val); + REGFN(a8000_optical_read_laster_adc_val); + REGFN(a8000_optical_scan_current_point_amp_adc_val); +} +void ZCanProtocolParser::_registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; } +void ZCanProtocolParser::registerModule(ZIModule* module) { + int32_t moduleid = 0; + module->getid(&moduleid); + uint16_t id = moduleid; + _registerModule(id, module); +} + +void ZCanProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len) { + // printf("onRceivePacket cmdid:%d subModuleid:%d cmdSubId:%d\n", rxcmd->cmdMainId, rxcmd->subModuleid, rxcmd->cmdSubId); + uint16_t subModuleid = rxcmd->subModuleid; + auto it = m_modulers.find(subModuleid); + if (it == m_modulers.end()) { + return; + } + + ZIModule* module = it->second; + int32_t cmdid = CMDID(rxcmd->cmdMainId, rxcmd->cmdSubId); + _onRceivePacket(module, rxcmd, cmdid, data, len); +} + +void ZCanProtocolParser::regCmdFn(int32_t cmdid, cmdcb_t cb) { + cmdfnlist[cmdfnNum].cmdid = cmdid; + cmdfnlist[cmdfnNum].cb = cb; + cmdfnNum++; +} +ZCanProtocolParser::cmdfn_t* ZCanProtocolParser::findcmdfn(int32_t cmdid) { + for (int i = 0; i < cmdfnNum; i++) { + if (cmdfnlist[i].cmdid == cmdid) { + return &cmdfnlist[i]; + } + } + return nullptr; +} + +void ZCanProtocolParser::_onRceivePacket(ZIModule* module, zcr_cmd_header_t* rxcmd, int32_t cmdid, uint8_t* param, int32_t len) { + int paramNum = (len) / sizeof(int32_t); + int32_t* ack = (int32_t*)&ackbuf[0]; + + cmdfn_t* fn = findcmdfn(cmdid); + if (!fn) { + m_cancmder->sendErrorAck(rxcmd, err::koperation_not_support); + return; + } + cmdcontxt_t cxt = {0}; + cxt.module = module; + cxt.rxcmd = rxcmd; + cxt.cmdid = cmdid; + cxt.param = param; + cxt.paramlen = len; + cxt.acklen = 0; + cxt.ackbuf = ackbuf; + + int32_t ecode = fn->cb(&cxt); + if (ecode != 0) { + m_cancmder->sendErrorAck(rxcmd, ecode); + } else { + m_cancmder->sendBufAck(rxcmd, cxt.ackbuf, cxt.acklen); + } + return; +} + +#define CHECK_AND_GET_MODULE(paraNum) \ + auto* module = dynamic_cast(cxt->module); \ + if (module == nullptr) return err::koperation_not_support; \ + if (paraNum >= 0 && paraNum != cxt->paramlen / 4) return err::kcmd_param_num_error; + +#define MODULE_CLASS ZIModule +int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_ping(); +} +int32_t ZCanProtocolParser::module_stop(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_stop(); +} +int32_t ZCanProtocolParser::module_break(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_break(); +} +int32_t ZCanProtocolParser::module_get_last_exec_status(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_get_last_exec_status(&ack[0]); +} +int32_t ZCanProtocolParser::module_get_status(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_get_status(&ack[0]); +} +int32_t ZCanProtocolParser::module_set_reg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->module_set_reg(cxt->param[0], cxt->param[1]); +} +int32_t ZCanProtocolParser::module_get_reg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_get_reg(cxt->param[0], &ack[0]); +} +int32_t ZCanProtocolParser::module_readio(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_readio(&ack[0]); +} +int32_t ZCanProtocolParser::module_writeio(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->module_writeio(cxt->param[0], cxt->param[1]); +} +int32_t ZCanProtocolParser::module_read_adc(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_read_adc(cxt->param[0], &ack[0]); +} +int32_t ZCanProtocolParser::module_get_error(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_get_error(&ack[0]); +} +int32_t ZCanProtocolParser::module_clear_error(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_clear_error(); +} +int32_t ZCanProtocolParser::module_set_inited_flag(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->module_set_inited_flag(cxt->param[0]); +} +int32_t ZCanProtocolParser::module_get_inited_flag(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->module_get_inited_flag(&ack[0]); +} +int32_t ZCanProtocolParser::module_factory_reset(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_factory_reset(); +} +int32_t ZCanProtocolParser::module_flush_cfg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_flush_cfg(); +} +int32_t ZCanProtocolParser::module_active_cfg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_active_cfg(); +} +int32_t ZCanProtocolParser::module_read_raw(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = ZCANCMD_READ_BUF_MAX_SIZE; + return module->module_read_raw(cxt->param[0], cxt->ackbuf, &cxt->acklen); +} +int32_t ZCanProtocolParser::module_enable(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->module_enable(cxt->param[0]); +} +int32_t ZCanProtocolParser::module_start(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->module_start(); +} +#undef MODULE_CLASS + +/*********************************************************************************************************************** + * ZIMotor * + ***********************************************************************************************************************/ + +#define MODULE_CLASS ZIMotor + +int32_t ZCanProtocolParser::motor_enable(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_enable(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_rotate_with_torque(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->motor_rotate_with_torque(cxt->param[0], cxt->param[1]); +} + +int32_t ZCanProtocolParser::motor_calculated_pos_by_move_to_zero(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->motor_calculated_pos_by_move_to_zero(); +} + +int32_t ZCanProtocolParser::motor_read_pos(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->motor_read_pos(&ack[0]); +} + +int32_t ZCanProtocolParser::motor_set_current_pos_by_change_shift(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_set_current_pos_by_change_shift(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_rotate(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_easy_rotate(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_move_by(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_easy_move_by(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_move_to(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_easy_move_to(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_move_to_zero(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_easy_move_to_zero(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_set_current_pos(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->motor_easy_set_current_pos(cxt->param[0]); +} + +int32_t ZCanProtocolParser::motor_easy_move_to_io(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->motor_easy_move_to_io(cxt->param[0], cxt->param[1]); +} + +#undef MODULE_CLASS + +#define MODULE_CLASS ZIXYMotor +int32_t ZCanProtocolParser::xymotor_enable(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->xymotor_enable(cxt->param[0]); +} + +int32_t ZCanProtocolParser::xymotor_move_by(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(3); + return module->xymotor_move_by(cxt->param[0], cxt->param[1], cxt->param[2]); +} + +int32_t ZCanProtocolParser::xymotor_move_to(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(3); + return module->xymotor_move_to(cxt->param[0], cxt->param[1], cxt->param[2]); +} + +int32_t ZCanProtocolParser::xymotor_move_to_zero(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->xymotor_move_to_zero(); +} + +int32_t ZCanProtocolParser::xymotor_move_to_zero_and_calculated_shift(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->xymotor_move_to_zero_and_calculated_shift(); +} + +int32_t ZCanProtocolParser::xymotor_read_pos(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 8; + return module->xymotor_read_pos(&ack[0], &ack[1]); +} + +int32_t ZCanProtocolParser::xymotor_calculated_pos_by_move_to_zero(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->xymotor_calculated_pos_by_move_to_zero(); +} + +#undef MODULE_CLASS + +/*********************************************************************************************************************** + * ZIPipetteCtrlModule * + ***********************************************************************************************************************/ + +#define MODULE_CLASS ZIPipetteCtrlModule +int32_t ZCanProtocolParser::pipette_ctrl_init_device(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->pipette_ctrl_init_device(); +} + +int32_t ZCanProtocolParser::pipette_ctrl_put_tip(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->pipette_ctrl_put_tip(); +} + +int32_t ZCanProtocolParser::pipette_ctrl_move_to_ul(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->pipette_ctrl_move_to_ul(cxt->param[0]); +} + +#undef MODULE_CLASS + +/*********************************************************************************************************************** + * ZIA8000OpticalModule * + ***********************************************************************************************************************/ + +#define MODULE_CLASS ZIA8000OpticalModule +int32_t ZCanProtocolParser::a8000_optical_module_power_ctrl(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->a8000_optical_module_power_ctrl(cxt->param[0]); +} + +int32_t ZCanProtocolParser::a8000_optical_open_laser(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->a8000_optical_open_laser(cxt->param[0]); +} + +int32_t ZCanProtocolParser::a8000_optical_close_laser(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->a8000_optical_close_laser(cxt->param[0]); +} + +int32_t ZCanProtocolParser::a8000_optical_set_laster_gain(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->a8000_optical_set_laster_gain(cxt->param[0], cxt->param[1]); +} + +int32_t ZCanProtocolParser::a8000_optical_set_scan_amp_gain(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->a8000_optical_set_scan_amp_gain(cxt->param[0], cxt->param[1]); +} + +int32_t ZCanProtocolParser::a8000_optical_read_scanner_adc_val(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->a8000_optical_read_scanner_adc_val(cxt->param[0], &ack[0]); +} + +int32_t ZCanProtocolParser::a8000_optical_read_laster_adc_val(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->a8000_optical_read_laster_adc_val(cxt->param[0], &ack[0]); +} + +int32_t ZCanProtocolParser::a8000_optical_scan_current_point_amp_adc_val(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(4); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 8; + return module->a8000_optical_scan_current_point_amp_adc_val(cxt->param[0], cxt->param[1], cxt->param[2], &ack[0], &ack[1]); +} + +#undef MODULE_CLASS diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp new file mode 100644 index 0000000..afb3ea5 --- /dev/null +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -0,0 +1,103 @@ +#pragma once +#include +#include "a8000_protocol\api\api.hpp" +#include "a8000_protocol\cmdid.hpp" + +namespace iflytop { +class ZCanProtocolParser : public IZCanReceiverListener { + public: + typedef struct { + ZIModule* module; + zcr_cmd_header_t* rxcmd; + int32_t cmdid; + uint8_t* param; + int32_t paramlen; + int32_t acklen; + uint8_t* ackbuf; + } cmdcontxt_t; + + typedef int32_t (*cmdcb_t)(cmdcontxt_t* cxt); + + typedef struct { + int32_t cmdid; + cmdcb_t cb; + } cmdfn_t; + + private: + IZCanReceiver* m_cancmder = nullptr; + map m_modulers; + + uint8_t ackbuf[ZCANCMD_READ_BUF_MAX_SIZE + 100]; + int32_t acklen = 0; + + cmdfn_t cmdfnlist[100]; + int32_t cmdfnNum = 0; + + public: + void initialize(IZCanReceiver* cancmder); + virtual void onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len); + void registerModule(ZIModule* module); + + private: + void _registerModule(uint16_t id, ZIModule* module); + void _onRceivePacket(ZIModule* module, zcr_cmd_header_t* rxcmd, int32_t cmdid, uint8_t* param, int32_t len); + void regCmdFn(int32_t cmdid, cmdcb_t cb); + cmdfn_t* findcmdfn(int32_t cmdid); + + private: + static int32_t module_ping(cmdcontxt_t* cxt); + static int32_t module_stop(cmdcontxt_t* cxt); + static int32_t module_break(cmdcontxt_t* cxt); + static int32_t module_get_last_exec_status(cmdcontxt_t* cxt); + static int32_t module_get_status(cmdcontxt_t* cxt); + static int32_t module_set_reg(cmdcontxt_t* cxt); + static int32_t module_get_reg(cmdcontxt_t* cxt); + static int32_t module_readio(cmdcontxt_t* cxt); + static int32_t module_writeio(cmdcontxt_t* cxt); + static int32_t module_read_adc(cmdcontxt_t* cxt); + static int32_t module_get_error(cmdcontxt_t* cxt); + static int32_t module_clear_error(cmdcontxt_t* cxt); + static int32_t module_set_inited_flag(cmdcontxt_t* cxt); + static int32_t module_get_inited_flag(cmdcontxt_t* cxt); + static int32_t module_factory_reset(cmdcontxt_t* cxt); + static int32_t module_flush_cfg(cmdcontxt_t* cxt); + static int32_t module_active_cfg(cmdcontxt_t* cxt); + static int32_t module_read_raw(cmdcontxt_t* cxt); + static int32_t module_enable(cmdcontxt_t* cxt); + static int32_t module_start(cmdcontxt_t* cxt); + + static int32_t motor_enable(cmdcontxt_t* cxt); + static int32_t motor_rotate_with_torque(cmdcontxt_t* cxt); + static int32_t motor_calculated_pos_by_move_to_zero(cmdcontxt_t* cxt); + static int32_t motor_read_pos(cmdcontxt_t* cxt); + static int32_t motor_set_current_pos_by_change_shift(cmdcontxt_t* cxt); + static int32_t motor_easy_rotate(cmdcontxt_t* cxt); + static int32_t motor_easy_move_by(cmdcontxt_t* cxt); + static int32_t motor_easy_move_to(cmdcontxt_t* cxt); + static int32_t motor_easy_move_to_zero(cmdcontxt_t* cxt); + static int32_t motor_easy_set_current_pos(cmdcontxt_t* cxt); + static int32_t motor_easy_move_to_io(cmdcontxt_t* cxt); + + static int32_t xymotor_enable(cmdcontxt_t* cxt); + static int32_t xymotor_move_by(cmdcontxt_t* cxt); + static int32_t xymotor_move_to(cmdcontxt_t* cxt); + static int32_t xymotor_move_to_zero(cmdcontxt_t* cxt); + static int32_t xymotor_move_to_zero_and_calculated_shift(cmdcontxt_t* cxt); + static int32_t xymotor_read_pos(cmdcontxt_t* cxt); + static int32_t xymotor_calculated_pos_by_move_to_zero(cmdcontxt_t* cxt); + + static int32_t pipette_ctrl_init_device(cmdcontxt_t* cxt); + static int32_t pipette_ctrl_put_tip(cmdcontxt_t* cxt); + static int32_t pipette_ctrl_move_to_ul(cmdcontxt_t* cxt); + + static int32_t a8000_optical_module_power_ctrl(cmdcontxt_t* cxt); + static int32_t a8000_optical_open_laser(cmdcontxt_t* cxt); + static int32_t a8000_optical_close_laser(cmdcontxt_t* cxt); + static int32_t a8000_optical_set_laster_gain(cmdcontxt_t* cxt); + static int32_t a8000_optical_set_scan_amp_gain(cmdcontxt_t* cxt); + static int32_t a8000_optical_read_scanner_adc_val(cmdcontxt_t* cxt); + static int32_t a8000_optical_read_laster_adc_val(cmdcontxt_t* cxt); + static int32_t a8000_optical_scan_current_point_amp_adc_val(cmdcontxt_t* cxt); +}; + +} // namespace iflytop \ No newline at end of file