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
d3f04ac663
commit
161b73adc6
2 changed files with 97 additions and 34 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. 126
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 5
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

126
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -46,14 +46,17 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO iotable
bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); }
int32_t StepMotorCtrlModule::move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) {
ZLOGI(TAG, "move_to_logic_point %d", logic_point_num);
if (logic_point_num >= ZARRAY_SIZE(m_flash_config.logic_point)) {
return err::kce_param_out_of_range;
}
logic_point_t logic_point = m_flash_config.logic_point[logic_point_num];
return move_to(logic_point.velocity, logic_point.x, status_cb);
return move_to(logic_point.x, logic_point.velocity, status_cb);
}
int32_t StepMotorCtrlModule::set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) {
ZLOGI(TAG, "set_logic_point %d %d %d %d %d", logic_point_num, x, vel, acc, dec);
if (logic_point_num >= ZARRAY_SIZE(m_flash_config.logic_point)) return err::kce_param_out_of_range;
if (logic_point_num < 0) logic_point_num = 0;
// memset(&m_flash_config, 0, sizeof(m_flash_config));
m_flash_config.logic_point[logic_point_num].x = x;
m_flash_config.logic_point[logic_point_num].velocity = vel;
@ -73,7 +76,9 @@ int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb)
zlock_guard lock(m_lock);
ZLOGI(TAG, "m%d move_to %d", m_id, tox);
m_status_cb = nullptr;
m_thread.stop();
m_status_cb = status_cb;
if (m_param.min_x != 0 && tox < m_param.min_x) {
tox = m_param.min_x;
@ -97,7 +102,9 @@ int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb)
int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "m%d move_by %d", m_id, dx);
m_status_cb = nullptr;
m_thread.stop();
m_status_cb = status_cb;
int32_t xnow = 0;
getnowpos(xnow);
@ -137,7 +144,9 @@ int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) {
int32_t StepMotorCtrlModule::calculate_curpos(action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "read_curpos_by_move2zero");
m_status_cb = nullptr;
m_thread.stop();
m_status_cb = status_cb;
m_thread.start(
[this, status_cb]() {
//
@ -158,7 +167,9 @@ int32_t StepMotorCtrlModule::read_calculate_curpos_action_result(int32_t& pos) {
int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_to_zero");
m_status_cb = nullptr;
m_thread.stop();
m_status_cb = status_cb;
m_thread.start(
[this, status_cb]() {
@ -176,7 +187,9 @@ int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) {
int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx);
m_status_cb = nullptr;
m_thread.stop();
m_status_cb = status_cb;
m_thread.start([this, nowx, status_cb]() {
int32_t dx;
@ -278,8 +291,11 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) {
}
int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
debug_info.status = m_thread.isworking() ? 1 : 0;
if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00;
if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00;
for (size_t i = 0; i < m_nio; i++) {
if (m_iotable[i].getState()) {
debug_info.io_state |= (0x01 << i);
}
}
debug_info.last_exec_status = m_lastexecstatus;
getnowpos(debug_info.x);
return 0;
@ -341,52 +357,96 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) {
}
int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
/*******************************************************************************
* 第一次移动到零点 *
*******************************************************************************/
if (!m_Xgpio->getState()) {
{
ZLOGI(TAG, "---------STEP1-------- move to zero first");
_motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed * 2, 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());
if (m_Xgpio->getState()) {
xreach_zero = true;
_motor_stop(-1);
while (!m_stepM1->isStoped()) { // 等待电机停止
_motor_stop(-1);
vTaskDelay(10);
}
break;
}
vTaskDelay(1);
}
if (!xreach_zero) {
// 触发异常回调
ZLOGE(TAG, "x reach zero failed");
return err::kce_not_found_x_zero_point;
}
}
}
/*******************************************************************************
* 远离X零点 *
*******************************************************************************/
if (m_Xgpio->getState()) {
{
ZLOGI(TAG, "---------STEP2-------- leave away zero");
/**
* @brief 如果设备已经在零点，则反向移动一定距离远离零点
*/
_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);
if (m_Xgpio->getState()) {
_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;
if (!m_Xgpio->getState()) break;
vTaskDelay(1);
}
}
if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return 0;
return err::kce_break_by_user;
}
if (m_Xgpio->getState()) {
ZLOGE(TAG, "leave away zero failed");
return err::kce_x_leave_away_zero_point_fail;
}
}
/*******************************************************************************
* 移动X轴到零点 *
*******************************************************************************/
_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());
if (m_Xgpio->getState()) {
xreach_zero = true;
_motor_stop(-1);
while (!m_stepM1->isStoped()) { // 等待电机停止
{
ZLOGI(TAG, "---------STEP3-------- move to zero");
_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());
if (m_Xgpio->getState()) {
xreach_zero = true;
_motor_stop(-1);
vTaskDelay(10);
while (!m_stepM1->isStoped()) { // 等待电机停止
_motor_stop(-1);
vTaskDelay(10);
}
break;
}
vTaskDelay(1);
}
break;
if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return err::kce_break_by_user;
}
vTaskDelay(1);
}
if (!xreach_zero) {
// 触发异常回调
ZLOGE(TAG, "x reach zero failed");
return err::kce_not_found_x_zero_point;
if (!xreach_zero) {
// 触发异常回调
ZLOGE(TAG, "x reach zero failed");
return err::kce_not_found_x_zero_point;
}
}
if (m_thread.getExitFlag()) {
@ -423,6 +483,7 @@ void StepMotorCtrlModule::_motor_move_by(int32_t dpos, int32_t maxv, int32_t acc
int32_t topos = nowpos + dpos;
int32_t motorpos = 0;
forward_kinematics(topos, motorpos);
ZLOGI(TAG, "motor moveTo %d %d", motorpos, maxv);
m_stepM1->moveTo(motorpos, maxv);
}
void StepMotorCtrlModule::_motor_stop(int32_t dec) {
@ -511,8 +572,9 @@ int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int32_t velocity, int ove
}
void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) {
m_lastexecstatus = status;
if (status_cb) status_cb(status);
m_lastexecstatus = status;
auto cache_status_cb = m_status_cb;
if (cache_status_cb) cache_status_cb(status);
}
void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) {
memset(&cfg, 0, sizeof(cfg));
@ -522,11 +584,11 @@ void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) {
cfg.base_param.iholddelay = 100;
cfg.base_param.acc = 3000000;
cfg.base_param.dec = 3000000;
cfg.base_param.maxspeed = 300000;
cfg.base_param.maxspeed = 500000;
cfg.base_param.run_to_zero_move_to_zero_max_d = INT32_MAX;
cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 3;
cfg.base_param.run_to_zero_speed = 300000;
cfg.base_param.run_to_zero_move_to_zero_max_d = 51200 * 1000;
cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 10;
cfg.base_param.run_to_zero_speed = 100000;
cfg.base_param.run_to_zero_dec = 3000000;
}

5
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -28,8 +28,9 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
ZGPIO* m_iotable;
int m_nio;
int32_t m_calculate_curpos_result = 0;
const char* m_flashmark = nullptr;
int32_t m_calculate_curpos_result = 0;
const char* m_flashmark = nullptr;
action_cb_status_t m_status_cb;
public:
void initialize(int id, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark);

Write Preview
Loading…
Cancel
Save