From b08a126ec4e1a0f5218383af6e79519c20c94404 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 29 May 2025 11:38:25 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B0=83=E6=95=B4TMC5130=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sdk/components/pipette_module/base/pipette_cfg.hpp | 3 + .../step_motor_ctrl_module.cpp | 43 +- sdk/components/tmc/basic/tmc_ic_interface.hpp | 33 -- sdk/components/tmc/ic/ztmc5130.cpp | 54 +-- sdk/components/tmc/ic/ztmc5130.hpp | 30 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 63 ++- .../zcan_protocol_parser/zcan_protocol_parser.cpp | 106 ++--- .../zcan_protocol_parser/zcan_protocol_parser.hpp | 16 +- .../plate_code_scaner_module.cpp | 439 --------------------- .../plate_code_scaner_module.cpp.bak | 439 +++++++++++++++++++++ .../plate_code_scaner_module.hpp | 142 ------- .../plate_code_scaner_module.hpp.bak | 142 +++++++ .../subboard20_plate_clamp_case.cpp | 15 +- .../optical_module_v2.cpp | 62 +-- 14 files changed, 806 insertions(+), 781 deletions(-) delete mode 100644 usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp create mode 100644 usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp.bak delete mode 100644 usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp create mode 100644 usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp.bak diff --git a/sdk/components/pipette_module/base/pipette_cfg.hpp b/sdk/components/pipette_module/base/pipette_cfg.hpp index c7b7bde..dfb7814 100644 --- a/sdk/components/pipette_module/base/pipette_cfg.hpp +++ b/sdk/components/pipette_module/base/pipette_cfg.hpp @@ -50,6 +50,7 @@ typedef struct { int32_t tip_picking_pos; // 开始取tip位置(绝对位置0.1mm) int32_t tip_picking_search_range; // 取tip的范围 int32_t tip_picking_append_distance; // 取tip时z轴的附加距离 + int32_t tip_picking_settling_time; // 取tip后的保持时间(ms) int32_t tip_deposit_pos; // 丢tip位置(绝对位置0.1mm) int32_t _transform_pos; // 移液枪安全移动的高度(绝对位置0.1mm) @@ -72,6 +73,7 @@ typedef enum { kplatinfo_tip_picking_pos, kplatinfo_tip_picking_search_range, kplatinfo_tip_picking_append_distance, + kplatinfo_tip_picking_settling_time, kplatinfo_tip_deposit_pos, kplatinfo_transform_pos, kplatinfo_tip_type, @@ -85,6 +87,7 @@ static inline const char *platinfo_index_to_string(platinfo_index_t index) { CASE_ENUM_TO_STRING(kplatinfo_tip_picking_pos) CASE_ENUM_TO_STRING(kplatinfo_tip_picking_search_range) CASE_ENUM_TO_STRING(kplatinfo_tip_picking_append_distance) + CASE_ENUM_TO_STRING(kplatinfo_tip_picking_settling_time) CASE_ENUM_TO_STRING(kplatinfo_tip_deposit_pos) CASE_ENUM_TO_STRING(kplatinfo_transform_pos) CASE_ENUM_TO_STRING(kplatinfo_tip_type) 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 3a2d1fd..d96e2a1 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 @@ -259,17 +259,20 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() { m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); m_stepM1->setMotorShaft(m_cfg.motor_shaft); m_stepM1->setGlobalScale(m_cfg.stepmotor_iglobalscaler); - - m_stepM1->set_vstart(m_cfg.motor_vstart); - m_stepM1->set_a1(m_cfg.motor_a1); - m_stepM1->set_amax(m_cfg.motor_amax); - m_stepM1->set_v1(m_cfg.motor_v1); - m_stepM1->set_dmax(m_cfg.motor_dmax); - m_stepM1->set_d1(m_cfg.motor_d1); - m_stepM1->set_vstop(m_cfg.motor_vstop); m_stepM1->set_tzerowait(m_cfg.motor_tzerowait); m_stepM1->set_enc_resolution(m_cfg.motor_enc_resolution); + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.motor_a1; + vcfg.amax = m_cfg.motor_amax; + vcfg.v1 = m_cfg.motor_v1; + vcfg.dmax = m_cfg.motor_dmax; + vcfg.d1 = m_cfg.motor_d1; + vcfg.vstart = m_cfg.motor_vstart; + vcfg.vstop = m_cfg.motor_vstop; + vcfg.vmax = m_cfg.motor_default_velocity; + m_stepM1->set_vcfg(&vcfg); + // stepmotor_iglobalscaler if (m_state.enable) { m_stepM1->enable(true); @@ -898,12 +901,32 @@ int32_t StepMotorCtrlModule::setnowpos(int32_t x) { } int32_t StepMotorCtrlModule::moveTo(int32_t x, int32_t v) { trysyncpos(); - m_stepM1->moveTo(x, v); + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.motor_a1; + vcfg.amax = m_cfg.motor_amax; + vcfg.v1 = m_cfg.motor_v1; + vcfg.dmax = m_cfg.motor_dmax; + vcfg.d1 = m_cfg.motor_d1; + vcfg.vstart = m_cfg.motor_vstart; + vcfg.vstop = m_cfg.motor_vstop; + vcfg.vmax = v; + m_stepM1->set_vcfg(&vcfg); + m_stepM1->moveTo(x); return 0; } int32_t StepMotorCtrlModule::rotate(int32_t direction, int32_t v) { trysyncpos(); - m_stepM1->moveToEnd(direction, v); + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.motor_a1; + vcfg.amax = m_cfg.motor_amax; + vcfg.v1 = m_cfg.motor_v1; + vcfg.dmax = m_cfg.motor_dmax; + vcfg.d1 = m_cfg.motor_d1; + vcfg.vstart = m_cfg.motor_vstart; + vcfg.vstop = m_cfg.motor_vstop; + vcfg.vmax = v; + m_stepM1->set_vcfg(&vcfg); + m_stepM1->moveToEnd(direction); return 0; } int32_t StepMotorCtrlModule::rotateInVelocityMode(int32_t v) { diff --git a/sdk/components/tmc/basic/tmc_ic_interface.hpp b/sdk/components/tmc/basic/tmc_ic_interface.hpp index 9e57954..0877308 100644 --- a/sdk/components/tmc/basic/tmc_ic_interface.hpp +++ b/sdk/components/tmc/basic/tmc_ic_interface.hpp @@ -23,37 +23,4 @@ typedef enum { // kTMC5160 = 0x30, } tmcic_id_t; -class IStepperMotor { - public: - IStepperMotor() {} - virtual ~IStepperMotor() {} - virtual void enable(bool enable) = 0; - - virtual void rotate(int32_t velocity) = 0; - virtual void moveTo(int32_t position, uint32_t velocityMax) = 0; - virtual void moveBy(int32_t relativePosition, uint32_t velocityMax) = 0; - virtual void stop() = 0; - - virtual void setXACTUAL(int32_t value) = 0; // 设置当前位置 - virtual int32_t getXACTUAL() = 0; // 当前位置 - virtual int32_t getVACTUAL() = 0; // 当前速度 - - virtual void setAcceleration(float accelerationpps2) = 0; // 设置最大加速度 - virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度 - virtual void setMotorShaft(bool reverse) = 0; - virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; - virtual void setGlobalScale(uint8_t globalscale) = 0; - virtual void setScale(int32_t scale) = 0; - virtual void setScaleDenominator(int32_t scale) = 0; - - virtual bool isReachTarget() = 0; // 是否到达目标位置 - virtual bool isStoped() = 0; // 是否停止运动 - - virtual void setNoAccLimit(bool enable) = 0; - - - - -}; - } // namespace iflytop diff --git a/sdk/components/tmc/ic/ztmc5130.cpp b/sdk/components/tmc/ic/ztmc5130.cpp index 679815b..0416e1f 100644 --- a/sdk/components/tmc/ic/ztmc5130.cpp +++ b/sdk/components/tmc/ic/ztmc5130.cpp @@ -257,34 +257,34 @@ void TMC51X0::rotate(int32_t velocity) { writeInt(TMC5130_VMAX, abs(velocity)); writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); } -void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) { - // position *= m_scale; - // velocityMax *= m_scale; - position = to_motor_pos(position); - velocityMax = to_motor_vel(velocityMax); - // ZLOGI("TMC5130", "moveTo %d %d", position, velocityMax); - writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); - writeInt(TMC5130_VMAX, velocityMax); - writeInt(TMC5130_XTARGET, position); -} -void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) { - ZLOGI("TMC5130", "moveToEnd %d %d", direction, velocityMax); - if (direction > 0) { - writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); - writeInt(TMC5130_VMAX, to_motor_vel(velocityMax)); - writeInt(TMC5130_XTARGET, INT32_MAX / 2 - 1000); - } else { - writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); - writeInt(TMC5130_VMAX, to_motor_vel(velocityMax)); - writeInt(TMC5130_XTARGET, INT32_MIN / 2 + 1000); - } -} +// void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) { +// // position *= m_scale; +// // velocityMax *= m_scale; +// position = to_motor_pos(position); +// velocityMax = to_motor_vel(velocityMax); +// // ZLOGI("TMC5130", "moveTo %d %d", position, velocityMax); +// writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); +// writeInt(TMC5130_VMAX, velocityMax); +// writeInt(TMC5130_XTARGET, position); +// } +// void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) { +// ZLOGI("TMC5130", "moveToEnd %d %d", direction, velocityMax); +// if (direction > 0) { +// writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); +// writeInt(TMC5130_VMAX, to_motor_vel(velocityMax)); +// writeInt(TMC5130_XTARGET, INT32_MAX / 2 - 1000); +// } else { +// writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); +// writeInt(TMC5130_VMAX, to_motor_vel(velocityMax)); +// writeInt(TMC5130_XTARGET, INT32_MIN / 2 + 1000); +// } +// } -void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move - int32_t pos = getXACTUAL(); - int32_t target = pos + relativePosition; - moveTo(target, velocityMax); -} +// void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move +// int32_t pos = getXACTUAL(); +// int32_t target = pos + relativePosition; +// moveTo(target, velocityMax); +// } void TMC51X0::rotateDirection(int32_t direction) { writeInt(TMC5130_RAMPMODE, (direction >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); } void TMC51X0::moveTo(int32_t position) { diff --git a/sdk/components/tmc/ic/ztmc5130.hpp b/sdk/components/tmc/ic/ztmc5130.hpp index a30dd5c..a312289 100644 --- a/sdk/components/tmc/ic/ztmc5130.hpp +++ b/sdk/components/tmc/ic/ztmc5130.hpp @@ -62,7 +62,7 @@ class Tmc5130RampStat { bool isSetted(ramp_stat_bit_t bit) { return (m_state & bit) != 0; } }; -class TMC51X0 : public IStepperMotor { +class TMC51X0 { public: typedef struct { uint32_t sg_result : 10; @@ -138,9 +138,9 @@ class TMC51X0 : public IStepperMotor { virtual void enable(bool enable); virtual void rotate(int32_t velocity); - virtual void moveTo(int32_t position, uint32_t velocityMax); - virtual void moveToEnd(int32_t direction, uint32_t velocityMax); - virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); + // virtual void moveTo(int32_t position, uint32_t velocityMax); + // virtual void moveToEnd(int32_t direction, uint32_t velocityMax); + // virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); virtual void rotateDirection(int32_t direction); virtual void moveTo(int32_t position); @@ -171,17 +171,7 @@ class TMC51X0 : public IStepperMotor { virtual bool isReachTarget(); // 是否到达目标位置 virtual bool isStoped() { return isReachTarget(); } - virtual void setNoAccLimit(bool enable) override; - - virtual void set_vstart(int32_t val); - virtual void set_a1(int32_t val); - virtual void set_amax(int32_t val); - virtual void set_v1(int32_t val); - virtual void set_dmax(int32_t val); - virtual void set_d1(int32_t val); - virtual void set_vstop(int32_t val); - virtual void set_tzerowait(int32_t val); - virtual void set_vmax(int32_t vmax); + virtual void setNoAccLimit(bool enable) ; virtual void set_vcfg(VCfg_t *vcfg); @@ -221,6 +211,7 @@ class TMC51X0 : public IStepperMotor { void writeInt(uint8_t address, int32_t value); int32_t readInt(uint8_t address); void setMRES(mres_type_t value); + virtual void set_tzerowait(int32_t val); private: uint32_t haspassedms(uint32_t now, uint32_t last); @@ -231,6 +222,15 @@ class TMC51X0 : public IStepperMotor { int32_t to_user_pos(int32_t pos); // int32_t to_user_vel(int32_t vel); + + virtual void set_vstart(int32_t val); + virtual void set_a1(int32_t val); + virtual void set_amax(int32_t val); + virtual void set_v1(int32_t val); + virtual void set_dmax(int32_t val); + virtual void set_d1(int32_t val); + virtual void set_vstop(int32_t val); + virtual void set_vmax(int32_t vmax); }; } // namespace iflytop #endif diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 11867ad..787f638 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -90,13 +90,13 @@ int32_t XYRobotCtrlModule::module_active_cfg() { m_stepM1->setGlobalScale(m_cfg.iglobalscaler); m_stepM1->setScale(m_cfg.one_circle_pulse / 2); m_stepM1->setScaleDenominator(m_cfg.one_circle_pulse_denominator); - m_stepM1->set_vstart(m_cfg.vstart); - m_stepM1->set_a1(m_cfg.a1); - m_stepM1->set_amax(m_cfg.amax); - m_stepM1->set_v1(m_cfg.v1); - m_stepM1->set_dmax(m_cfg.dmax); - m_stepM1->set_d1(m_cfg.d1); - m_stepM1->set_vstop(m_cfg.vstop); + // m_stepM1->set_vstart(m_cfg.vstart); + // m_stepM1->set_a1(m_cfg.a1); + // m_stepM1->set_amax(m_cfg.amax); + // m_stepM1->set_v1(m_cfg.v1); + // m_stepM1->set_dmax(m_cfg.dmax); + // m_stepM1->set_d1(m_cfg.d1); + // m_stepM1->set_vstop(m_cfg.vstop); m_stepM1->set_tzerowait(m_cfg.tzerowait); m_stepM1->set_enc_resolution(m_cfg.enc_resolution); @@ -104,13 +104,13 @@ int32_t XYRobotCtrlModule::module_active_cfg() { m_stepM2->setGlobalScale(m_cfg.iglobalscaler); m_stepM2->setScale(m_cfg.one_circle_pulse / 2); m_stepM2->setScaleDenominator(m_cfg.one_circle_pulse_denominator); - m_stepM2->set_vstart(m_cfg.vstart); - m_stepM2->set_a1(m_cfg.a1); - m_stepM2->set_amax(m_cfg.amax); - m_stepM2->set_v1(m_cfg.v1); - m_stepM2->set_dmax(m_cfg.dmax); - m_stepM2->set_d1(m_cfg.d1); - m_stepM2->set_vstop(m_cfg.vstop); + // m_stepM2->set_vstart(m_cfg.vstart); + // m_stepM2->set_a1(m_cfg.a1); + // m_stepM2->set_amax(m_cfg.amax); + // m_stepM2->set_v1(m_cfg.v1); + // m_stepM2->set_dmax(m_cfg.dmax); + // m_stepM2->set_d1(m_cfg.d1); + // m_stepM2->set_vstop(m_cfg.vstop); m_stepM2->set_tzerowait(m_cfg.tzerowait); m_stepM2->set_enc_resolution(m_cfg.enc_resolution); @@ -560,8 +560,21 @@ void XYRobotCtrlModule::moveTo(int32_t t_x, int32_t t_y, int32_t v) { int32_t target_m1pos, target_m2pos; forward_kinematics(t_x, t_y, target_m1pos, target_m2pos); ZLOGI(TAG, "moveTo x:%d y:%d m1:%d m2:%d v:%d", t_x, t_y, target_m1pos, target_m2pos, v); - m_stepM1->moveTo(target_m1pos, v); - m_stepM2->moveTo(target_m2pos, v); + + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.a1; + vcfg.amax = m_cfg.amax; + vcfg.v1 = m_cfg.v1; + vcfg.dmax = m_cfg.dmax; + vcfg.d1 = m_cfg.d1; + vcfg.vstart = m_cfg.vstart; + vcfg.vstop = m_cfg.vstop; + vcfg.vmax = v; + m_stepM1->set_vcfg(&vcfg); + m_stepM2->set_vcfg(&vcfg); + + m_stepM1->moveTo(target_m1pos); + m_stepM2->moveTo(target_m2pos); } void XYRobotCtrlModule::moveBy(int32_t dx, int32_t dy, int32_t v) { int32_t x, y; @@ -585,8 +598,22 @@ int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) { } int32_t XYRobotCtrlModule::xymotor_motor_move_by_direct(int32_t motor1_dpos, int32_t motor2_dpos) { - m_stepM1->moveBy(motor1_dpos, m_cfg.default_velocity); - m_stepM2->moveBy(motor2_dpos, m_cfg.default_velocity); + + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.a1; + vcfg.amax = m_cfg.amax; + vcfg.v1 = m_cfg.v1; + vcfg.dmax = m_cfg.dmax; + vcfg.d1 = m_cfg.d1; + vcfg.vstart = m_cfg.vstart; + vcfg.vstop = m_cfg.vstop; + vcfg.vmax = m_cfg.default_velocity; + m_stepM1->set_vcfg(&vcfg); + m_stepM2->set_vcfg(&vcfg); + + + m_stepM1->moveBy(motor1_dpos); + m_stepM2->moveBy(motor2_dpos); return 0; } int32_t XYRobotCtrlModule::xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) { diff --git a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp index a3503ce..673a4ea 100644 --- a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp +++ b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp @@ -12,7 +12,7 @@ #include "sdk\components\water_cooling_temperature_control_module\water_cooling_temperature_control_module.hpp" #include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp" #include "subboards\subboard100_idcard_reader\eeprom_service.hpp" -#include "subboards\subboard20_plate_clamp_case\plate_code_scaner_module.hpp" +// #include "subboards\subboard20_plate_clamp_case\plate_code_scaner_module.hpp" #include "subboards\subboard40_and_50_temperature_ctrl\zcan_fan_ctrl_module.hpp" #include "subboards\subboard90_optical_module\optical_module_v2.hpp" // @@ -140,14 +140,14 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) { REGFN(a8000_idcard_erase); REGFN(a8000_idcard_earse_unlock); - REGFN(plate_code_scaner_push_card_and_scan); - REGFN(plate_code_scaner_stop_scan); - REGFN(plate_code_scaner_read_result); - REGFN(plate_code_scaner_read_result_point_num); - REGFN(plate_code_scaner_read_code); - REGFN(plate_code_scaner_adc_readraw); - REGFN(plate_code_scaner_open_laser); - REGFN(plate_code_scaner_close_laser); + // REGFN(plate_code_scaner_push_card_and_scan); + // REGFN(plate_code_scaner_stop_scan); + // REGFN(plate_code_scaner_read_result); + // REGFN(plate_code_scaner_read_result_point_num); + // REGFN(plate_code_scaner_read_code); + // REGFN(plate_code_scaner_adc_readraw); + // REGFN(plate_code_scaner_open_laser); + // REGFN(plate_code_scaner_close_laser); REGFN(pipette_set_zmbcfg); REGFN(pipette_get_zmbcfg); @@ -858,58 +858,58 @@ int32_t ZCanProtocolParser::a8000_idcard_earse_unlock(cmdcontxt_t* cxt) { #undef MODULE_CLASS -#define MODULE_CLASS PlateCodeScanerModule -int32_t ZCanProtocolParser::plate_code_scaner_push_card_and_scan(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(1); - return module->plate_code_scaner_push_card_and_scan(cxt->params[0]); -} +// #define MODULE_CLASS PlateCodeScanerModule +// int32_t ZCanProtocolParser::plate_code_scaner_push_card_and_scan(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(1); +// return module->plate_code_scaner_push_card_and_scan(cxt->params[0]); +// } -int32_t ZCanProtocolParser::plate_code_scaner_stop_scan(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - return module->plate_code_scaner_stop_scan(); -} +// int32_t ZCanProtocolParser::plate_code_scaner_stop_scan(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// return module->plate_code_scaner_stop_scan(); +// } -int32_t ZCanProtocolParser::plate_code_scaner_read_result(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(1); - cxt->acklen = ZCANCMD_PACKET_MAX_LEN; - int32_t suc = module->plate_code_scaner_read_result(cxt->params[0], cxt->ackbuf, &cxt->acklen); - if (suc != 0) { - cxt->acklen = 0; - } - return suc; -} +// int32_t ZCanProtocolParser::plate_code_scaner_read_result(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(1); +// cxt->acklen = ZCANCMD_PACKET_MAX_LEN; +// int32_t suc = module->plate_code_scaner_read_result(cxt->params[0], cxt->ackbuf, &cxt->acklen); +// if (suc != 0) { +// cxt->acklen = 0; +// } +// return suc; +// } -int32_t ZCanProtocolParser::plate_code_scaner_read_result_point_num(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = 4; - return module->plate_code_scaner_read_result_point_num(&ack[0]); -} +// int32_t ZCanProtocolParser::plate_code_scaner_read_result_point_num(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// int32_t* ack = (int32_t*)cxt->ackbuf; +// cxt->acklen = 4; +// return module->plate_code_scaner_read_result_point_num(&ack[0]); +// } -int32_t ZCanProtocolParser::plate_code_scaner_read_code(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - cxt->acklen = 4 * 4; - return module->plate_code_scaner_read_code(&cxt->params[0], &cxt->params[1], &cxt->params[2], &cxt->params[3]); -} +// int32_t ZCanProtocolParser::plate_code_scaner_read_code(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// cxt->acklen = 4 * 4; +// return module->plate_code_scaner_read_code(&cxt->params[0], &cxt->params[1], &cxt->params[2], &cxt->params[3]); +// } -int32_t ZCanProtocolParser::plate_code_scaner_adc_readraw(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = 4; - return module->plate_code_scaner_adc_readraw(&ack[0]); -} +// int32_t ZCanProtocolParser::plate_code_scaner_adc_readraw(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// int32_t* ack = (int32_t*)cxt->ackbuf; +// cxt->acklen = 4; +// return module->plate_code_scaner_adc_readraw(&ack[0]); +// } -int32_t ZCanProtocolParser::plate_code_scaner_open_laser(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - return module->plate_code_scaner_open_laser(); -} +// int32_t ZCanProtocolParser::plate_code_scaner_open_laser(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// return module->plate_code_scaner_open_laser(); +// } -int32_t ZCanProtocolParser::plate_code_scaner_close_laser(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - return module->plate_code_scaner_close_laser(); -} +// int32_t ZCanProtocolParser::plate_code_scaner_close_laser(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// return module->plate_code_scaner_close_laser(); +// } -#undef MODULE_CLASS +// #undef MODULE_CLASS /*********************************************************************************************************************** * PipetteModuleV2 * diff --git a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp index 32223c9..f1df48e 100644 --- a/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp +++ b/sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp @@ -160,14 +160,14 @@ class ZCanProtocolParser : public IZCanRxProcesser { CMDFN(a8000_idcard_erase); CMDFN(a8000_idcard_earse_unlock); - CMDFN(plate_code_scaner_push_card_and_scan); - CMDFN(plate_code_scaner_stop_scan); - CMDFN(plate_code_scaner_read_result); - CMDFN(plate_code_scaner_read_result_point_num); - CMDFN(plate_code_scaner_read_code); - CMDFN(plate_code_scaner_adc_readraw); - CMDFN(plate_code_scaner_open_laser); - CMDFN(plate_code_scaner_close_laser); + // CMDFN(plate_code_scaner_push_card_and_scan); + // CMDFN(plate_code_scaner_stop_scan); + // CMDFN(plate_code_scaner_read_result); + // CMDFN(plate_code_scaner_read_result_point_num); + // CMDFN(plate_code_scaner_read_code); + // CMDFN(plate_code_scaner_adc_readraw); + // CMDFN(plate_code_scaner_open_laser); + // CMDFN(plate_code_scaner_close_laser); // CMDFN(pipette_set_zmbcfg); diff --git a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp deleted file mode 100644 index 01615b4..0000000 --- a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp +++ /dev/null @@ -1,439 +0,0 @@ -#include "plate_code_scaner_module.hpp" - -#include -#include -#include - -using namespace iflytop; -using namespace std; -#define RAW_SECTION_SIZE 10 - -#define ONE_BIT_WIDTH 12 // 12*.0.1mm -#define BIT_NUM 15 -#define MAX_POINT (12 * 15 + 2 * 12 + 1) // 最多12*15+2个点,多扫描的两个点用于对齐使用 - -#define TAG "PlateCodeScaner" -#define LOGI(fmt, ...) ZLOGI(TAG, fmt, ##__VA_ARGS__) - -#define DO(func) \ - { \ - int32_t ecode = func; \ - if (ecode != 0) { \ - ZLOGE(TAG, "do %s fail, error %s(%d)", #func, err::error2str(ecode), ecode); \ - return ecode; \ - } \ - } - -#define DO_IN_THREAD(func) \ - { \ - int32_t ecode = func; \ - if (ecode != 0) { \ - ZLOGE(TAG, "do %s fail, error %s(%d)", #func, err::error2str(ecode), ecode); \ - module_errorcode = ecode; \ - return; \ - } \ - } - -#define BITVAL(code, off) ((code & (1 << off)) >> off) -static void decode(uint32_t rawcode, int32_t* pitem, int32_t* plot) { // - // Lot:3:6 - // bit3->bit3, bit4->bit2, bit5->bit1, bit6->bit0 - uint32_t lot = 0; - lot = BITVAL(rawcode, 3) << 3 | BITVAL(rawcode, 4) << 2 | BITVAL(rawcode, 5) << 1 | BITVAL(rawcode, 6) << 0; - - // =(bit1)*2^6+(bit2)*2^5+(bit11)*2^4+(bit10)*2^0+(bit9)*2^1+(bit8)*2^2+(bit7)*2^3 - uint32_t item = 0; - item = (BITVAL(rawcode, 1) << 6) // - | (BITVAL(rawcode, 2) << 5) // - | (BITVAL(rawcode, 11) << 4) // - | (BITVAL(rawcode, 10) << 0) // - | (BITVAL(rawcode, 9) << 1) // - | (BITVAL(rawcode, 8) << 2) // - | (BITVAL(rawcode, 7) << 3); - - ZLOGI(TAG, "item-lot: %d-%d\n", item, lot); - *pitem = item; - *plot = lot; -} - -static int32_t t_detector_gain_to_potentiometer_val(float scan_gain) { - int32_t potentiometer_val = 0; - potentiometer_val = (scan_gain * 4.7 - 2.4) * 256. / 100. + 0.5; - if (potentiometer_val < 1) potentiometer_val = 1; - if (potentiometer_val > 255) potentiometer_val = 255; - return potentiometer_val; -} - -void PlateCodeScanerModule::initialize(int32_t moduleid, hardware_config_t* hardwarecfg) { // - module_id = moduleid; - m_hardwarecfg = *hardwarecfg; - - m_t_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_amp_mcp41); - m_t_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_scaner_mcp41); - m_f_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_amp_mcp41); - m_f_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_scaner_mcp41); - - m_t_laster_enable_io.initAsOutput(&m_hardwarecfg.t_laster_enable_io); - m_f_opt_laster_enable_io.initAsOutput(&m_hardwarecfg.f_opt_laster_enable_io); - m_channel_select_io_red_led3.initAsOutput(&m_hardwarecfg.channel_select_io_red_led3); - m_channel_select_io_blue_led1.initAsOutput(&m_hardwarecfg.channel_select_io_blue_led1); - m_channel_select_io_green_led1.initAsOutput(&m_hardwarecfg.channel_select_io_green_led1); - m_amp_negative_5v_gpio.initAsOutput(&m_hardwarecfg.amp_negative_5v_gpio); - - m_t_amp_sw_io.initAsOutput(&m_hardwarecfg.t_amp_sw_io); - m_f_amp_sw_io.initAsOutput(&m_hardwarecfg.f_amp_sw_io); - - t_laster_adc.initialize(&m_hardwarecfg.t_laster_adc); - t_result_adc.initialize(&m_hardwarecfg.t_result_adc); - f_laster_adc.initialize(&m_hardwarecfg.f_laster_adc); - f_result_adc.initialize(&m_hardwarecfg.f_result_adc); - - m_motor = (StepMotorCtrlModule*)m_hardwarecfg.motor; - ZASSERT(m_motor); - - adc_capture_buf = (int16_t*)calloc(2, MAX_POINT); - ZASSERT(adc_capture_buf); - - m_t_laster_enable_io.setState(false); - m_amp_negative_5v_gpio.setState(false); - m_t_amp_sw_io.setState(false); - m_f_amp_sw_io.setState(false); - - // while (true) { - // int32_t adcv; - // t_result_adc.get_adc_value(adcv); - // ZLOGI(TAG, "t:%d", adcv); - // osDelay(100); - // } - - m_reg.laster_intensity = 99; - m_reg.scan_gain = 1; - m_reg.scan_velocity = 50; - m_reg.scan_start_pos = 960; - m_reg.code_judgment_threshold = 1000; - m_reg.item = 0; - m_reg.lot = 0; - m_reg.rawcode = 0; - m_reg.code_legal = -1; - m_thread.init("PlateCodeScanerModule", 1024, osPriorityNormal); -} - -int32_t PlateCodeScanerModule::module_set_reg(int32_t regindex, int32_t val) { - int32_t ret = 0; - switch (regindex) { - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_laster_intensity, m_reg.laster_intensity); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_gain, m_reg.scan_gain); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_velocity, m_reg.scan_velocity); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_start_pos, m_reg.scan_start_pos); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_code_judgment_threshold, m_reg.code_judgment_threshold); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_item, m_reg.item); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_lot, m_reg.lot); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_rawcode, m_reg.rawcode); - MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_code_legal, m_reg.code_legal); - default: - return err::kmodule_not_find_reg; - break; - } - if (ret != 0) return ret; - module_active_cfg(); - return 0; -} -int32_t PlateCodeScanerModule::module_get_reg(int32_t regindex, int32_t* val) { - int32_t ret = 0; - switch (regindex) { - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_laster_intensity, m_reg.laster_intensity); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_gain, m_reg.scan_gain); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_velocity, m_reg.scan_velocity); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_start_pos, m_reg.scan_start_pos); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_code_judgment_threshold, m_reg.code_judgment_threshold); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_item, m_reg.item); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_lot, m_reg.lot); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_rawcode, m_reg.rawcode); - MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_code_legal, m_reg.code_legal); - default: - return err::kmodule_not_find_reg; - break; - } - if (ret != 0) return ret; - return 0; -} - -int32_t PlateCodeScanerModule::module_stop() { - m_thread.stop(); - return 0; -} - -int32_t PlateCodeScanerModule::module_active_cfg() { - scaner_set_gain(m_reg.scan_gain / 10.0); - laser_set_intensity(m_reg.laster_intensity); - return 0; -} - -int32_t PlateCodeScanerModule::plate_code_scaner_push_card_and_scan(int32_t final_stop_pos) { - ZLOGI(TAG, "plate_code_scaner_push_card_and_scan: %d", final_stop_pos); - /** - * @brief - */ - m_thread.stop(); - module_status = 1; - m_thread.start( - - [this, final_stop_pos]() { // - DO_IN_THREAD(before_run()); - ZLOGI(TAG, "plate_code_scaner_open_laser"); - plate_code_scaner_open_laser(); - - /*********************************************************************************************************************** - * 移动电机到扫描启动位置 * - ***********************************************************************************************************************/ - ZLOGI(TAG, "step_motor_easy_move_to start pos: %d", m_reg.scan_start_pos); - m_motor->step_motor_easy_move_to(m_reg.scan_start_pos); - while (true) { - if (!check_when_run() || m_thread.getExitFlag()) break; - - int32_t status = 0; - m_motor->module_get_status(&status); - if (status == 0) break; // 电机停止 - if (status == 2) return; // 电机异常 - osDelay(5); - } - ZLOGI(TAG, "has moved to start pos"); - /*********************************************************************************************************************** - * 开始扫码 * - ***********************************************************************************************************************/ - ZLOGI(TAG, "start scan"); - - m_reg.item = -1; - m_reg.lot = -1; - m_reg.rawcode = 0; - m_reg.code_legal = -1; - - auto _motor = m_motor->getMotor(); - int32_t targetpos = 0; - targetpos = m_reg.scan_start_pos + ONE_BIT_WIDTH * BIT_NUM; - ZLOGI(TAG, "move to: %d", targetpos); - _motor->moveTo(targetpos, m_reg.scan_velocity); - - reset_capture_buf(); - // chip_critical_enter(); - while (true) { - if (m_thread.getExitFlag()) break; - if (_motor->isReachTarget()) break; - - // 读取ADC - int32_t pos = _motor->getXACTUAL(); - int16_t adcv = read_adc_val(); - int32_t dpos = abs(pos - m_reg.scan_start_pos); - push_one_point(dpos, adcv); - // osDelay(1); - } - // chip_critical_exit(); - - /*********************************************************************************************************************** - * 移动到扫码结束位置 * - ***********************************************************************************************************************/ - - ZLOGI(TAG, "step_motor_easy_move_to end pos: %d", final_stop_pos); - m_motor->step_motor_easy_move_to(final_stop_pos); - while (true) { - if (!check_when_run() || m_thread.getExitFlag()) break; - - int32_t status = 0; - m_motor->module_get_status(&status); - if (status == 0) break; // 电机停止 - if (status == 2) return; // 电机异常 - osDelay(5); - } - ZLOGI(TAG, "has moved to start pos"); - dumppoint(); - parsepoint(); - - }, - [this]() { // exit fn - after_run(); - m_motor->step_motor_stop(0); - plate_code_scaner_close_laser(); - } // - ); - - return 0; -} - -int32_t PlateCodeScanerModule::plate_code_scaner_stop_scan() { - ZLOGI(TAG, "plate_code_scaner_stop_scan"); - m_thread.stop(); - return 0; -} - -void PlateCodeScanerModule::push_one_point(int32_t pos, int16_t pointval) { - if (pos >= MAX_POINT) { - return; - } - if (pos < 0) { - return; - } - - for (int32_t i = adc_capture_last_point_index + 1; i < pos; i++) { - adc_capture_buf[i] = adc_capture_buf[adc_capture_last_point_index]; - } - - adc_capture_buf[pos] = pointval; - adc_capture_point_num = pos + 1; - adc_capture_last_point_index = pos; -} -void PlateCodeScanerModule::reset_capture_buf() { - adc_capture_point_num = 0; - adc_capture_last_point_index = 0; - for (int32_t i = 0; i < MAX_POINT; i++) { - adc_capture_buf[i] = 0; - } -} - -void PlateCodeScanerModule::dumppoint() { - ZLOGI(TAG, " ===== capture points: %d ==== ", adc_capture_point_num); - for (int32_t i = 0; i < adc_capture_point_num; i++) { - ZLOGI(TAG, "%5d %5d", i, adc_capture_buf[i]); - } -} -void PlateCodeScanerModule::parsepoint() { - // - uint8_t code[15]; - for (int32_t i = 0; i < 15; i++) { - int pos = i * 12 + 6; - // 黑色数值更小，黑色1，白色0 - if (adc_capture_buf[i] < m_reg.code_judgment_threshold) { - code[i] = 1; - } else { - code[i] = 0; - } - } - - uint32_t rawcode = 0; - - for (int32_t i = 0; i < 15; i++) { - rawcode |= code[i] << i; - } - - int32_t item = 0; - int32_t lot = 0; - decode(rawcode, &item, &lot); - - m_reg.item = item; - m_reg.lot = lot; - m_reg.rawcode = rawcode; - - if (code[14] == 1 && code[13] == 0 && code[12] == 0) { - m_reg.code_legal = 1; - } else { - m_reg.code_legal = 0; - } - - ZLOGI(TAG, "rawcode:%x code: %d-%d (legal:%d)", rawcode, m_reg.item, m_reg.lot, m_reg.code_legal); -} - -int32_t PlateCodeScanerModule::before_run() { return 0; } -int32_t PlateCodeScanerModule::after_run() { - if (module_errorcode != 0) { - module_status = 2; - } else { - module_status = 0; - } - return 0; -} -bool PlateCodeScanerModule::check_when_run() { return true; } - -int32_t PlateCodeScanerModule::plate_code_scaner_read_result(int32_t packetIndex, uint8_t* data, int32_t* len) { - // 每次传10个点 - int32_t start = packetIndex * RAW_SECTION_SIZE; - int32_t numpoint = RAW_SECTION_SIZE; - if (start + numpoint > adc_capture_point_num) { - numpoint = adc_capture_point_num - start; - } - - if (numpoint <= 0) { - *len = 0; - return 0; - } - - memcpy(data, adc_capture_buf + start, numpoint * 2); - *len = numpoint * 2; - - return 0; -} -int32_t PlateCodeScanerModule::plate_code_scaner_read_result_point_num(int32_t* packetNum) { - *packetNum = adc_capture_point_num; - return 0; -} - -int32_t PlateCodeScanerModule::plate_code_scaner_adc_readraw(int32_t* val) { - *val = read_adc_val(); - return 0; -} -int32_t PlateCodeScanerModule::plate_code_scaner_read_code(int32_t* rawcode, int32_t* legal, int32_t* item, int32_t* lot) { - *item = m_reg.item; - *lot = m_reg.lot; - *rawcode = m_reg.rawcode; - *legal = m_reg.code_legal; - return 0; -} - -int32_t PlateCodeScanerModule::plate_code_scaner_open_laser() { - scaner_set_gain(m_reg.scan_gain / 10.0); - laser_set_intensity(m_reg.laster_intensity); - m_t_laster_enable_io.setState(true); - m_amp_negative_5v_gpio.setState(true); - m_t_amp_sw_io.setState(true); - m_f_amp_sw_io.setState(true); - m_lasterIsOn = true; - return 0; -} - -void PlateCodeScanerModule::scaner_set_gain(float gain) { - int32_t potentiometer_val = 0; - potentiometer_val = t_detector_gain_to_potentiometer_val(gain); - ZLOGI(TAG, "scaner_set_gain: %.2f(potentiometer_val: %d)", gain, potentiometer_val); - m_t_optical_scaner_mcp41_ohm.setPotentiometerValue_0(potentiometer_val); -} -void PlateCodeScanerModule::laser_set_intensity(float intensity) { - int32_t potentiometer_val = 0; - potentiometer_val = intensity / 100.0 * 150.0; - ZLOGI(TAG, "laser_set_intensity: %.2f(potentiometer_val: %d)", intensity, potentiometer_val); - m_t_optical_amp_mcp41_ohm.setPotentiometerValue_0(potentiometer_val); -} - -int32_t PlateCodeScanerModule::plate_code_scaner_close_laser() { - m_t_laster_enable_io.setState(false); - m_lasterIsOn = false; - return 0; -} - -int32_t PlateCodeScanerModule::read_adc_val() { - int32_t val = 0; -#define CAPTURE_NUM 3 - - int32_t cache[CAPTURE_NUM] = {0}; - for (int i = 0; i < CAPTURE_NUM; i++) { - int32_t e = t_result_adc.get_adc_value(cache[i]); - // ZLOGI(TAG, "read adc: %d", cache[i]); - if (e != 0) { - ZLOGE(TAG, "read adc fail"); - } - } - - // 先排序，取中间10个数的中值 - for (int i = 0; i < CAPTURE_NUM; i++) { - for (int j = i + 1; j < CAPTURE_NUM; j++) { - if (cache[i] > cache[j]) { - int temp = cache[i]; - cache[i] = cache[j]; - cache[j] = temp; - } - } - } - - for (int i = 1; i < CAPTURE_NUM - 1; i++) { - val += cache[i]; - } - - return val / (CAPTURE_NUM - 2); -} diff --git a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp.bak b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp.bak new file mode 100644 index 0000000..01615b4 --- /dev/null +++ b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.cpp.bak @@ -0,0 +1,439 @@ +#include "plate_code_scaner_module.hpp" + +#include +#include +#include + +using namespace iflytop; +using namespace std; +#define RAW_SECTION_SIZE 10 + +#define ONE_BIT_WIDTH 12 // 12*.0.1mm +#define BIT_NUM 15 +#define MAX_POINT (12 * 15 + 2 * 12 + 1) // 最多12*15+2个点,多扫描的两个点用于对齐使用 + +#define TAG "PlateCodeScaner" +#define LOGI(fmt, ...) ZLOGI(TAG, fmt, ##__VA_ARGS__) + +#define DO(func) \ + { \ + int32_t ecode = func; \ + if (ecode != 0) { \ + ZLOGE(TAG, "do %s fail, error %s(%d)", #func, err::error2str(ecode), ecode); \ + return ecode; \ + } \ + } + +#define DO_IN_THREAD(func) \ + { \ + int32_t ecode = func; \ + if (ecode != 0) { \ + ZLOGE(TAG, "do %s fail, error %s(%d)", #func, err::error2str(ecode), ecode); \ + module_errorcode = ecode; \ + return; \ + } \ + } + +#define BITVAL(code, off) ((code & (1 << off)) >> off) +static void decode(uint32_t rawcode, int32_t* pitem, int32_t* plot) { // + // Lot:3:6 + // bit3->bit3, bit4->bit2, bit5->bit1, bit6->bit0 + uint32_t lot = 0; + lot = BITVAL(rawcode, 3) << 3 | BITVAL(rawcode, 4) << 2 | BITVAL(rawcode, 5) << 1 | BITVAL(rawcode, 6) << 0; + + // =(bit1)*2^6+(bit2)*2^5+(bit11)*2^4+(bit10)*2^0+(bit9)*2^1+(bit8)*2^2+(bit7)*2^3 + uint32_t item = 0; + item = (BITVAL(rawcode, 1) << 6) // + | (BITVAL(rawcode, 2) << 5) // + | (BITVAL(rawcode, 11) << 4) // + | (BITVAL(rawcode, 10) << 0) // + | (BITVAL(rawcode, 9) << 1) // + | (BITVAL(rawcode, 8) << 2) // + | (BITVAL(rawcode, 7) << 3); + + ZLOGI(TAG, "item-lot: %d-%d\n", item, lot); + *pitem = item; + *plot = lot; +} + +static int32_t t_detector_gain_to_potentiometer_val(float scan_gain) { + int32_t potentiometer_val = 0; + potentiometer_val = (scan_gain * 4.7 - 2.4) * 256. / 100. + 0.5; + if (potentiometer_val < 1) potentiometer_val = 1; + if (potentiometer_val > 255) potentiometer_val = 255; + return potentiometer_val; +} + +void PlateCodeScanerModule::initialize(int32_t moduleid, hardware_config_t* hardwarecfg) { // + module_id = moduleid; + m_hardwarecfg = *hardwarecfg; + + m_t_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_amp_mcp41); + m_t_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.t_optical_scaner_mcp41); + m_f_optical_amp_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_amp_mcp41); + m_f_optical_scaner_mcp41_ohm.initialize(&m_hardwarecfg.f_optical_scaner_mcp41); + + m_t_laster_enable_io.initAsOutput(&m_hardwarecfg.t_laster_enable_io); + m_f_opt_laster_enable_io.initAsOutput(&m_hardwarecfg.f_opt_laster_enable_io); + m_channel_select_io_red_led3.initAsOutput(&m_hardwarecfg.channel_select_io_red_led3); + m_channel_select_io_blue_led1.initAsOutput(&m_hardwarecfg.channel_select_io_blue_led1); + m_channel_select_io_green_led1.initAsOutput(&m_hardwarecfg.channel_select_io_green_led1); + m_amp_negative_5v_gpio.initAsOutput(&m_hardwarecfg.amp_negative_5v_gpio); + + m_t_amp_sw_io.initAsOutput(&m_hardwarecfg.t_amp_sw_io); + m_f_amp_sw_io.initAsOutput(&m_hardwarecfg.f_amp_sw_io); + + t_laster_adc.initialize(&m_hardwarecfg.t_laster_adc); + t_result_adc.initialize(&m_hardwarecfg.t_result_adc); + f_laster_adc.initialize(&m_hardwarecfg.f_laster_adc); + f_result_adc.initialize(&m_hardwarecfg.f_result_adc); + + m_motor = (StepMotorCtrlModule*)m_hardwarecfg.motor; + ZASSERT(m_motor); + + adc_capture_buf = (int16_t*)calloc(2, MAX_POINT); + ZASSERT(adc_capture_buf); + + m_t_laster_enable_io.setState(false); + m_amp_negative_5v_gpio.setState(false); + m_t_amp_sw_io.setState(false); + m_f_amp_sw_io.setState(false); + + // while (true) { + // int32_t adcv; + // t_result_adc.get_adc_value(adcv); + // ZLOGI(TAG, "t:%d", adcv); + // osDelay(100); + // } + + m_reg.laster_intensity = 99; + m_reg.scan_gain = 1; + m_reg.scan_velocity = 50; + m_reg.scan_start_pos = 960; + m_reg.code_judgment_threshold = 1000; + m_reg.item = 0; + m_reg.lot = 0; + m_reg.rawcode = 0; + m_reg.code_legal = -1; + m_thread.init("PlateCodeScanerModule", 1024, osPriorityNormal); +} + +int32_t PlateCodeScanerModule::module_set_reg(int32_t regindex, int32_t val) { + int32_t ret = 0; + switch (regindex) { + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_laster_intensity, m_reg.laster_intensity); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_gain, m_reg.scan_gain); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_velocity, m_reg.scan_velocity); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_scan_start_pos, m_reg.scan_start_pos); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_code_judgment_threshold, m_reg.code_judgment_threshold); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_item, m_reg.item); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_lot, m_reg.lot); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_rawcode, m_reg.rawcode); + MODULE_REG_CASE__SET_REG_TO(kreg_plate_code_scaner_code_legal, m_reg.code_legal); + default: + return err::kmodule_not_find_reg; + break; + } + if (ret != 0) return ret; + module_active_cfg(); + return 0; +} +int32_t PlateCodeScanerModule::module_get_reg(int32_t regindex, int32_t* val) { + int32_t ret = 0; + switch (regindex) { + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_laster_intensity, m_reg.laster_intensity); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_gain, m_reg.scan_gain); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_velocity, m_reg.scan_velocity); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_scan_start_pos, m_reg.scan_start_pos); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_code_judgment_threshold, m_reg.code_judgment_threshold); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_item, m_reg.item); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_lot, m_reg.lot); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_rawcode, m_reg.rawcode); + MODULE_CFG_CASE__GET_REG_FROM(kreg_plate_code_scaner_code_legal, m_reg.code_legal); + default: + return err::kmodule_not_find_reg; + break; + } + if (ret != 0) return ret; + return 0; +} + +int32_t PlateCodeScanerModule::module_stop() { + m_thread.stop(); + return 0; +} + +int32_t PlateCodeScanerModule::module_active_cfg() { + scaner_set_gain(m_reg.scan_gain / 10.0); + laser_set_intensity(m_reg.laster_intensity); + return 0; +} + +int32_t PlateCodeScanerModule::plate_code_scaner_push_card_and_scan(int32_t final_stop_pos) { + ZLOGI(TAG, "plate_code_scaner_push_card_and_scan: %d", final_stop_pos); + /** + * @brief + */ + m_thread.stop(); + module_status = 1; + m_thread.start( + + [this, final_stop_pos]() { // + DO_IN_THREAD(before_run()); + ZLOGI(TAG, "plate_code_scaner_open_laser"); + plate_code_scaner_open_laser(); + + /*********************************************************************************************************************** + * 移动电机到扫描启动位置 * + ***********************************************************************************************************************/ + ZLOGI(TAG, "step_motor_easy_move_to start pos: %d", m_reg.scan_start_pos); + m_motor->step_motor_easy_move_to(m_reg.scan_start_pos); + while (true) { + if (!check_when_run() || m_thread.getExitFlag()) break; + + int32_t status = 0; + m_motor->module_get_status(&status); + if (status == 0) break; // 电机停止 + if (status == 2) return; // 电机异常 + osDelay(5); + } + ZLOGI(TAG, "has moved to start pos"); + /*********************************************************************************************************************** + * 开始扫码 * + ***********************************************************************************************************************/ + ZLOGI(TAG, "start scan"); + + m_reg.item = -1; + m_reg.lot = -1; + m_reg.rawcode = 0; + m_reg.code_legal = -1; + + auto _motor = m_motor->getMotor(); + int32_t targetpos = 0; + targetpos = m_reg.scan_start_pos + ONE_BIT_WIDTH * BIT_NUM; + ZLOGI(TAG, "move to: %d", targetpos); + _motor->moveTo(targetpos, m_reg.scan_velocity); + + reset_capture_buf(); + // chip_critical_enter(); + while (true) { + if (m_thread.getExitFlag()) break; + if (_motor->isReachTarget()) break; + + // 读取ADC + int32_t pos = _motor->getXACTUAL(); + int16_t adcv = read_adc_val(); + int32_t dpos = abs(pos - m_reg.scan_start_pos); + push_one_point(dpos, adcv); + // osDelay(1); + } + // chip_critical_exit(); + + /*********************************************************************************************************************** + * 移动到扫码结束位置 * + ***********************************************************************************************************************/ + + ZLOGI(TAG, "step_motor_easy_move_to end pos: %d", final_stop_pos); + m_motor->step_motor_easy_move_to(final_stop_pos); + while (true) { + if (!check_when_run() || m_thread.getExitFlag()) break; + + int32_t status = 0; + m_motor->module_get_status(&status); + if (status == 0) break; // 电机停止 + if (status == 2) return; // 电机异常 + osDelay(5); + } + ZLOGI(TAG, "has moved to start pos"); + dumppoint(); + parsepoint(); + + }, + [this]() { // exit fn + after_run(); + m_motor->step_motor_stop(0); + plate_code_scaner_close_laser(); + } // + ); + + return 0; +} + +int32_t PlateCodeScanerModule::plate_code_scaner_stop_scan() { + ZLOGI(TAG, "plate_code_scaner_stop_scan"); + m_thread.stop(); + return 0; +} + +void PlateCodeScanerModule::push_one_point(int32_t pos, int16_t pointval) { + if (pos >= MAX_POINT) { + return; + } + if (pos < 0) { + return; + } + + for (int32_t i = adc_capture_last_point_index + 1; i < pos; i++) { + adc_capture_buf[i] = adc_capture_buf[adc_capture_last_point_index]; + } + + adc_capture_buf[pos] = pointval; + adc_capture_point_num = pos + 1; + adc_capture_last_point_index = pos; +} +void PlateCodeScanerModule::reset_capture_buf() { + adc_capture_point_num = 0; + adc_capture_last_point_index = 0; + for (int32_t i = 0; i < MAX_POINT; i++) { + adc_capture_buf[i] = 0; + } +} + +void PlateCodeScanerModule::dumppoint() { + ZLOGI(TAG, " ===== capture points: %d ==== ", adc_capture_point_num); + for (int32_t i = 0; i < adc_capture_point_num; i++) { + ZLOGI(TAG, "%5d %5d", i, adc_capture_buf[i]); + } +} +void PlateCodeScanerModule::parsepoint() { + // + uint8_t code[15]; + for (int32_t i = 0; i < 15; i++) { + int pos = i * 12 + 6; + // 黑色数值更小，黑色1，白色0 + if (adc_capture_buf[i] < m_reg.code_judgment_threshold) { + code[i] = 1; + } else { + code[i] = 0; + } + } + + uint32_t rawcode = 0; + + for (int32_t i = 0; i < 15; i++) { + rawcode |= code[i] << i; + } + + int32_t item = 0; + int32_t lot = 0; + decode(rawcode, &item, &lot); + + m_reg.item = item; + m_reg.lot = lot; + m_reg.rawcode = rawcode; + + if (code[14] == 1 && code[13] == 0 && code[12] == 0) { + m_reg.code_legal = 1; + } else { + m_reg.code_legal = 0; + } + + ZLOGI(TAG, "rawcode:%x code: %d-%d (legal:%d)", rawcode, m_reg.item, m_reg.lot, m_reg.code_legal); +} + +int32_t PlateCodeScanerModule::before_run() { return 0; } +int32_t PlateCodeScanerModule::after_run() { + if (module_errorcode != 0) { + module_status = 2; + } else { + module_status = 0; + } + return 0; +} +bool PlateCodeScanerModule::check_when_run() { return true; } + +int32_t PlateCodeScanerModule::plate_code_scaner_read_result(int32_t packetIndex, uint8_t* data, int32_t* len) { + // 每次传10个点 + int32_t start = packetIndex * RAW_SECTION_SIZE; + int32_t numpoint = RAW_SECTION_SIZE; + if (start + numpoint > adc_capture_point_num) { + numpoint = adc_capture_point_num - start; + } + + if (numpoint <= 0) { + *len = 0; + return 0; + } + + memcpy(data, adc_capture_buf + start, numpoint * 2); + *len = numpoint * 2; + + return 0; +} +int32_t PlateCodeScanerModule::plate_code_scaner_read_result_point_num(int32_t* packetNum) { + *packetNum = adc_capture_point_num; + return 0; +} + +int32_t PlateCodeScanerModule::plate_code_scaner_adc_readraw(int32_t* val) { + *val = read_adc_val(); + return 0; +} +int32_t PlateCodeScanerModule::plate_code_scaner_read_code(int32_t* rawcode, int32_t* legal, int32_t* item, int32_t* lot) { + *item = m_reg.item; + *lot = m_reg.lot; + *rawcode = m_reg.rawcode; + *legal = m_reg.code_legal; + return 0; +} + +int32_t PlateCodeScanerModule::plate_code_scaner_open_laser() { + scaner_set_gain(m_reg.scan_gain / 10.0); + laser_set_intensity(m_reg.laster_intensity); + m_t_laster_enable_io.setState(true); + m_amp_negative_5v_gpio.setState(true); + m_t_amp_sw_io.setState(true); + m_f_amp_sw_io.setState(true); + m_lasterIsOn = true; + return 0; +} + +void PlateCodeScanerModule::scaner_set_gain(float gain) { + int32_t potentiometer_val = 0; + potentiometer_val = t_detector_gain_to_potentiometer_val(gain); + ZLOGI(TAG, "scaner_set_gain: %.2f(potentiometer_val: %d)", gain, potentiometer_val); + m_t_optical_scaner_mcp41_ohm.setPotentiometerValue_0(potentiometer_val); +} +void PlateCodeScanerModule::laser_set_intensity(float intensity) { + int32_t potentiometer_val = 0; + potentiometer_val = intensity / 100.0 * 150.0; + ZLOGI(TAG, "laser_set_intensity: %.2f(potentiometer_val: %d)", intensity, potentiometer_val); + m_t_optical_amp_mcp41_ohm.setPotentiometerValue_0(potentiometer_val); +} + +int32_t PlateCodeScanerModule::plate_code_scaner_close_laser() { + m_t_laster_enable_io.setState(false); + m_lasterIsOn = false; + return 0; +} + +int32_t PlateCodeScanerModule::read_adc_val() { + int32_t val = 0; +#define CAPTURE_NUM 3 + + int32_t cache[CAPTURE_NUM] = {0}; + for (int i = 0; i < CAPTURE_NUM; i++) { + int32_t e = t_result_adc.get_adc_value(cache[i]); + // ZLOGI(TAG, "read adc: %d", cache[i]); + if (e != 0) { + ZLOGE(TAG, "read adc fail"); + } + } + + // 先排序，取中间10个数的中值 + for (int i = 0; i < CAPTURE_NUM; i++) { + for (int j = i + 1; j < CAPTURE_NUM; j++) { + if (cache[i] > cache[j]) { + int temp = cache[i]; + cache[i] = cache[j]; + cache[j] = temp; + } + } + } + + for (int i = 1; i < CAPTURE_NUM - 1; i++) { + val += cache[i]; + } + + return val / (CAPTURE_NUM - 2); +} diff --git a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp deleted file mode 100644 index 1709c00..0000000 --- a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp +++ /dev/null @@ -1,142 +0,0 @@ -#pragma once -#include "sdk\components\sensors\mcp41xxx\mcp41xxx.hpp" -#include "sdk\os\zos.hpp" -// -#include "a8000_protocol\protocol.hpp" -#include "sdk\components\api\zi_module.hpp" -#include "sdk\components\hardware\adc\z_simple_adc.hpp" -#include "sdk\components\step_motor_ctrl_module\step_motor_ctrl_module.hpp" - -namespace iflytop { -using namespace std; - -class PlateCodeScanerModule : public ZIModule { - public: - PlateCodeScanerModule() : ZIModule(ka8000_plate_code_scaner, APP_VERSION) {} - - public: - typedef struct { - ZGPIO::OutputGpioCfg_t amp_negative_5v_gpio; - - ZADC::adc_config_t t_laster_adc; - ZADC::adc_config_t t_result_adc; - MCP41XXX::hardware_config_t t_optical_amp_mcp41; - MCP41XXX::hardware_config_t t_optical_scaner_mcp41; - ZGPIO::OutputGpioCfg_t t_laster_enable_io; - ZGPIO::OutputGpioCfg_t t_amp_sw_io; - - MCP41XXX::hardware_config_t f_optical_amp_mcp41; - MCP41XXX::hardware_config_t f_optical_scaner_mcp41; - ZADC::adc_config_t f_laster_adc; - ZADC::adc_config_t f_result_adc; - ZGPIO::OutputGpioCfg_t f_opt_laster_enable_io; - ZGPIO::OutputGpioCfg_t f_amp_sw_io; - - ZGPIO::OutputGpioCfg_t channel_select_io_red_led3; // 引脚命名来源于巴迪泰原理图 - ZGPIO::OutputGpioCfg_t channel_select_io_blue_led1; // 引脚命名来源于巴迪泰原理图 - ZGPIO::OutputGpioCfg_t channel_select_io_green_led1; // 引脚命名来源于巴迪泰原理图 - - StepMotorCtrlModule* motor; - - } hardware_config_t; - - typedef struct { - int32_t laster_intensity; // 0->100 - int32_t scan_gain; // 1 = 0.1倍增益 - int32_t scan_velocity; // 扫描速度 - int32_t scan_start_pos; // 扫描开始位置 - int32_t code_judgment_threshold; // 编码解析时，判断当前位置的编码是0还是1的阈值 - int32_t item; // - int32_t lot; // - int32_t rawcode; // - int32_t code_legal; // - } reg_table_t; - - private: - int32_t m_id = 0; - hardware_config_t m_hardwarecfg; - reg_table_t m_reg = {0}; - - MCP41XXX m_t_optical_amp_mcp41_ohm; - MCP41XXX m_t_optical_scaner_mcp41_ohm; - MCP41XXX m_f_optical_amp_mcp41_ohm; - MCP41XXX m_f_optical_scaner_mcp41_ohm; - // amp_negative_5v - ZGPIO m_amp_negative_5v_gpio; - - ZGPIO m_t_laster_enable_io; - ZGPIO m_f_opt_laster_enable_io; - ZGPIO m_channel_select_io_red_led3; - ZGPIO m_channel_select_io_blue_led1; - ZGPIO m_channel_select_io_green_led1; - - ZGPIO m_t_amp_sw_io; - ZGPIO m_f_amp_sw_io; - - ZADC t_laster_adc; - ZADC t_result_adc; - ZADC f_laster_adc; - ZADC f_result_adc; - - StepMotorCtrlModule* m_motor = nullptr; - - ZThread m_thread; - - int16_t* adc_capture_buf = nullptr; - int32_t adc_capture_point_num = 0; - int32_t adc_capture_last_point_index = 0; - - int32_t f_adc_cache[20]; - - bool m_lasterIsOn = false; - - public: - virtual ~PlateCodeScanerModule() {}; - - void initialize(int32_t moduleid, hardware_config_t* hardwarecfg); - - public: - /*********************************************************************************************************************** - * 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 { return 0; }; - virtual int32_t module_stop() override; - - // virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override; - // virtual int32_t module_reset_reg() override { return 0; }; - // virtual int32_t module_stop() override; - virtual int32_t module_active_cfg(); - - /*********************************************************************************************************************** - * PlateCodeScanerModule * - ***********************************************************************************************************************/ - virtual int32_t plate_code_scaner_push_card_and_scan(int32_t final_stop_pos); - virtual int32_t plate_code_scaner_stop_scan(); - virtual int32_t plate_code_scaner_read_result(int32_t packetIndex, uint8_t* data, int32_t* len); - virtual int32_t plate_code_scaner_read_result_point_num(int32_t* packetNum); - virtual int32_t plate_code_scaner_read_code(int32_t* rawcode, int32_t* legal, int32_t* item, int32_t* lot); - - virtual int32_t plate_code_scaner_adc_readraw(int32_t* val); - virtual int32_t plate_code_scaner_open_laser(); - virtual int32_t plate_code_scaner_close_laser(); - - private: - void push_one_point(int32_t pos, int16_t pointval); - void reset_capture_buf(); - void dumppoint(); - void parsepoint(); - - - private: - int32_t before_run(); - int32_t after_run(); - bool check_when_run(); - - int32_t read_adc_val(); - void scaner_set_gain(float gain); - void laser_set_intensity(float intensity); -}; -} // namespace iflytop diff --git a/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp.bak b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp.bak new file mode 100644 index 0000000..1709c00 --- /dev/null +++ b/usrc/subboards/subboard20_plate_clamp_case/plate_code_scaner_module.hpp.bak @@ -0,0 +1,142 @@ +#pragma once +#include "sdk\components\sensors\mcp41xxx\mcp41xxx.hpp" +#include "sdk\os\zos.hpp" +// +#include "a8000_protocol\protocol.hpp" +#include "sdk\components\api\zi_module.hpp" +#include "sdk\components\hardware\adc\z_simple_adc.hpp" +#include "sdk\components\step_motor_ctrl_module\step_motor_ctrl_module.hpp" + +namespace iflytop { +using namespace std; + +class PlateCodeScanerModule : public ZIModule { + public: + PlateCodeScanerModule() : ZIModule(ka8000_plate_code_scaner, APP_VERSION) {} + + public: + typedef struct { + ZGPIO::OutputGpioCfg_t amp_negative_5v_gpio; + + ZADC::adc_config_t t_laster_adc; + ZADC::adc_config_t t_result_adc; + MCP41XXX::hardware_config_t t_optical_amp_mcp41; + MCP41XXX::hardware_config_t t_optical_scaner_mcp41; + ZGPIO::OutputGpioCfg_t t_laster_enable_io; + ZGPIO::OutputGpioCfg_t t_amp_sw_io; + + MCP41XXX::hardware_config_t f_optical_amp_mcp41; + MCP41XXX::hardware_config_t f_optical_scaner_mcp41; + ZADC::adc_config_t f_laster_adc; + ZADC::adc_config_t f_result_adc; + ZGPIO::OutputGpioCfg_t f_opt_laster_enable_io; + ZGPIO::OutputGpioCfg_t f_amp_sw_io; + + ZGPIO::OutputGpioCfg_t channel_select_io_red_led3; // 引脚命名来源于巴迪泰原理图 + ZGPIO::OutputGpioCfg_t channel_select_io_blue_led1; // 引脚命名来源于巴迪泰原理图 + ZGPIO::OutputGpioCfg_t channel_select_io_green_led1; // 引脚命名来源于巴迪泰原理图 + + StepMotorCtrlModule* motor; + + } hardware_config_t; + + typedef struct { + int32_t laster_intensity; // 0->100 + int32_t scan_gain; // 1 = 0.1倍增益 + int32_t scan_velocity; // 扫描速度 + int32_t scan_start_pos; // 扫描开始位置 + int32_t code_judgment_threshold; // 编码解析时，判断当前位置的编码是0还是1的阈值 + int32_t item; // + int32_t lot; // + int32_t rawcode; // + int32_t code_legal; // + } reg_table_t; + + private: + int32_t m_id = 0; + hardware_config_t m_hardwarecfg; + reg_table_t m_reg = {0}; + + MCP41XXX m_t_optical_amp_mcp41_ohm; + MCP41XXX m_t_optical_scaner_mcp41_ohm; + MCP41XXX m_f_optical_amp_mcp41_ohm; + MCP41XXX m_f_optical_scaner_mcp41_ohm; + // amp_negative_5v + ZGPIO m_amp_negative_5v_gpio; + + ZGPIO m_t_laster_enable_io; + ZGPIO m_f_opt_laster_enable_io; + ZGPIO m_channel_select_io_red_led3; + ZGPIO m_channel_select_io_blue_led1; + ZGPIO m_channel_select_io_green_led1; + + ZGPIO m_t_amp_sw_io; + ZGPIO m_f_amp_sw_io; + + ZADC t_laster_adc; + ZADC t_result_adc; + ZADC f_laster_adc; + ZADC f_result_adc; + + StepMotorCtrlModule* m_motor = nullptr; + + ZThread m_thread; + + int16_t* adc_capture_buf = nullptr; + int32_t adc_capture_point_num = 0; + int32_t adc_capture_last_point_index = 0; + + int32_t f_adc_cache[20]; + + bool m_lasterIsOn = false; + + public: + virtual ~PlateCodeScanerModule() {}; + + void initialize(int32_t moduleid, hardware_config_t* hardwarecfg); + + public: + /*********************************************************************************************************************** + * 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 { return 0; }; + virtual int32_t module_stop() override; + + // virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override; + // virtual int32_t module_reset_reg() override { return 0; }; + // virtual int32_t module_stop() override; + virtual int32_t module_active_cfg(); + + /*********************************************************************************************************************** + * PlateCodeScanerModule * + ***********************************************************************************************************************/ + virtual int32_t plate_code_scaner_push_card_and_scan(int32_t final_stop_pos); + virtual int32_t plate_code_scaner_stop_scan(); + virtual int32_t plate_code_scaner_read_result(int32_t packetIndex, uint8_t* data, int32_t* len); + virtual int32_t plate_code_scaner_read_result_point_num(int32_t* packetNum); + virtual int32_t plate_code_scaner_read_code(int32_t* rawcode, int32_t* legal, int32_t* item, int32_t* lot); + + virtual int32_t plate_code_scaner_adc_readraw(int32_t* val); + virtual int32_t plate_code_scaner_open_laser(); + virtual int32_t plate_code_scaner_close_laser(); + + private: + void push_one_point(int32_t pos, int16_t pointval); + void reset_capture_buf(); + void dumppoint(); + void parsepoint(); + + + private: + int32_t before_run(); + int32_t after_run(); + bool check_when_run(); + + int32_t read_adc_val(); + void scaner_set_gain(float gain); + void laser_set_intensity(float intensity); +}; +} // namespace iflytop diff --git a/usrc/subboards/subboard20_plate_clamp_case/subboard20_plate_clamp_case.cpp b/usrc/subboards/subboard20_plate_clamp_case/subboard20_plate_clamp_case.cpp index c25a9d9..70da555 100644 --- a/usrc/subboards/subboard20_plate_clamp_case/subboard20_plate_clamp_case.cpp +++ b/usrc/subboards/subboard20_plate_clamp_case/subboard20_plate_clamp_case.cpp @@ -2,7 +2,7 @@ extern "C" { #include "subboard20_plate_clamp_case_board.h" } -#include "plate_code_scaner_module.hpp" +// #include "plate_code_scaner_module.hpp" #include "pri_board.h" #include "public_service/instance_init.hpp" #include "public_service/public_service.hpp" @@ -10,7 +10,6 @@ extern "C" { #include "sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp" #include "sdk/components/sensors/m3078/m3078_code_scaner.hpp" #include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp" - #define TAG "Subboard20PlateClampCase" @@ -20,7 +19,7 @@ using namespace iflytop; * IMPL * ***********************************************************************************************************************/ void Subboard20PlateClampCase::initialize() { - module_id = getmoduleId(0); + module_id = getmoduleId(0); IO_INIT(); GService::inst()->getZCanProtocolParser()->registerModule(this); @@ -32,8 +31,14 @@ void Subboard20PlateClampCase::initialize() { /*********************************************************************************************************************** * ID1 * ***********************************************************************************************************************/ - { TMC5130_MOTOR_INITER(1, 1); } - { TMC5130_MOTOR_INITER(2, 2); } + { + TMC5130_MOTOR_INITER(1, 1); + } + { + TMC5130_MOTOR_INITER(2, 2); + } +#endif +#if 0 push_rod_motor = // dynamic_cast(GService::inst()->getZCanProtocolParser()->getModule(getmoduleId(2))); diff --git a/usrc/subboards/subboard90_optical_module/optical_module_v2.cpp b/usrc/subboards/subboard90_optical_module/optical_module_v2.cpp index 97e8a67..435bd27 100644 --- a/usrc/subboards/subboard90_optical_module/optical_module_v2.cpp +++ b/usrc/subboards/subboard90_optical_module/optical_module_v2.cpp @@ -704,35 +704,43 @@ void OpticalModuleV2::active_motor_default_cfg() { motor->step_motor_active_cfg( void OpticalModuleV2::recover_motor_default_cfg() { active_motor_default_cfg(); } void OpticalModuleV2::active_motor_opt_cfg(optical_type_t optType) { - auto _motor = motor->getMotor(); + auto _motor = motor->getMotor(); + TMC51X0::VCfg_t vcfg; + if (optType == kf_optical) { - _motor->set_vstart(m_reg.f_scan_vstart); - _motor->set_vstop(m_reg.f_scan_vstop); - _motor->set_a1(m_reg.f_scan_a1); - _motor->set_d1(m_reg.f_scan_d1); - _motor->set_v1(m_reg.f_scan_v1); - _motor->set_amax(m_reg.f_scan_amax); - _motor->set_dmax(m_reg.f_scan_dmax); + vcfg.vstart = m_reg.f_scan_vstart; + vcfg.vstop = m_reg.f_scan_vstop; + vcfg.a1 = m_reg.f_scan_a1; + vcfg.d1 = m_reg.f_scan_d1; + vcfg.v1 = m_reg.f_scan_v1; + vcfg.amax = m_reg.f_scan_amax; + vcfg.dmax = m_reg.f_scan_dmax; + vcfg.vmax = m_reg.f_scan_vdefault; // 这里的vmax是默认速度 + _motor->set_vcfg(&vcfg); _motor->set_tzerowait(m_reg.f_scan_tzerowait); _motor->setIHOLD_IRUN(m_reg.f_scan_irun, m_reg.f_scan_irun, 1000); } else if (optType == kt_optical) { - _motor->set_vstart(m_reg.t_scan_vstart); - _motor->set_vstop(m_reg.t_scan_vstop); - _motor->set_a1(m_reg.t_scan_a1); - _motor->set_d1(m_reg.t_scan_d1); - _motor->set_v1(m_reg.t_scan_v1); - _motor->set_amax(m_reg.t_scan_amax); - _motor->set_dmax(m_reg.t_scan_dmax); + vcfg.vstart = m_reg.t_scan_vstart; + vcfg.vstop = m_reg.t_scan_vstop; + vcfg.a1 = m_reg.t_scan_a1; + vcfg.d1 = m_reg.t_scan_d1; + vcfg.v1 = m_reg.t_scan_v1; + vcfg.amax = m_reg.t_scan_amax; + vcfg.dmax = m_reg.t_scan_dmax; + vcfg.vmax = m_reg.t_scan_vdefault; // 这里的vmax是默认速度 + _motor->set_vcfg(&vcfg); _motor->set_tzerowait(m_reg.t_scan_tzerowait); _motor->setIHOLD_IRUN(m_reg.t_scan_irun, m_reg.t_scan_irun, 1000); } else if (optType == kbarcode_optical) { - _motor->set_vstart(m_reg.barcode_scan_vstart); - _motor->set_vstop(m_reg.barcode_scan_vstop); - _motor->set_a1(m_reg.barcode_scan_a1); - _motor->set_d1(m_reg.barcode_scan_d1); - _motor->set_v1(m_reg.barcode_scan_v1); - _motor->set_amax(m_reg.barcode_scan_amax); - _motor->set_dmax(m_reg.barcode_scan_dmax); + vcfg.vstart = m_reg.barcode_scan_vstart; + vcfg.vstop = m_reg.barcode_scan_vstop; + vcfg.a1 = m_reg.barcode_scan_a1; + vcfg.d1 = m_reg.barcode_scan_d1; + vcfg.v1 = m_reg.barcode_scan_v1; + vcfg.amax = m_reg.barcode_scan_amax; + vcfg.dmax = m_reg.barcode_scan_dmax; + vcfg.vmax = m_reg.barcode_scan_vdefault; // 这里的vmax是默认速度 + _motor->set_vcfg(&vcfg); _motor->set_tzerowait(m_reg.barcode_scan_tzerowait); _motor->setIHOLD_IRUN(m_reg.barcode_scan_irun, m_reg.barcode_scan_irun, 1000); } else { @@ -742,15 +750,7 @@ void OpticalModuleV2::active_motor_opt_cfg(optical_type_t optType) { int32_t OpticalModuleV2::move_to_block(optical_type_t optType, int32_t pos) { auto _motor = motor->getMotor(); - if (optType == kf_optical) { - _motor->moveTo(pos, m_reg.f_scan_vdefault); - } else if (optType == kt_optical) { - _motor->moveTo(pos, m_reg.t_scan_vdefault); - } else if (optType == kbarcode_optical) { - _motor->moveTo(pos, m_reg.barcode_scan_vdefault); - } else { - _motor->moveTo(pos, motor->getCfg()->motor_default_velocity); - } + _motor->moveTo(pos); while (!_motor->isReachTarget()) { } return 0;