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
a70210f249
commit
8d9925fc92
13 changed files with 299 additions and 198 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. 2
      components/flash/znvs.cpp
  2. 3
      components/flash/znvs.hpp
  3. 115
      components/pipette_module/pipette_ctrl_module.cpp
  4. 37
      components/pipette_module/pipette_ctrl_module.hpp
  5. 25
      components/sensors/smtp2/smtp2.cpp
  6. 6
      components/sensors/smtp2/smtp2.hpp
  7. 177
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  8. 47
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  9. 12
      components/zcancmder_module/zcan_step_motor_ctrl_module.cpp
  10. 2
      components/zprotocols/zcancmder
  11. 3
      os/zos.cpp
  12. 49
      os/zthread.cpp
  13. 19
      os/zthread.hpp

2
components/flash/znvs.cpp
View File

@ -71,12 +71,14 @@ void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint
cfg->is_initialed = true;
cfg->type = (uint8_t)type;
memcpy(cfg->val, default_val, len);
#if 0
/**
* @brief 刷新flash
*
* TODO:优化这里的代码，不要每次都刷新flash
*/
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
#endif
break;
}
}

3
components/flash/znvs.hpp
View File

@ -41,6 +41,8 @@ class ZNVS {
void factory_reset();
void dumpcfg();
void flush();
int8_t get_config_int8(const char* key, int8_t default_val);
void set_config_int8(const char* key, int8_t val);
@ -71,7 +73,6 @@ class ZNVS {
cfg_t* get_cfg(const char* key);
void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
void flush();
};
} // namespace iflytop

115
components/pipette_module/pipette_ctrl_module.cpp
View File

