diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index df53d92..9c9a299 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -33,7 +33,7 @@ int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime; return 0; } -int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, function status_cb) { +int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) { ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, torque, run_time); m_thread.stop(); if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; @@ -44,18 +44,31 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, functio m_thread.start([this, speed, torque, run_time, status_cb]() { int32_t entertime = zos_get_tick(); - while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) { + while (true) { + if (m_thread.getExitFlag() && run_time != 0) { + stop(0); + call_status_cb(status_cb, err::kcommon_error_break_by_user); + break; + } + + if (m_thread.getExitFlag() && run_time == 0) { + stop(0); + call_status_cb(status_cb, 0); + break; + } + + if (run_time != 0 && zos_haspassedms(entertime) > run_time) { + stop(0); + call_status_cb(status_cb, 0); + break; + } m_thread.sleep(10); } - stop(0); - rotate_cb_status_t status; - status.has_run_time = zos_haspassedms(entertime); - if (status_cb) status_cb(status); }); return 0; } -int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function status_cb) { +int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { ZLOGI(TAG, "move_to pos:%d speed:%d torque:%d", pos, speed, torque); m_thread.stop(); @@ -70,22 +83,27 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, functionmoveTo(m_id, pos, speed, torque)) return err::kce_subdevice_overtime; m_thread.start([this, pos, speed, torque, status_cb]() { - while (!m_thread.getExitFlag()) { + while (true) { uint8_t moveflag = 0; m_bus->getMoveFlag(m_id, moveflag); - if (moveflag == 0) break; + if (moveflag == 0) { + stop(0); + call_status_cb(status_cb, 0); + break; + } + + if (m_thread.getExitFlag()) { + stop(0); + call_status_cb(status_cb, err::kcommon_error_break_by_user); + break; + } m_thread.sleep(10); } - stop(0); - move_to_cb_status_t status; - status.pos = pos; - status.exec_status = 0; - if (status_cb) status_cb(status); }); return 0; } -int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, function status_cb) { +int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_status_t status_cb) { ZLOGI(TAG, "move_by pos:%d speed:%d torque:%d", dpos, speed, torque); m_thread.stop(); @@ -100,16 +118,16 @@ int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, function 4095) targetpos = 4095; if (targetpos < 0) targetpos = 0; - move_to(targetpos, speed, torque, [this, nowpos, status_cb](move_to_cb_status_t& status) { - move_by_cb_status_t moveby_status = {0}; - moveby_status.dpos = status.pos - nowpos; - moveby_status.exec_status = status.exec_status; - if (status_cb) status_cb(moveby_status); + move_to(targetpos, speed, torque, [this, nowpos, status_cb](int32_t status) { + // move_by_cb_status_t moveby_status = {0}; + // moveby_status.dpos = status.pos - nowpos; + // moveby_status.exec_status = status.exec_status; + if (status_cb) status_cb(status); }); return 0; } -int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function status_cb) { +int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) { ZLOGI(TAG, "run_with_torque torque:%d run_time:%d", torque, run_time); m_thread.stop(); @@ -121,19 +139,33 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function< m_thread.start([this, torque, run_time, status_cb]() { int32_t entertime = zos_get_tick(); - while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) { + // while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) { + while (true) { + if (m_thread.getExitFlag() && run_time != 0) { + stop(0); + call_status_cb(status_cb, err::kcommon_error_break_by_user); + break; + } + if (m_thread.getExitFlag() && run_time == 0) { + stop(0); + call_status_cb(status_cb, 0); + break; + } + if (run_time != 0 && zos_haspassedms(entertime) > run_time) { + stop(0); + call_status_cb(status_cb, 0); + break; + } m_thread.sleep(10); } stop(0); - run_with_torque_cb_status_t status; - status.has_run_time = zos_haspassedms(entertime); - if (status_cb) status_cb(status); + if (status_cb) status_cb(0); }); return 0; } -int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function status_cb) { return err::kcommon_error_operation_not_support; } +int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { return err::kcommon_error_operation_not_support; } int32_t MiniRobotCtrlModule::read_version(version_t& version) { uint8_t mainversion, subversion, miniserv_mainversion, miniserv_subversion; @@ -185,3 +217,8 @@ int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& para int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; } int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; } int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; } + +void MiniRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) { + m_last_exec_status = status; + if (cb) cb(status); +} diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index 5fb3a8f..24d73e7 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -12,6 +12,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule { ZThread m_thread; s32 m_pos_shift; + s32 m_last_exec_status = 0; + public: void initialize(FeiTeServoMotor* bus, uint8_t id); @@ -19,11 +21,11 @@ class MiniRobotCtrlModule : public I_MiniServoModule { virtual int32_t stop(u8 stop_type) override; virtual int32_t position_calibrate(s32 nowpos) override; - virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, function status_cb) override; - virtual int32_t move_to(s32 pos, s32 speed, s32 torque, function status_cb) override; - virtual int32_t move_by(s32 pos, s32 speed, s32 torque, function status_cb) override; - virtual int32_t run_with_torque(s32 torque, s32 run_time, function status_cb) override; - virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, function status_cb) override; + virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) override; + virtual int32_t move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override; + virtual int32_t move_by(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override; + virtual int32_t run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) override; + virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override; virtual int32_t read_version(version_t& version) override; virtual int32_t read_status(status_t& status) override; @@ -38,5 +40,6 @@ class MiniRobotCtrlModule : public I_MiniServoModule { virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override; private: + void call_status_cb(action_cb_status_t cb, int32_t status); }; } // namespace iflytop \ No newline at end of file diff --git a/components/pipette_module/pipette_ctrl_module.cpp b/components/pipette_module/pipette_ctrl_module.cpp index e0400b1..b245d01 100644 --- a/components/pipette_module/pipette_ctrl_module.cpp +++ b/components/pipette_module/pipette_ctrl_module.cpp @@ -83,7 +83,7 @@ int32_t PipetteModule::take_tip(s16 vel, s16 height_mm, s16 tip_hight_mm, action return err::kSMTP2_TipAlreadyLoad; } - m_tip_hight_mm = tip_hight_mm; + // m_tip_hight_mm = tip_hight_mm; m_thread.stop(); m_thread.start([this, vel, height_mm, tip_hight_mm, status_cb]() { // 移动移液枪到取Tip位置 @@ -288,6 +288,9 @@ int32_t PipetteModule::take_and_split_liquid(s16 max_deep_height_ #endif int32_t PipetteModule::hight_trans(s16 targethith, s16 &motormovehight) { + motormovehight = targethith; + return 0; +#if 0 bool hastip = false; int32_t err = m_smtp2->read_tip_state(hastip); if (err != 0) { @@ -306,6 +309,22 @@ int32_t PipetteModule::hight_trans(s16 targethith, s16 &motormovehight) { motormovehight = 0; } return 0; +#endif +} + +int32_t PipetteModule::get_status(status_t &status) { +#if 0 + uint8_t status; // 设备状态 + uint8_t io_state; // IO0:z_zero_io/IO1:hastip/ + int16_t zpos_mm; // z轴位置，如果有tip,则自动加上tip高度 + int16_t pipette_ul; // 移液枪液量 +#endif + status.status = m_thread.isworking() ? 1 : 0; + status.pipette_gun_status = m_smtp2->getState(); + status.io_state = m_stepMotor->read_zero_io_state() ? 1 : 0; + status.has_tip = m_smtp2->read_tip_state(); + status.zpos_mm = m_stepMotor->read_pos(); + status.pipette_ul = m_smtp2->read_pos_ul(); } s16 PipetteModule::hight_trans(s16 targethith) { diff --git a/components/pipette_module/pipette_ctrl_module.hpp b/components/pipette_module/pipette_ctrl_module.hpp index 5ad8e68..3cf3bc8 100644 --- a/components/pipette_module/pipette_ctrl_module.hpp +++ b/components/pipette_module/pipette_ctrl_module.hpp @@ -22,18 +22,12 @@ class PipetteModule : public I_PipetteModule { base_param_t m_base_param; int16_t m_transfer_height_mm = 0; - s16 m_tip_hight_mm = 0; - public: void initialize(SMTP2 *smtp2, StepMotorCtrlModule *stepMotor); - // void set_platform_para(platformpara_t *platformpara); - - int32_t set_z_motor_para(const I_StepMotorCtrlModule::base_param_t &z_motor_param); - int32_t get_z_motor_para(I_StepMotorCtrlModule::base_param_t &z_motor_param); - int32_t set_base_param(const base_param_t &base_param); - int32_t get_base_param(base_param_t &base_param); - + /******************************************************************************* + * ACTION * + *******************************************************************************/ virtual int32_t enable(u8 enable); virtual int32_t stop(u8 stop_type); @@ -48,17 +42,22 @@ class PipetteModule : public I_PipetteModule { virtual int32_t shake_volume(s16 shake_times, s16 shake_volume, action_cb_status_t status_cb); virtual int32_t take_and_split_liquid(s16 take_volume_mm, action_cb_status_t status_cb); - int32_t exec_move_to_with_lld(s16 vel, s16 lld_cap_thr, s16 lld_max_hight_mm); - /** - * @brief 移动到指定高度 - * - * @param z_height - * @param status_cb - * @return int32_t - */ - virtual int32_t z_move_to(s16 z_height_mm, action_cb_status_t status_cb); + /******************************************************************************* + * ReadStatus * + *******************************************************************************/ + virtual int32_t get_status(status_t &status); + + /******************************************************************************* + * SETTING * + *******************************************************************************/ + virtual int32_t set_z_motor_para(const I_StepMotorCtrlModule::base_param_t &z_motor_param); + virtual int32_t get_z_motor_para(I_StepMotorCtrlModule::base_param_t &z_motor_param); + virtual int32_t set_base_param(const base_param_t &base_param); + virtual int32_t get_base_param(base_param_t &base_param); public: + int32_t exec_move_to_with_lld(s16 vel, s16 lld_cap_thr, s16 lld_max_hight_mm); + int32_t hight_trans(s16 targethith, s16 &motormovehight); s16 hight_trans(s16 targethith); }; diff --git a/components/sensors/smtp2/smtp2.cpp b/components/sensors/smtp2/smtp2.cpp index 963b3ec..670c8cc 100644 --- a/components/sensors/smtp2/smtp2.cpp +++ b/components/sensors/smtp2/smtp2.cpp @@ -97,8 +97,8 @@ using namespace iflytop; */ static int32_t Get1fromfloat(const float& val) { - float temp1 = val; - int32_t t1 = (uint32_t)(temp1 * 10) % 10; + float temp1 = val; + int32_t t1 = (uint32_t)(temp1 * 10) % 10; if (t1 > 4) { temp1 = (float)(uint32_t)(temp1 + 1); } else { @@ -169,7 +169,7 @@ int32_t SMTP2::readparaint(int32_t paramidx, int32_t& val) { } int32_t SMTP2::readpara_bool(int32_t paramidx, bool& val) { - int32_t valint = 0; + int32_t valint = 0; int32_t ret = readparaint(paramidx, valint); if (ret != 0) return ret; if (valint == 1) { @@ -184,6 +184,31 @@ int32_t SMTP2::read_capacitance_val(int32_t& capval) { return readparaint(68, ca int32_t SMTP2::read_tip_state(bool& hastip) { return readpara_bool(31, hastip); } +bool SMTP2::read_tip_state() { + bool hastip = false; + int32_t ret = read_tip_state(hastip); + if (ret != 0) { + return false; + } + return hastip; +} + +int32_t SMTP2::read_pos_ul(int32_t& ul) { + int32_t pos = 0; + int32_t ret = readparaint(3, pos); // 读取nl + if (ret != 0) { + return ret; + } + ul = pos * 1000; + return 0; +} +int32_t SMTP2::read_pos_ul() { + int32_t ul = 0; + int32_t ret = read_pos_ul(ul); + if (ret != 0) return 0; + return ul; +} + char* SMTP2::fmt(const char* fmt, ...) { va_list args; va_start(args, fmt); diff --git a/components/sensors/smtp2/smtp2.hpp b/components/sensors/smtp2/smtp2.hpp index 68cd783..e455928 100644 --- a/components/sensors/smtp2/smtp2.hpp +++ b/components/sensors/smtp2/smtp2.hpp @@ -109,6 +109,16 @@ class SMTP2 { * @return int */ int32_t read_tip_state(bool& hastip); + bool read_tip_state(); + + /** + * @brief 读取当前位置 + * + * @param ul + * @return int32_t + */ + int32_t read_pos_ul(int32_t& ul); + int32_t read_pos_ul(); /** * @brief 设置分辨率 * 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 cf63a9c..4eec01a 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -250,6 +250,7 @@ int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) { ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.run_to_zero_speed); ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.run_to_zero_dec); ZLOGI(TAG, "=================================="); + return 0; } int32_t StepMotorCtrlModule::get_base_param(base_param_t& param) { param = m_param; @@ -260,6 +261,16 @@ int32_t StepMotorCtrlModule::read_pos(int32_t& pos) { getnowpos(pos); return 0; } +int32_t StepMotorCtrlModule::read_pos() { + int32_t pos; + getnowpos(pos); + return pos; +} + +bool StepMotorCtrlModule::read_zero_io_state() { + if (m_Xgpio) return m_Xgpio->getState(); + return false; +} int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) { int32_t xnow; @@ -436,4 +447,4 @@ int32_t StepMotorCtrlModule::move_to_block(int32_t velocity, int32_t tox) { } int32_t StepMotorCtrlModule::move_by_block(int32_t velocity, int32_t dx) { return do_motor_action_block([this, velocity, dx]() { return move_by(velocity, dx, nullptr); }); -} \ No newline at end of file +} diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 0c4de01..c33b801 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -61,6 +61,8 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual int32_t get_base_param(base_param_t& param) override; virtual int32_t read_pos(int32_t& pos) override; + virtual int32_t read_pos() override; + virtual bool read_zero_io_state() override; #if 0 virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; diff --git a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp b/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp index 807ecf5..b08ca78 100644 --- a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp @@ -60,60 +60,60 @@ ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_get_warning_limit_param, CMD(u8 id; u8 opt_ END_PP(); #endif PROCESS_PACKET(kcmd_mini_servo_ctrl_rotate, m_id) { // - errorcode = m_module->rotate(cmd->speed, cmd->torque, cmd->run_time, [this, cmdheader](I_MiniServoModule::rotate_cb_status_t& status) { + errorcode = m_module->rotate(cmd->speed, cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_mini_servo_ctrl_rotate_report_t report = {0}; - ZLOGI(TAG, "report kcmd_mini_servo_ctrl_rotate exec_status:%d %d", status.exec_status, status.has_run_time); - report.id = m_id; - report.report = status; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_rotate exec_status:%d", status); + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_mini_servo_ctrl_move_to, m_id) { // - errorcode = m_module->move_to(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_to_cb_status_t& status) { + errorcode = m_module->move_to(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_mini_servo_ctrl_move_to_report_t report = {0}; - ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_to exec_status:%d %d", status.exec_status, status.pos); - report.id = m_id; - report.report = status; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_to exec_status:%d", status); + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by, m_id) { // - errorcode = m_module->move_by(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_by_cb_status_t& status) { + errorcode = m_module->move_by(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_mini_servo_ctrl_move_by_report_t report = {0}; - ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by exec_status:%d %d", status.exec_status, status.dpos); - report.id = m_id; - report.report = status; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by exec_status:%d", status); + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_mini_servo_ctrl_run_with_torque, m_id) { // - errorcode = m_module->run_with_torque(cmd->torque, cmd->run_time, [this, cmdheader](I_MiniServoModule::run_with_torque_cb_status_t& status) { + errorcode = m_module->run_with_torque(cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_mini_servo_ctrl_run_with_torque_report_t report = {0}; - ZLOGI(TAG, "report kcmd_mini_servo_ctrl_run_with_torque exec_status:%d %d", status.exec_status, status.has_run_time); - report.id = m_id; - report.report = status; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_run_with_torque exec_status:%d", status); + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by_nolimit, m_id) { // - errorcode = m_module->move_by_nolimit(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_by_nolimit_cb_status_t& status) { + errorcode = m_module->move_by_nolimit(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_mini_servo_ctrl_move_by_nolimit_report_t report = {0}; - ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by_nolimit exec_status:%d %d", status.exec_status, status.pos); - report.id = m_id; - report.report = status; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by_nolimit exec_status:%d", status); + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } @@ -145,5 +145,4 @@ ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_get_warning_limit_param, CMD(u8 id; u8 opt_ PROCESS_PACKET(kcmd_mini_servo_ctrl_get_warning_limit_param, m_id) { errorcode = m_module->get_warning_limit_param(ack->ack); } END_PP(); - } diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 6eecdd0..25c741a 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 6eecdd008ec0ca37582579c70fc8e9bf40269d41 +Subproject commit 25c741a63cac28ee61d0658350dc82946e26e9cc