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 1 year ago
parent
21bf093562
commit
6060bcb657
5 changed files with 72 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 87
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 22
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  3. 4
      components/tmc/basic/tmc_ic_interface.hpp
  4. 9
      components/tmc/ic/ztmc5130.cpp
  5. 19
      components/tmc/ic/ztmc5130.hpp

87
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -10,7 +10,7 @@
using namespace iflytop; using namespace iflytop;
#define TAG "SMCM" #define TAG "SMCM"
void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg) {
void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg) {
m_id = moduleid; m_id = moduleid;
m_stepM1 = stepM; m_stepM1 = stepM;
@ -51,24 +51,9 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
cfg.stepmotor_ihold = 1; cfg.stepmotor_ihold = 1;
cfg.stepmotor_irun = 3; cfg.stepmotor_irun = 3;
cfg.stepmotor_iholddelay = 100; cfg.stepmotor_iholddelay = 100;
cfg.motor_default_acc = 300;
cfg.motor_default_dec = 300;
cfg.motor_default_velocity = 500; cfg.motor_default_velocity = 500;
cfg.motor_run_to_zero_speed = 100;
cfg.motor_run_to_zero_dec = 600;
cfg.motor_look_zero_edge_speed = 100;
cfg.motor_look_zero_edge_dec = 600;
// cfg.motor_vstart = 2000;
// cfg.motor_a1 = ;
// cfg.motor_amax = 300;
// cfg.motor_v1 = ;
// cfg.motor_dmax = 300;
// cfg.motor_d1 = ;
// cfg.motor_vstop = 2000;
// cfg.motor_tzerowait = ;
cfg.motor_run_to_zero_speed = 100;
cfg.motor_look_zero_edge_speed = 100;
} }
/*********************************************************************************************************************** /***********************************************************************************************************************
@ -77,24 +62,6 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
int32_t suc = pri_module_xxx_reg(param_id, read, val); int32_t suc = pri_module_xxx_reg(param_id, read, val);
if (suc != 0) return suc; if (suc != 0) return suc;
switch (param_id) {
case kreg_step_motor_shaft:
case kreg_step_motor_one_circle_pulse:
case kreg_step_motor_one_circle_pulse_denominator:
case kreg_step_motor_default_velocity:
case kreg_step_motor_default_acc:
case kreg_step_motor_default_dec:
case kreg_step_motor_run_to_zero_speed:
case kreg_step_motor_run_to_zero_dec:
case kreg_step_motor_look_zero_edge_speed:
case kreg_step_motor_look_zero_edge_dec:
case kreg_step_motor_ihold:
case kreg_step_motor_irun:
case kreg_step_motor_iholddelay:
case kreg_step_motor_max_d:
case kreg_step_motor_min_d:
return 0;
}
return suc; return suc;
} }
int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
@ -106,12 +73,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse)); PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse));
PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_cfg.motor_one_circle_pulse_denominator), REG_SET(m_cfg.motor_one_circle_pulse_denominator)); PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_cfg.motor_one_circle_pulse_denominator), REG_SET(m_cfg.motor_one_circle_pulse_denominator));
PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_cfg.motor_default_velocity), REG_SET(m_cfg.motor_default_velocity)); PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_cfg.motor_default_velocity), REG_SET(m_cfg.motor_default_velocity));
PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_cfg.motor_default_acc), REG_SET(m_cfg.motor_default_acc));
PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_cfg.motor_default_dec), REG_SET(m_cfg.motor_default_dec));
PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_cfg.motor_run_to_zero_speed), REG_SET(m_cfg.motor_run_to_zero_speed)); PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_cfg.motor_run_to_zero_speed), REG_SET(m_cfg.motor_run_to_zero_speed));
PROCESS_REG(kreg_step_motor_run_to_zero_dec, REG_GET(m_cfg.motor_run_to_zero_dec), REG_SET(m_cfg.motor_run_to_zero_dec));
PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_cfg.motor_look_zero_edge_speed), REG_SET(m_cfg.motor_look_zero_edge_speed)); PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_cfg.motor_look_zero_edge_speed), REG_SET(m_cfg.motor_look_zero_edge_speed));
PROCESS_REG(kreg_step_motor_look_zero_edge_dec, REG_GET(m_cfg.motor_look_zero_edge_dec), REG_SET(m_cfg.motor_look_zero_edge_dec));
PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_cfg.stepmotor_ihold), REG_SET(m_cfg.stepmotor_ihold)); PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_cfg.stepmotor_ihold), REG_SET(m_cfg.stepmotor_ihold));
PROCESS_REG(kreg_step_motor_irun, REG_GET(m_cfg.stepmotor_irun), REG_SET(m_cfg.stepmotor_irun)); PROCESS_REG(kreg_step_motor_irun, REG_GET(m_cfg.stepmotor_irun), REG_SET(m_cfg.stepmotor_irun));
PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_cfg.stepmotor_iholddelay), REG_SET(m_cfg.stepmotor_iholddelay)); PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_cfg.stepmotor_iholddelay), REG_SET(m_cfg.stepmotor_iholddelay));
@ -120,6 +83,15 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_cfg.stepmotor_iglobalscaler), REG_SET(m_cfg.stepmotor_iglobalscaler)); PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_cfg.stepmotor_iglobalscaler), REG_SET(m_cfg.stepmotor_iglobalscaler));
PROCESS_REG(kreg_step_motor_in_debug_mode, REG_GET(m_state.debugmode), REG_SET(m_state.debugmode)); PROCESS_REG(kreg_step_motor_in_debug_mode, REG_GET(m_state.debugmode), REG_SET(m_state.debugmode));
PROCESS_REG(kreg_step_motor_vstart, REG_GET(m_cfg.motor_vstart), REG_SET(m_cfg.motor_vstart));
PROCESS_REG(kreg_step_motor_a1, REG_GET(m_cfg.motor_a1), REG_SET(m_cfg.motor_a1));
PROCESS_REG(kreg_step_motor_amax, REG_GET(m_cfg.motor_amax), REG_SET(m_cfg.motor_amax));
PROCESS_REG(kreg_step_motor_v1, REG_GET(m_cfg.motor_v1), REG_SET(m_cfg.motor_v1));
PROCESS_REG(kreg_step_motor_dmax, REG_GET(m_cfg.motor_dmax), REG_SET(m_cfg.motor_dmax));
PROCESS_REG(kreg_step_motor_d1, REG_GET(m_cfg.motor_d1), REG_SET(m_cfg.motor_d1));
PROCESS_REG(kreg_step_motor_vstop, REG_GET(m_cfg.motor_vstop), REG_SET(m_cfg.motor_vstop));
PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait));
default: default:
return err::kmodule_not_find_config_index; return err::kmodule_not_find_config_index;
break; break;
@ -160,6 +132,16 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() {
m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator);
m_stepM1->setMotorShaft(m_cfg.motor_shaft); m_stepM1->setMotorShaft(m_cfg.motor_shaft);
m_stepM1->setGlobalScale(m_cfg.stepmotor_iglobalscaler); m_stepM1->setGlobalScale(m_cfg.stepmotor_iglobalscaler);
m_stepM1->set_vstart(m_cfg.motor_vstart);
m_stepM1->set_a1(m_cfg.motor_a1);
m_stepM1->set_amax(m_cfg.motor_amax);
m_stepM1->set_v1(m_cfg.motor_v1);
m_stepM1->set_dmax(m_cfg.motor_dmax);
m_stepM1->set_d1(m_cfg.motor_d1);
m_stepM1->set_vstop(m_cfg.motor_vstop);
m_stepM1->set_tzerowait(m_cfg.motor_tzerowait);
// stepmotor_iglobalscaler // stepmotor_iglobalscaler
if (m_state.enable) { if (m_state.enable) {
m_stepM1->enable(true); m_stepM1->enable(true);
@ -405,10 +387,10 @@ bool StepMotorCtrlModule::check_when_run() {
bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) { bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) {
int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed; int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed;
int32_t runToPointDec = m_cfg.motor_run_to_zero_dec;
int32_t runToPointDec = m_cfg.motor_amax;
int32_t lookPointVelocity = m_cfg.motor_look_zero_edge_speed; int32_t lookPointVelocity = m_cfg.motor_look_zero_edge_speed;
int32_t lookPointDec = m_cfg.motor_look_zero_edge_dec;
int32_t lookPointDec = m_cfg.motor_amax;
direction = direction >= 0 ? 1 : -1; direction = direction >= 0 ? 1 : -1;
@ -417,7 +399,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
if (!gpio->getState()) { if (!gpio->getState()) {
ZLOGI(TAG, "---------STEP1-------- move to point"); ZLOGI(TAG, "---------STEP1-------- move to point");
_rotate(runToPointSpeed * direction, runToPointDec, runToPointDec);
_rotate(runToPointSpeed * direction, runToPointDec);
bool xreach_io = false; bool xreach_io = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -448,7 +430,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
// 如果设备已经在零点，则反向移动一定距离远离零点 // 如果设备已经在零点，则反向移动一定距离远离零点
if (gpio->getState()) { if (gpio->getState()) {
ZLOGI(TAG, "---------STEP2-------- find edge"); ZLOGI(TAG, "---------STEP2-------- find edge");
_rotate(-direction * lookPointVelocity, lookPointDec, lookPointDec);
_rotate(-direction * lookPointVelocity, lookPointDec);
bool reach_edge = false; bool reach_edge = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -477,7 +459,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
// 如果设备已经在零点，则反向移动一定距离远离零点 // 如果设备已经在零点，则反向移动一定距离远离零点
if (!gpio->getState()) { if (!gpio->getState()) {
ZLOGI(TAG, "---------STEP3-------- bak to gpio trigger edge"); ZLOGI(TAG, "---------STEP3-------- bak to gpio trigger edge");
_rotate(direction * lookPointVelocity, lookPointDec, lookPointDec);
_rotate(direction * lookPointVelocity, lookPointDec);
bool reach_edge = false; bool reach_edge = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -512,11 +494,9 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
return true; return true;
} }
void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) {
ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec);
m_stepM1->setAcceleration(acc);
m_stepM1->setDeceleration(dec);
m_stepM1->rotate(vel);
void StepMotorCtrlModule::_rotate(int32_t velocity, int32_t acc) {
m_stepM1->set_amax(acc);
m_stepM1->rotate(velocity);
} }
/*********************************************************************************************************************** /***********************************************************************************************************************
@ -619,9 +599,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
[this, direction]() { [this, direction]() {
befor_motor_move(); befor_motor_move();
{ {
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
m_stepM1->set_amax(m_cfg.motor_amax);
int32_t vel = m_cfg.motor_default_velocity; int32_t vel = m_cfg.motor_default_velocity;
if (direction <= 0) vel = -vel; if (direction <= 0) vel = -vel;
m_stepM1->rotate(vel); m_stepM1->rotate(vel);
@ -647,8 +625,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) {
befor_motor_move(); befor_motor_move();
{ {
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
m_stepM1->setAcceleration(m_cfg.motor_amax);
int32_t motor_pos = 0; int32_t motor_pos = 0;
inter_forward_kinematics(tox, motor_pos); inter_forward_kinematics(tox, motor_pos);
m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity); m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity);

22
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -17,18 +17,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t stepmotor_irun; int32_t stepmotor_irun;
int32_t stepmotor_iholddelay; int32_t stepmotor_iholddelay;
int32_t stepmotor_iglobalscaler; int32_t stepmotor_iglobalscaler;
int32_t motor_default_acc;
int32_t motor_default_dec;
int32_t motor_default_velocity; int32_t motor_default_velocity;
int32_t min_d; int32_t min_d;
int32_t max_d; int32_t max_d;
//
int32_t motor_run_to_zero_speed; int32_t motor_run_to_zero_speed;
int32_t motor_run_to_zero_dec;
//
int32_t motor_look_zero_edge_speed; int32_t motor_look_zero_edge_speed;
int32_t motor_look_zero_edge_dec;
//
int32_t motor_vstart; int32_t motor_vstart;
int32_t motor_a1; int32_t motor_a1;
int32_t motor_amax; int32_t motor_amax;
@ -37,7 +30,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t motor_d1; int32_t motor_d1;
int32_t motor_vstop; int32_t motor_vstop;
int32_t motor_tzerowait; int32_t motor_tzerowait;
} config_t; } config_t;
typedef struct { typedef struct {
@ -49,10 +41,10 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
} state_t; } state_t;
private: private:
IStepperMotor* m_stepM1 = nullptr;
int m_id = 0;
config_t m_cfg;
state_t m_state = {0};
TMC51X0* m_stepM1 = nullptr;
int m_id = 0;
config_t m_cfg;
state_t m_state = {0};
ZGPIO* m_iotable = nullptr; ZGPIO* m_iotable = nullptr;
int m_nio = 0; int m_nio = 0;
@ -60,7 +52,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
ZThread m_thread; ZThread m_thread;
public: public:
void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg);
void initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg);
static void create_default_cfg(config_t& cfg); static void create_default_cfg(config_t& cfg);
@ -127,7 +119,9 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
bool check_when_run(); bool check_when_run();
bool exec_move_to_io_task(int32_t ioindex, int32_t direction); bool exec_move_to_io_task(int32_t ioindex, int32_t direction);
void _rotate(int32_t vel, int32_t acc, int32_t dec);
// void _rotate(int32_t vel, int32_t acc, int32_t dec);
void _rotate(int32_t velocity, int32_t acc);
int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val); int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val);

4
components/tmc/basic/tmc_ic_interface.hpp
View File

@ -50,6 +50,10 @@ class IStepperMotor {
virtual bool isStoped() = 0; // 是否停止运动 virtual bool isStoped() = 0; // 是否停止运动
virtual void setNoAccLimit(bool enable) = 0; virtual void setNoAccLimit(bool enable) = 0;
}; };
} // namespace iflytop } // namespace iflytop

9
components/tmc/ic/ztmc5130.cpp
View File

@ -87,6 +87,15 @@ void TMC51X0::setNoAccLimit(bool enable) {
} }
} }
void TMC51X0::set_vstart(int32_t vel) { writeInt(TMC5130_VSTART, to_motor_vel(vel)); }
void TMC51X0::set_a1(int32_t acc) { writeInt(TMC5130_A1, to_motor_acc(acc)); }
void TMC51X0::set_amax(int32_t acc) { writeInt(TMC5130_AMAX, to_motor_acc(acc)); }
void TMC51X0::set_v1(int32_t vel) { writeInt(TMC5130_V1, to_motor_vel(vel)); }
void TMC51X0::set_dmax(int32_t acc) { writeInt(TMC5130_DMAX, to_motor_acc(acc)); }
void TMC51X0::set_d1(int32_t acc) { writeInt(TMC5130_D1, to_motor_acc(acc)); }
void TMC51X0::set_vstop(int32_t vel) { writeInt(TMC5130_VSTOP, to_motor_vel(vel)); }
void TMC51X0::set_tzerowait(int32_t val) { writeInt(TMC5130_TZEROWAIT, val); }
void TMC51X0::enable(bool enable) { void TMC51X0::enable(bool enable) {
// m_port->TMC5130Port_setENNPinState(m_channel, !enable); // m_port->TMC5130Port_setENNPinState(m_channel, !enable);
SET_PIN(m_ennpin, !enable); SET_PIN(m_ennpin, !enable);

19
components/tmc/ic/ztmc5130.hpp
View File

@ -14,6 +14,16 @@ extern "C" {
namespace iflytop { namespace iflytop {
#define TMC5130_LISTENER_MAX 5 #define TMC5130_LISTENER_MAX 5
/**
* @brief
* 注意事项
*
* 1. 6点加速控制只在位置模式下有效，在速度模式下无效
* 2. 速度模式下只有AMAX有效
*
*/
class Tmc5130RampStat { class Tmc5130RampStat {
public: public:
uint32_t m_state; uint32_t m_state;
@ -125,6 +135,15 @@ class TMC51X0 : public IStepperMotor {
virtual bool isStoped() { return isReachTarget(); } virtual bool isStoped() { return isReachTarget(); }
virtual void setNoAccLimit(bool enable) override; virtual void setNoAccLimit(bool enable) override;
virtual void set_vstart(int32_t val);
virtual void set_a1(int32_t val);
virtual void set_amax(int32_t val);
virtual void set_v1(int32_t val);
virtual void set_dmax(int32_t val);
virtual void set_d1(int32_t val);
virtual void set_vstop(int32_t val);
virtual void set_tzerowait(int32_t val);
public: public:
DevStatusReg_t getDevStatus() { // R 读后不清 DevStatusReg_t getDevStatus() { // R 读后不清
static_assert(sizeof(DevStatusReg_t) == 4); static_assert(sizeof(DevStatusReg_t) == 4);

Write Preview
Loading…
Cancel
Save