@ -2,6 +2,19 @@
using namespace iflytop;
using namespace std;
#define TAG "PipetteModule"
#define DO(ACTION) \
{ \
int exec_ret = ACTION; \
if (exec_ret != 0) { \
ZLOGE(TAG, "do " #ACTION " fail, ret = %d", exec_ret); \
m_lastexecstatus = exec_ret; \
if (status_cb) status_cb(exec_ret); \
return; \
} \
ZLOGI(TAG, "do " #ACTION " complete"); \
}
void PipetteModule::initialize(SMTP2 *smtp2, //
StepMotorCtrlModule *stepMotor) {
m_smtp2 = smtp2;
@ -20,73 +33,67 @@ int32_t PipetteModule::stop(u8 stop_type) {
}
int32_t PipetteModule::zero_pos_calibrate(action_cb_status_t status_cb) {
ZLOGI(TAG, "zero_pos_calibrate");
m_thread.stop();
m_thread.start([this, status_cb]() {
int32_t exec_ret = do_motor_action_block([this]() { //
return m_stepMotor->move_to_zero_with_calibrate(0, nullptr);
});
if (exec_ret != 0) {
ZLOGE(TAG, "move_to_zero_with_calibrate fail, exec_ret = %d", exec_ret);
m_lastexecstatus = exec_ret;
if (status_cb) status_cb(exec_ret);
return;
}
ZLOGI(TAG, "zero_pos_calibrate complete");
m_thread.start([this, status_cb]() { //
DO(m_stepMotor->move_to_zero_with_calibrate_block(0));
m_lastexecstatus = 0;
if (status_cb) status_cb(m_lastexecstatus);
return;
});
return 0;
}
int32_t PipetteModule::reset_device(action_cb_status_t status_cb) {
ZLOGI(TAG, "reset_device");
m_thread.stop();
m_thread.start([this, status_cb]() {
/*******************************************************************************
* m_smtp2 *
*******************************************************************************/
// 移液枪复位
int ret = m_smtp2->doaction_block(&m_thread, [this]() { return m_smtp2->init_device(); });
if (ret != 0) {
ZLOGE(TAG, "init_device fail,%d", ret);
m_lastexecstatus = ret;
if (status_cb) status_cb(ret);
return;
}
ZLOGI(TAG, "SMTP2 init_device complete");
/*******************************************************************************
* m_stepMotor *
*******************************************************************************/
ret = do_motor_action_block( //
[this]() { return m_stepMotor->move_to_zero(nullptr); });
if (ret != 0) {
ZLOGE(TAG, "move_to_zero fail, ret = %d", ret);
m_lastexecstatus = ret;
if (status_cb) status_cb(m_lastexecstatus);
return;
}
ZLOGI(TAG, "Z axis reset complete");
m_lastexecstatus = ret;
DO(m_smtp2->init_device_block())
// Z轴复位
DO(m_stepMotor->move_to_zero_block());
//
m_lastexecstatus = 0;
if (status_cb) status_cb(m_lastexecstatus);
});
return 0;
}
int32_t PipetteModule::do_motor_action_block(function<int32_t()> action) {
if (action == nullptr) return -1;
int32_t ret = action();
if (ret != 0) {
m_stepMotor->stop(0);
return ret;
}
int32_t PipetteModule::take_tip(int take_tip_high, action_cb_status_t status_cb) {
ZLOGI(TAG, "take_tip %d", tipid);
while (!m_thread.getExitFlag()) {
if (!m_stepMotor->isbusy()) break;
m_thread.sleep(1000);
}
m_thread.stop();
m_thread.start([this, status_cb]() {
// 移动移液枪到取Tip位置
DO(m_stepMotor->move_to(m_platformpara.take_tip_height_mm, nullptr));
// 移液枪返回零点
DO(m_stepMotor->move_to(m_platformpara.transfer_height_mm, nullptr));
if (m_thread.getExitFlag()) {
m_stepMotor->stop(0);
return err::kcommon_error_break_by_user;
}
m_lastexecstatus = 0;
if (status_cb) status_cb(m_lastexecstatus);
});
}
int32_t PipetteModule::remove_tip(int remove_tip_high, action_cb_status_t status_cb) {
ZLOGI(TAG, "rmove_tip ");
m_thread.stop();
m_thread.start([this, status_cb]() {
// 移动移液枪到取Tip位置
DO(m_stepMotor->move_to(m_platformpara.remove_tip_height_mm, nullptr));
// 丢弃tip头
DO(m_smtp2->put_tip_block());
// 移液枪返回零点
DO(m_stepMotor->move_to(m_platformpara.transfer_height_mm, nullptr));
return m_stepMotor->get_last_exec_status();
m_lastexecstatus = 0;
if (status_cb) status_cb(m_lastexecstatus);
});
}
int32_t PipetteModule::take_and_split_liquid(u8 tube_id, s16 liquid_volume, s16 zhight, s16 abs_zhight, s16 shake_times, s16 shake_volume, //
action_cb_status_t status_cb) {
/**
* @brief
*/
return 0;
}

37
components/pipette_module/pipette_ctrl_module.hpp
View File

@ -22,19 +22,46 @@ class PipetteModule : public I_PipetteModule {
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);
virtual int32_t enable(u8 enable);
virtual int32_t stop(u8 stop_type);
virtual int32_t zero_pos_calibrate(action_cb_status_t status_cb);
virtual int32_t reset_device(action_cb_status_t status_cb);
virtual int32_t take_tip(int tipid, action_cb_status_t status_cb);
virtual int32_t remove_tip(action_cb_status_t status_cb);
// 取液(平台参数，试管参数, 取样体积，取样高度，摇匀次数，摇匀体积)
virtual int32_t take_and_split_liquid(u8 tube_id, s16 liquid_volume, s16 zhight, s16 abs_zhight, s16 shake_times, s16 shake_volume, //
virtual int32_t take_tip(int take_tip_high, action_cb_status_t status_cb);
virtual int32_t remove_tip(int remove_tip_high, action_cb_status_t status_cb);
/**
* @brief 取液摇匀（自动探查液面）
*
* @param max_deep_height_mm 最大深度
* @param relative_liquid_height_mm 相对液面高度
* @param shake_times 摇匀次数
* @param shake_volume 摇匀体积
* @param take_volume 取样体积
* @param status_cb
* @return int32_t
*/
virtual int32_t take_and_split_liquid(s16 max_deep_height_mm, //
s16 relative_liquid_height_mm, //
s16 shake_times, //
s16 shake_volume, //
s16 take_volume, //
action_cb_status_t status_cb);
/**
* @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);
public:
int32_t do_motor_action_block(function<int32_t()> action);
};
} // namespace iflytop

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

@ -110,15 +110,27 @@ int SMTP2::init_device() {
ZLOGI(TAG, "init_device");
return doaction(fmt("/1ZR\r"));
}
int SMTP2::init_device_block() {
return doaction_block([this]() { return init_device(); });
}
int SMTP2::put_tip() {
// ZLOGI(TAG, "put_tip:%s", "/1E1R");
ZLOGI(TAG, "put_tip");
return doaction(fmt("/1E1R\r"));
}
int SMTP2::put_tip_block() {
return doaction_block([this]() { return put_tip(); });
}
int SMTP2::move_to(int pos) {
ZLOGI(TAG, "move_to %d", pos);
return doaction(fmt("/1N%dA%dR\r", 0, pos));
}
int SMTP2::move_to_block(int pos) {
return doaction_block([this, pos]() { return move_to(pos); });
}
int SMTP2::move_to_ul(int ul) {
/**
* @brief 分辨率在0.319ul进行操作
@ -128,6 +140,10 @@ int SMTP2::move_to_ul(int ul) {
int stepNum = Get1fromfloat(stepNumfloat);
return doaction(fmt("/1N%dA%dR\r", 0, stepNum));
}
int SMTP2::move_to_ul_block(int ul) {
return doaction_block([this, ul]() { return move_to_ul(ul); });
}
int SMTP2::set_resolution(int resolution) {
ZLOGI(TAG, "set_resolution %d", resolution);
return doaction(fmt("/1N%dR\r", resolution));
@ -334,9 +350,10 @@ int SMTP2::sendcmd_block(const char* cmd, size_t txlen, char* rxbuf, size_t rxbu
return 0;
}
int32_t SMTP2::doaction_block(ZThread* thread, function<int32_t()> action) {
int32_t SMTP2::doaction_block(function<int32_t()> action) {
// 移液枪复位
int ret = action();
ThisThread thisThread;
int ret = action();
if (ret != 0) {
return ret;
}
@ -352,10 +369,10 @@ int32_t SMTP2::doaction_block(ZThread* thread, function<int32_t()> action) {
return ret;
}
}
if (thread->getExitFlag()) {
if (thisThread.getExitFlag()) {
stop();
return err::kcommon_error_break_by_user;
}
thread->sleep(1000);
thisThread.sleep(1000);
}
}

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

@ -67,12 +67,14 @@ class SMTP2 {
* @return int
*/
int init_device();
int init_device_block();
/**
* @brief 放Tip
*
* @return int
*/
int put_tip();
int put_tip_block();
/**
* @brief 移动到指定位置
@ -81,6 +83,7 @@ class SMTP2 {
* @return int
*/
int move_to(int pos);
int move_to_block(int pos);
/**
* @brief 移动到指定位置
*
@ -88,6 +91,7 @@ class SMTP2 {
* @return int
*/
int move_to_ul(int ul);
int move_to_ul_block(int ul);
/**
* @brief 读取电容值
@ -119,7 +123,7 @@ class SMTP2 {
*/
void stop();
int32_t doaction_block(ZThread* thread, function<int32_t()> action);
int32_t doaction_block(function<int32_t()> action);
private:
int32_t doaction(char* cmd);

177
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -14,6 +14,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g
m_statu_lock.init();
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
#if 0
run_param_t run_param;
run_to_zero_param_t run_to_zero_param;
warning_limit_param_t warning_limit_param;
@ -25,6 +26,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g
set_run_param(run_param);
set_run_to_zero_param(run_to_zero_param);
set_warning_limit_param(warning_limit_param);
#endif
}
bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); }
@ -35,16 +37,16 @@ int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb)
ZLOGI(TAG, "m%d move_to %d", m_id, tox);
m_thread.stop();
if (m_cfg_min_x != 0 && tox < m_cfg_min_x) {
tox = m_cfg_min_x;
if (m_param.min_x != 0 && tox < m_param.min_x) {
tox = m_param.min_x;
}
if (m_cfg_max_x != 0 && tox > m_cfg_max_x) {
tox = m_cfg_max_x;
if (m_param.max_x != 0 && tox > m_param.max_x) {
tox = m_param.max_x;
}
m_thread.start(
[this, tox, status_cb]() {
_motor_move_to(tox, m_cfg_velocity, m_cfg_acc, m_cfg_dec);
_motor_move_to(tox, m_param.maxspeed, m_param.acc, m_param.dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(10);
@ -112,8 +114,8 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb
}
int32_t calibrate_x, calibrate_y;
calibrate_x = dx + nowx;
m_zero_shift_x = calibrate_x;
calibrate_x = dx + nowx;
m_param.shift_x = calibrate_x;
m_stepM1->setXACTUAL(0);
m_lastexecstatus = 0;
@ -146,21 +148,21 @@ int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) {
}
int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { //
zlock_guard l(m_lock);
ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_cfg_acc, m_cfg_dec);
ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec);
if (lastforms < 0) {
ZLOGW(TAG, "lastforms:%d < 0", lastforms);
return err::kcommon_error_param_out_of_range;
}
if (abs(speed) > m_cfg_max_speed) {
ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_cfg_max_speed);
speed = m_cfg_max_speed;
if (abs(speed) > m_param.maxspeed) {
ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.maxspeed);
speed = m_param.maxspeed;
}
m_thread.stop();
m_thread.start([this, lastforms, speed, status_cb]() {
m_stepM1->setAcceleration(m_cfg_acc);
m_stepM1->setDeceleration(m_cfg_dec);
m_stepM1->setAcceleration(m_param.acc);
m_stepM1->setDeceleration(m_param.dec);
m_stepM1->rotate(speed);
int32_t startticket = zos_get_tick();
@ -199,83 +201,34 @@ int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info)
return 0;
}
int32_t StepMotorCtrlModule::set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) {
if (operation == kset_cmd_type_set) {
m_cfg_distance_scale = param.distance_scale / 1000.0;
m_cfg_acc = param.acc;
m_cfg_dec = param.dec;
m_cfg_max_speed = param.maxspeed;
m_cfg_min_x = param.min_x;
m_cfg_max_x = param.max_x;
m_cfg_ihold = param.ihold;
m_cfg_irun = param.irun;
m_cfg_iholddelay = param.iholddelay;
m_cfg_x_shaft = param.x_shaft;
m_cfg_shift_x = param.shift_x;
ZLOGI(TAG, "=========== run param ============");
ZLOGI(TAG, "= distance_scale :%f", m_cfg_distance_scale);
ZLOGI(TAG, "= acc :%d", m_cfg_acc);
ZLOGI(TAG, "= dec :%d", m_cfg_dec);
ZLOGI(TAG, "= max_speed :%d", m_cfg_max_speed);
ZLOGI(TAG, "= min_x :%d", m_cfg_min_x);
ZLOGI(TAG, "= max_x :%d", m_cfg_max_x);
ZLOGI(TAG, "= ihold :%d", m_cfg_ihold);
ZLOGI(TAG, "= irun :%d", m_cfg_irun);
ZLOGI(TAG, "= iholddelay :%d", m_cfg_iholddelay);
ZLOGI(TAG, "= x_shaft :%d", m_cfg_x_shaft);
ZLOGI(TAG, "= shift_x :%d", m_cfg_shift_x);
ZLOGI(TAG, "==================================");
m_stepM1->setIHOLD_IRUN(m_cfg_ihold, m_cfg_irun, m_cfg_iholddelay);
m_stepM1->setScale(m_cfg_distance_scale);
}
ack.distance_scale = m_cfg_distance_scale * 1000;
ack.acc = m_cfg_acc;
ack.dec = m_cfg_dec;
ack.maxspeed = m_cfg_max_speed;
ack.min_x = m_cfg_min_x;
ack.max_x = m_cfg_max_x;
ack.ihold = m_cfg_ihold;
ack.irun = m_cfg_irun;
ack.iholddelay = m_cfg_iholddelay;
ack.x_shaft = m_cfg_x_shaft;
ack.shift_x = m_cfg_shift_x;
return 0;
int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) {
m_param = param;
m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay);
m_stepM1->setScale(m_param.distance_scale / 1000.0);
ZLOGI(TAG, "=========== base param ============");
ZLOGI(TAG, "= x_shaft :%d", m_param.x_shaft);
ZLOGI(TAG, "= distance_scale :%f", m_param.distance_scale);
ZLOGI(TAG, "= ihold :%d", m_param.ihold);
ZLOGI(TAG, "= irun :%d", m_param.irun);
ZLOGI(TAG, "= iholddelay :%d", m_param.iholddelay);
ZLOGI(TAG, "= acc :%d", m_param.acc);
ZLOGI(TAG, "= dec :%d", m_param.dec);
ZLOGI(TAG, "= maxspeed :%d", m_param.maxspeed);
ZLOGI(TAG, "= min_x :%d", m_param.min_x);
ZLOGI(TAG, "= max_x :%d", m_param.max_x);
ZLOGI(TAG, "= shift_x :%d", m_param.shift_x);
ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d);
ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d);
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, "==================================");
}
int32_t StepMotorCtrlModule::set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) {
if (operation == kset_cmd_type_set) {
m_cfg_runtozero_maxX = param.move_to_zero_max_d;
m_cfg_runtozero_leave_away_zero_max_distance = param.leave_from_zero_max_d;
m_cfg_runtozero_speed = param.speed;
m_cfg_runtozero_dec = param.dec;
ZLOGI(TAG, "=========== run_to_zero_param ============");
ZLOGI(TAG, "= move_to_zero_max_d :%d", m_cfg_runtozero_maxX);
ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_cfg_runtozero_leave_away_zero_max_distance);
ZLOGI(TAG, "= speed :%d", m_cfg_runtozero_speed);
ZLOGI(TAG, "= dec :%d", m_cfg_runtozero_dec);
ZLOGI(TAG, "===========================================");
m_stepM1->setAcceleration(m_cfg_runtozero_dec);
m_stepM1->setDeceleration(m_cfg_runtozero_dec);
}
ack.move_to_zero_max_d = m_cfg_runtozero_maxX;
ack.leave_from_zero_max_d = m_cfg_runtozero_leave_away_zero_max_distance;
ack.speed = m_cfg_runtozero_speed;
ack.dec = m_cfg_runtozero_dec;
return 0;
}
int32_t StepMotorCtrlModule::set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) {
if (operation == kset_cmd_type_set) {
ZLOGI(TAG, "set warning limit param");
}
int32_t StepMotorCtrlModule::get_base_param(base_param_t& param) {
param = m_param;
return 0;
}
int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) {
int32_t xnow;
getnowpos(xnow);
@ -296,7 +249,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
/**
* @brief 如果设备已经在零点，则反向移动一定距离远离零点
*/
_motor_move_by(m_cfg_runtozero_leave_away_zero_max_distance, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
_motor_move_by(m_param.run_to_zero_leave_from_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(1);
@ -314,7 +267,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
/*******************************************************************************
* 移动X轴到零点 *
*******************************************************************************/
_motor_move_by(-m_cfg_runtozero_maxX, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
_motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
bool xreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
@ -379,12 +332,12 @@ bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTar
void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) {
// m_zero_shift_x
x = motor_pos;
if (m_cfg_x_shaft != 0) x = -x;
x += m_zero_shift_x;
if (m_param.x_shaft != 0) x = -x;
x += m_param.shift_x;
}
void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) {
if (m_cfg_x_shaft != 0) x = -x;
x -= m_zero_shift_x;
if (m_param.x_shaft != 0) x = -x;
x -= m_param.shift_x;
motor_pos = x;
}
#if 0
@ -392,4 +345,40 @@ void StepMotorCtrlModule::updateStatus(uint8_t status) {
zlock_guard lock(m_statu_lock);
m_status = status;
}
#endif
#endif
int32_t StepMotorCtrlModule::do_motor_action_block(function<int32_t()> action) {
if (action == nullptr) return -1;
int32_t ret = action();
if (ret != 0) {
stop(0);
return ret;
}
ThisThread thisThread;
while (!thisThread.getExitFlag()) {
if (!isbusy()) break;
thisThread.sleep(1000);
}
if (isbusy()) {
stop(0);
return err::kcommon_error_break_by_user;
}
return get_last_exec_status();
}
int32_t StepMotorCtrlModule::move_to_block(int32_t tox) {
return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); });
}
int32_t StepMotorCtrlModule::move_by_block(int32_t dx) {
return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); });
}
int32_t StepMotorCtrlModule::move_to_zero_block() {
return do_motor_action_block([this]() { return move_to_zero(nullptr); });
}
int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x) {
return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); });
}

