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

V1212: 

1. 增加步进电机驱动IO镜像配置项
        2. 修复ztmc5130部分BUG
master
zhaohe 3 weeks ago
parent
cd94c9f031
commit
c5e89cfab0
6 changed files with 51 additions and 55 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. 27
      README.md
  2. 66
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  3. 7
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  4. 2
      sdk/components/tmc/ic/ztmc5130.cpp
  5. 2
      usrc/a8000_protocol/protocol/reg_index.hpp
  6. 2
      usrc/version.h

27
README.md
View File

@ -4,8 +4,11 @@
V602
1. 修复移液枪Z轴控制部分BUG
V1212:
1. 增加步进电机驱动IO镜像配置项
```
```
编译:
@ -37,28 +40,6 @@ ID地址:
```
```
```
```
修改
1. 去除掉input_reg，修改驱动读取gpio的寄存器为kreg_step_motor_io_state
2. 增加舵机专用寄存器和方法
```
```
```

66
sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -16,6 +16,7 @@ void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable
m_iotable = iotable;
m_nio = nio;
m_thread.init(TAG, 1024, osPriorityNormal);
m_cfg = *cfg;
m_default_cfg = *cfg;
@ -137,6 +138,9 @@ int32_t StepMotorCtrlModule::module_set_reg(int32_t regindex, int32_t val) {
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_pos_devi_tolerance, (m_cfg.pos_devi_tolerance));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_io_trigger_append_distance, (m_cfg.io_trigger_append_distance));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_zero_io_mirror, (m_cfg.zero_io_mirror));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_limit_io_mirror, (m_cfg.limit_io_mirror));
default:
return err::kmodule_not_find_reg;
break;
@ -215,6 +219,9 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t regindex, int32_t* val) {
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_pos_devi_tolerance, m_cfg.pos_devi_tolerance);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_io_trigger_append_distance, m_cfg.io_trigger_append_distance);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_zero_io_mirror, (m_cfg.zero_io_mirror));
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_limit_io_mirror, (m_cfg.limit_io_mirror));
default:
return err::kmodule_not_find_reg;
break;
@ -262,16 +269,17 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() {
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);
// 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);
m_stepM1->stop();
// stepmotor_iglobalscaler
if (m_state.enable) {
@ -285,7 +293,7 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t*
if (ioindex < 0 || ioindex >= m_nio) {
return err::kstep_motor_ioindex_out_of_range;
}
*state = m_iotable[ioindex].getState();
*state = getIOState(ioindex) ? 1 : 0;
return 0;
}
@ -411,11 +419,13 @@ bool StepMotorCtrlModule::check_when_run() {
} else if (state.uv_cp) {
ZLOGE(TAG, "motor uv_cp when run");
dumpTMC5130Status(&devStatus);
module_detail_errorcode = *((int32_t*)&devStatus);
setErrorFlag(err::kstep_motor_uv_cp);
return false;
} else if (state.drv_err) {
ZLOGE(TAG, "motor drv_err when run");
setErrorFlag(err::kstep_motor_drv_err);
module_detail_errorcode = *((int32_t*)&devStatus);
dumpTMC5130Status(&devStatus);
return false;
}
@ -430,16 +440,15 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
direction = direction >= 0 ? 1 : -1;
ZGPIO* gpio = &m_iotable[ioindex];
if (!check_when_run()) return false;
if (!gpio->getState()) {
if (!getIOState(ioindex)) {
ZLOGI(TAG, "---------STEP1-------- move to point");
rotate(direction, runToPointSpeed);
bool xreach_io = false;
while (!m_thread.getExitFlag()) {
if (gpio->getState()) {
if (getIOState(ioindex)) {
xreach_io = true;
m_stepM1->stop();
break;
@ -465,13 +474,13 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
// 如果设备已经在零点，则反向移动一定距离远离零点
if (gpio->getState()) {
if (getIOState(ioindex)) {
ZLOGI(TAG, "---------STEP2-------- find edge");
rotate(-direction, lookPointVelocity);
bool reach_edge = false;
while (!m_thread.getExitFlag()) {
if (!gpio->getState()) {
if (!getIOState(ioindex)) {
reach_edge = true;
m_stepM1->stop();
break;
@ -494,13 +503,13 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
if (!check_when_run()) return false;
// 如果设备已经在零点，则反向移动一定距离远离零点
if (!gpio->getState()) {
if (!getIOState(ioindex)) {
ZLOGI(TAG, "---------STEP3-------- bak to gpio trigger edge");
rotate(direction, lookPointVelocity);
bool reach_edge = false;
while (!m_thread.getExitFlag()) {
if (gpio->getState()) {
if (getIOState(ioindex)) {
reach_edge = true;
m_stepM1->stop();
break;
@ -623,9 +632,6 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() {
return err::kstep_motor_ioindex_out_of_range;
}
if (m_iotable[0].isNull()) {
return err::kstep_motor_ioindex_out_of_range;
}
return do_step_motor_easy_move_to_zero();
}
@ -648,9 +654,6 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() {
if (m_nio <= 1) {
return err::kstep_motor_ioindex_out_of_range;
}
if (m_iotable[1].isNull()) {
return err::kstep_motor_ioindex_out_of_range;
}
return do_step_motor_easy_move_to_end_point();
}
int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) {
@ -793,9 +796,6 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int3
if (ioindex < 0 || ioindex >= m_nio) {
return err::kstep_motor_ioindex_out_of_range;
}
if (m_iotable[ioindex].isNull()) {
return err::kstep_motor_ioindex_out_of_range;
}
m_thread.stop();
module_status = 1;
@ -985,3 +985,15 @@ int32_t StepMotorCtrlModule::getspeed(int32_t speedlevel) {
}
return speed;
}
bool StepMotorCtrlModule::getIOState(int32_t ioindex) {
if (ioindex < 0 || ioindex >= m_nio) {
return false;
}
if (ioindex == 0) {
return m_cfg.zero_io_mirror ? !m_iotable[ioindex].getState() : m_iotable[ioindex].getState();
} else if (ioindex == 1) {
return m_cfg.limit_io_mirror ? !m_iotable[ioindex].getState() : m_iotable[ioindex].getState();
} else {
return m_iotable[ioindex].getState();
}
}

7
sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -42,6 +42,8 @@ class StepMotorCtrlModule : public ZIModule {
int32_t motor_dzero; // 零点偏移
int32_t pos_devi_tolerance;
int32_t io_trigger_append_distance;
int32_t zero_io_mirror;
int32_t limit_io_mirror;
} config_t;
typedef struct {
@ -162,9 +164,6 @@ class StepMotorCtrlModule : public ZIModule {
private:
int32_t getspeed(int32_t speedlevel);
// int32_t read_enc_val(int32_t& enc_val);
// int32_t write_enc_val(int32_t enc_val);
// virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution);
bool getIOState(int32_t ioindex);
};
} // namespace iflytop

2
sdk/components/tmc/ic/ztmc5130.cpp
View File

@ -190,6 +190,7 @@ void TMC51X0::setXACTUAL(int32_t value) {
rotate(0);
writeInt(TMC5130_XACTUAL, to_motor_pos(value));
writeInt(TMC5130_XTARGET, to_motor_pos(value));
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); // 切换回位置模式
}
int32_t TMC51X0::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }
void TMC51X0::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度
@ -246,6 +247,7 @@ Tmc5130RampStat TMC51X0::getTMC5130_RAMPSTAT2() {
void TMC51X0::stop() {
int mode = readInt(TMC5130_RAMPMODE);
if (mode == TMC5130_MODE_POSITION) {
writeInt(TMC5130_TZEROWAIT, 0);
writeInt(TMC5130_XTARGET, readInt(TMC5130_XACTUAL));
} else {
rotate(0);

2
usrc/a8000_protocol/protocol/reg_index.hpp
View File

@ -217,6 +217,8 @@ typedef enum {
kreg_step_motor_dzero_pos = 10191, //
kreg_step_motor_pos_devi_tolerance = 10192, // 位置偏差容忍度
kreg_step_motor_io_trigger_append_distance = 10193, // 移动到IO时，会在原点触发的边沿，稍稍里面移动一点，用来保证IO稳定触发
kreg_step_motor_zero_io_mirror = 10194, // 零点IO有效位是否翻转
kreg_step_motor_limit_io_mirror = 10195, // 零点IO有效位是否翻转
/***********************************************************************************************************************
* step_motor *

2
usrc/version.h
View File

@ -1,2 +1,2 @@
#pragma once
#define APP_VERSION 1211
#define APP_VERSION 1212
Write Preview
Loading…
Cancel
Save