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

update

master
zhaohe 2 years ago
parent
ab0aa61d85
commit
20e9dd6ea9
10 changed files with 181 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 89
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  2. 13
      components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
  3. 21
      components/pipette_module/pipette_ctrl_module.cpp
  4. 35
      components/pipette_module/pipette_ctrl_module.hpp
  5. 31
      components/sensors/smtp2/smtp2.cpp
  6. 10
      components/sensors/smtp2/smtp2.hpp
  7. 13
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  8. 2
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  9. 41
      components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp
  10. 2
      components/zprotocols/zcancmder

89
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -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<void(rotate_cb_status_t& status)> 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<void(move_to_cb_status_t& status)> 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, function<vo
if (!m_bus->moveTo(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<void(move_by_cb_status_t& status)> 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<v
if (targetpos > 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<void(run_with_torque_cb_status_t& status)> 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<void(move_by_nolimit_cb_status_t& status)> 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);
}

13
components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
View File

@ -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<void(rotate_cb_status_t& status)> status_cb) override;
virtual int32_t move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) override;
virtual int32_t move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) override;
virtual int32_t run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) override;
virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> 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

21
components/pipette_module/pipette_ctrl_module.cpp
View File

@ -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) {

35
components/pipette_module/pipette_ctrl_module.hpp
View File

@ -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);
};

31
components/sensors/smtp2/smtp2.cpp
View File

@ -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);

10
components/sensors/smtp2/smtp2.hpp
View File

@ -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 设置分辨率
*

13
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -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); });
}
}

2
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -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;

41
components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp
View File

@ -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();
}

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 6eecdd008ec0ca37582579c70fc8e9bf40269d41
Subproject commit 25c741a63cac28ee61d0658350dc82946e26e9cc
Write Preview
Loading…
Cancel
Save