47
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -16,34 +16,11 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
ZThread m_thread;
// uint8_t m_status = 0;
int32_t m_zero_shift_x = 0;
zmutex m_lock;
zmutex m_statu_lock;
int32_t m_cfg_acc = 300000;
int32_t m_cfg_dec = 300000;
int32_t m_cfg_break_dec = 300000;
int32_t m_cfg_velocity = 300000;
int32_t m_cfg_max_speed = 300000;
u8 m_cfg_ihold = 0;
u8 m_cfg_irun = 0;
u16 m_cfg_iholddelay = 0;
u8 m_cfg_x_shaft = 0;
int32_t m_cfg_shift_x = 0;
int32_t m_cfg_min_x = 0;
int32_t m_cfg_max_x = 0;
int32_t m_cfg_runtozero_maxX = 5120000;
int32_t m_cfg_runtozero_speed = 30000;
int32_t m_cfg_runtozero_dec = 600000;
int32_t m_cfg_runtozero_leave_away_zero_max_distance = 51200;
float m_cfg_distance_scale = 1.0f;
base_param_t m_param;
// bool m_x_shaft = false;
int32_t m_lastexecstatus = 0;
public:
@ -52,10 +29,16 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
virtual bool isbusy() override;
virtual int32_t get_last_exec_status() override { return m_lastexecstatus; };
virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb);
virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb);
virtual int32_t move_to_zero(action_cb_status_t status_cb);
virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb);
virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb) override;
virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) override;
virtual int32_t move_to_zero(action_cb_status_t status_cb) override;
virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) override;
virtual int32_t move_to_block(int32_t tox) override;
virtual int32_t move_by_block(int32_t dx) override;
virtual int32_t move_to_zero_block() override;
virtual int32_t move_to_zero_with_calibrate_block(int32_t x) override;
virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb);
virtual int32_t enable(bool venable) override;
@ -66,8 +49,12 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
virtual int32_t read_status(status_t& status) override;
virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
virtual int32_t set_base_param(const base_param_t& param) override;
virtual int32_t get_base_param(base_param_t& param) override;
#if 0
virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override;
virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;
// virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;
virtual int32_t set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) override;
int32_t read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); }
@ -77,8 +64,10 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
int32_t set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); }
int32_t set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); }
int32_t set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); }
#endif
private:
int32_t do_motor_action_block(function<int32_t()> action);
// void updateStatus(uint8_t status);
void getnowpos(int32_t& pos);

12
components/zcancmder_module/zcan_step_motor_ctrl_module.cpp
View File

@ -102,19 +102,15 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
}
END_PP();
PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_param, m_id) { //
errorcode = m_module->set_run_param(cmd->opt_type, cmd->param, ack->ack);
PROCESS_PACKET(kcmd_step_motor_ctrl_set_base_param, m_id) { //
errorcode = m_module->set_base_param(cmd->param);
}
END_PP();
PROCESS_PACKET(kcmd_step_motor_ctrl_set_warning_limit_param, m_id) { //
errorcode = m_module->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack);
PROCESS_PACKET(kcmd_step_motor_ctrl_get_base_param, m_id) { //
errorcode = m_module->get_base_param(ack->ack);
}
END_PP();
PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_to_zero_param, m_id) { //
errorcode = m_module->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack);
}
END_PP();
}
#endif

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 358d158302546707f113d4028d4fa66e1da29d89
Subproject commit 37039700689af7ef1de1f5f83c2277bae69a139a

3
os/zos.cpp
View File

@ -1,7 +1,10 @@
#include "zos.hpp"
#include "zthread.hpp"
extern "C" {
void zos_init(zos_cfg_t* cfg) { //
zos_loggger_init();
iflytop::OSDefaultSchduler::getInstance()->initialize();
iflytop::ZThread::zthread_module_init();
}
}

49
os/zthread.cpp
View File

@ -1,17 +1,46 @@
#include "zthread.hpp"
#include <map>
#include "zoslogger.hpp"
using namespace iflytop;
using namespace std;
static bool s_map_inited = false;
static zmutex s_zthread_map_lock;
static map<osThreadId, ZThread *> s_zthread_map;
static ZThread g_default_thread;
static void zosthread_default_task(void const *argument) {
ZThread *thread = (ZThread *)argument;
ZASSERT(thread);
thread->threadcb();
};
void ZThread::zthread_module_init() {
if (s_map_inited) return;
s_map_inited = true;
s_zthread_map_lock.init();
}
ZThread *ZThread::getThread() {
ZThread *thread = nullptr;
{
zlock_guard guard(s_zthread_map_lock);
thread = s_zthread_map[osThreadGetId()];
}
return thread;
}
void ZThread::threadcb() {
m_status = kidle;
{
zlock_guard guard(s_zthread_map_lock);
s_zthread_map[osThreadGetId()] = this;
}
while (true) {
if (m_threadisworkingFlagCallSide) {
m_status = kworking;
@ -83,4 +112,24 @@ void ZThread::wake() {
} else {
xTaskNotifyGive(m_defaultTaskHandle);
}
}
ThisThread::ThisThread() {
ZThread *thread = ZThread::getThread();
this->thread = thread;
}
bool ThisThread::getExitFlag() {
if (thread) return thread->getExitFlag();
return false;
}
bool ThisThread::isworking() {
if (thread) return thread->isworking();
return true;
}
void ThisThread::sleep(uint32_t ms) {
if (thread) {
thread->sleep(ms);
} else {
vTaskDelay(pdMS_TO_TICKS(ms));
}
}

19
os/zthread.hpp
View File

@ -32,6 +32,11 @@ class ZThread {
SemaphoreHandle_t m_lock;
public:
/**
* @brief init in zos.cpp, called by zos_init
*/
static void zthread_module_init();
void init(const char* threadname, int stack_size = 1024, osPriority priority = osPriorityNormal);
void start(zosthread_cb_t cb);
void start(zosthread_cb_t cb, zosthread_cb_t exitcb);
@ -40,11 +45,23 @@ class ZThread {
bool getExitFlag() { return !m_threadisworkingFlagCallSide; }
bool isworking() { return m_status == kworking; }
void sleep(uint32_t ms);
void wake();
static void sleep(uint32_t ms);
static ZThread* getThread();
public:
void threadcb();
};
class ThisThread {
ZThread* thread;
public:
ThisThread();
bool getExitFlag();
bool isworking();
void sleep(uint32_t ms);
};
} // namespace iflytop
Write Preview
Loading…
Cancel
Save