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
7321486341
commit
b457781a95
5 changed files with 66 additions and 116 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. 142
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 27
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  3. 5
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  4. 7
      components/zcancmder/zcan_protocol_parser.cpp
  5. 1
      components/zcancmder/zcan_protocol_parser.hpp

142
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -80,9 +80,7 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
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) {
switch (param_id) { switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB(); MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_step_motor_pos, inter_get_pos(val), ACTION_NONE);
PROCESS_REG(kreg_step_motor_enc_val, read_enc_val(val), write_enc_val(val));
PROCESS_REG(kreg_step_motor_denc_val, REG_GET(m_state.denc), ACTION_NONE);
PROCESS_REG(kreg_step_motor_pos, getnowpos(val), ACTION_NONE);
PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE); PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE);
PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft)); PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft));
@ -108,7 +106,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
PROCESS_REG(kreg_step_motor_vstop, REG_GET(m_cfg.motor_vstop), REG_SET(m_cfg.motor_vstop)); 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)); PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait));
PROCESS_REG(kreg_step_motor_enc_resolution, REG_GET(m_cfg.motor_enc_resolution), REG_SET(m_cfg.motor_enc_resolution)); PROCESS_REG(kreg_step_motor_enc_resolution, REG_GET(m_cfg.motor_enc_resolution), REG_SET(m_cfg.motor_enc_resolution));
PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc_resolution), REG_SET(m_cfg.motor_enable_enc_resolution));
PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc), REG_SET(m_cfg.motor_enable_enc));
default: default:
return err::kmodule_not_find_reg; return err::kmodule_not_find_reg;
@ -123,11 +121,7 @@ int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) {
m_state.enable = enable; m_state.enable = enable;
return 0; return 0;
} }
int32_t StepMotorCtrlModule::step_motor_read_pos(int32_t* pos) {
int32_t motor_pos = m_stepM1->getXACTUAL();
inter_inverse_kinematics(motor_pos, *pos);
return 0;
}
int32_t StepMotorCtrlModule::step_motor_read_pos(int32_t* pos) { return getnowpos(*pos); }
int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) { int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) {
ZLOGI(TAG, "m%d motor_stop %d", m_id, breakstop); ZLOGI(TAG, "m%d motor_stop %d", m_id, breakstop);
m_thread.stop(); m_thread.stop();
@ -135,13 +129,7 @@ int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) {
return 0; return 0;
} }
int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) {
int32_t motor_pos = 0;
inter_forward_kinematics(pos, motor_pos);
m_stepM1->setXACTUAL(motor_pos);
m_stepM1->set_enc_val(motor_pos);
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { return setnowpos(pos); }
int32_t StepMotorCtrlModule::step_motor_active_cfg() { int32_t StepMotorCtrlModule::step_motor_active_cfg() {
ZLOGI(TAG, "m%d motor_active_cfg", m_id); ZLOGI(TAG, "m%d motor_active_cfg", m_id);
@ -226,32 +214,16 @@ int32_t StepMotorCtrlModule::step_motor_read_io_index_in_stm32(int32_t ioindex,
/*********************************************************************************************************************** /***********************************************************************************************************************
* INTER * * INTER *
***********************************************************************************************************************/ ***********************************************************************************************************************/
void StepMotorCtrlModule::inter_inverse_kinematics(int32_t motor_pos, int32_t& x) { x = motor_pos; }
void StepMotorCtrlModule::inter_forward_kinematics(int32_t x, int32_t& motor_pos) { motor_pos = x; }
int StepMotorCtrlModule::inter_get_pos() {
int32_t pos;
int32_t motor_pos = m_stepM1->getXACTUAL();
inter_inverse_kinematics(motor_pos, pos);
return pos;
}
int StepMotorCtrlModule::inter_get_pos(int32_t& x) {
x = inter_get_pos();
return 0;
}
void StepMotorCtrlModule::befor_motor_move() { //
m_state.before_move_pos = m_stepM1->getXACTUAL();
m_state.before_move_enc = m_stepM1->read_enc_val();
creg.module_status = 1;
creg.module_errorcode = 0;
//
void StepMotorCtrlModule::befor_motor_move() { //
getnowpos(m_state.before_move_pos);
creg.module_status = 1;
creg.module_errorcode = 0;
m_stepM1->enable(1); m_stepM1->enable(1);
} }
void StepMotorCtrlModule::after_motor_move() { void StepMotorCtrlModule::after_motor_move() {
m_state.after_move_pos = m_stepM1->getXACTUAL();
m_state.after_move_enc = m_stepM1->read_enc_val();
m_state.dpos = m_state.after_move_pos - m_state.before_move_pos;
m_state.denc = m_state.after_move_enc - m_state.before_move_enc;
getnowpos(m_state.after_move_pos);
m_state.dpos = m_state.after_move_pos - m_state.before_move_pos;
if (creg.module_errorcode != 0) { if (creg.module_errorcode != 0) {
creg.module_status = 2; creg.module_status = 2;
} }
@ -275,22 +247,6 @@ void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) {
void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode = ecode; } void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode = ecode; }
int32_t StepMotorCtrlModule::read_enc_val(int32_t& enc_val) {
auto tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) {
tmc5130->read_enc_val(enc_val);
return 0;
}
return err::kcmd_not_support;
}
int32_t StepMotorCtrlModule::write_enc_val(int32_t enc_val) {
auto tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) {
tmc5130->set_enc_val(enc_val);
return 0;
}
return err::kcmd_not_support;
}
bool StepMotorCtrlModule::check_when_run() { bool StepMotorCtrlModule::check_when_run() {
// //
if (m_state.debugmode) return true; if (m_state.debugmode) return true;
@ -320,11 +276,8 @@ 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 runToPointDec = m_cfg.motor_amax;
int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed;
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_amax;
direction = direction >= 0 ? 1 : -1; direction = direction >= 0 ? 1 : -1;
@ -333,7 +286,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);
rotate(direction, runToPointSpeed);
bool xreach_io = false; bool xreach_io = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -364,7 +317,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);
rotate(-direction, lookPointVelocity);
bool reach_edge = false; bool reach_edge = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -393,7 +346,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);
rotate(direction, lookPointVelocity);
bool reach_edge = false; bool reach_edge = false;
while (!m_thread.getExitFlag()) { while (!m_thread.getExitFlag()) {
@ -428,16 +381,6 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
return true; return true;
} }
void StepMotorCtrlModule::_rotate(int32_t velocity, int32_t acc) {
if (velocity > 0) {
m_stepM1->moveToEnd(1, abs(velocity));
// m_stepM1->rotate(velocity);
} else {
// m_stepM1->rotate(velocity);
m_stepM1->moveToEnd(-1, abs(velocity));
}
}
/*********************************************************************************************************************** /***********************************************************************************************************************
* 电机控制接口 * * 电机控制接口 *
***********************************************************************************************************************/ ***********************************************************************************************************************/
@ -531,15 +474,6 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() {
} }
return do_step_motor_easy_move_to_end_point(); return do_step_motor_easy_move_to_end_point();
} }
int32_t StepMotorCtrlModule::step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) {
*pos = m_stepM1->getXACTUAL();
if (m_cfg.motor_enable_enc_resolution != 0) {
*encpos = m_stepM1->read_enc_val();
} else {
*encpos = *pos;
}
return 0;
}
int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
m_thread.stop(); m_thread.stop();
@ -547,12 +481,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
[this, direction]() { [this, direction]() {
befor_motor_move(); befor_motor_move();
{ {
// m_stepM1->set_amax(m_cfg.motor_amax);
// int32_t vel = m_cfg.motor_default_velocity;
// if (direction <= 0) vel = -vel;
// m_stepM1->rotate(vel);
m_stepM1->moveToEnd(direction, m_cfg.motor_default_velocity);
rotate(direction, m_cfg.motor_default_velocity);
while (true) { while (true) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
@ -575,10 +504,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) {
befor_motor_move(); befor_motor_move();
{ {
int32_t motor_pos = 0;
inter_forward_kinematics(tox, motor_pos);
m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity);
moveTo(tox, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) { while (!m_stepM1->isReachTarget()) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
if (!check_when_run()) break; if (!check_when_run()) break;
@ -608,8 +534,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() {
befor_motor_move(); befor_motor_move();
exec_move_to_io_task(0, -1); exec_move_to_io_task(0, -1);
after_motor_move(); after_motor_move();
m_stepM1->setXACTUAL(0);
m_stepM1->set_enc_val(0);
setnowpos(0);
}, },
[this]() { m_stepM1->stop(); }); [this]() { m_stepM1->stop(); });
return 0; return 0;
@ -636,3 +561,36 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int3
} }
int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_end_point() { return do_step_motor_easy_move_to_io(1, 1); } int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_end_point() { return do_step_motor_easy_move_to_io(1, 1); }
//
int32_t StepMotorCtrlModule::getnowpos(int32_t& x) {
if (m_cfg.motor_enable_enc == 0) {
x = m_stepM1->getXACTUAL();
return 0;
}
x = m_stepM1->read_enc_val();
return 0;
}
int32_t StepMotorCtrlModule::trysyncpos() {
if (m_cfg.motor_enable_enc == 0) {
return 0;
}
int32_t x = m_stepM1->read_enc_val();
m_stepM1->setXACTUAL(x);
return 0;
}
int32_t StepMotorCtrlModule::setnowpos(int32_t x) {
m_stepM1->setXACTUAL(x);
m_stepM1->set_enc_val(x);
return 0;
}
int32_t StepMotorCtrlModule::moveTo(int32_t x, int32_t v) {
trysyncpos();
m_stepM1->moveTo(x, v);
return 0;
}
int32_t StepMotorCtrlModule::rotate(int32_t direction, int32_t v) {
m_stepM1->moveToEnd(direction, m_cfg.motor_default_velocity);
return 0;
}

27
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -31,16 +31,13 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t motor_vstop; int32_t motor_vstop;
int32_t motor_tzerowait; int32_t motor_tzerowait;
int32_t motor_enc_resolution; int32_t motor_enc_resolution;
int32_t motor_enable_enc_resolution;
int32_t motor_enable_enc;
} config_t; } config_t;
typedef struct { typedef struct {
int32_t dpos; int32_t dpos;
int32_t denc;
int32_t after_move_pos; int32_t after_move_pos;
int32_t before_move_pos; int32_t before_move_pos;
int32_t after_move_enc;
int32_t before_move_enc;
int32_t enable; int32_t enable;
int32_t debugmode; int32_t debugmode;
} state_t; } state_t;
@ -87,7 +84,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t step_motor_easy_move_to_zero() override; virtual int32_t step_motor_easy_move_to_zero() override;
virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override;
virtual int32_t step_motor_easy_move_to_end_point() override; virtual int32_t step_motor_easy_move_to_end_point() override;
virtual int32_t step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) override;
virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override; virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override;
virtual int32_t step_motor_active_cfg() override; virtual int32_t step_motor_active_cfg() override;
@ -96,9 +92,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t step_motor_read_tmc5130_status(int32_t* errorflag) override; virtual int32_t step_motor_read_tmc5130_status(int32_t* errorflag) override;
virtual int32_t step_motor_read_tmc5130_state(int32_t* status) override; virtual int32_t step_motor_read_tmc5130_state(int32_t* status) override;
// virtual int32_t step_motor_read_enc_val(int32_t* enc_val) override;
// virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution) override;
// virtual int32_t step_motor_get_enc_resolution(int32_t* enc_resolution) override;
virtual int32_t step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) override; virtual int32_t step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) override;
virtual int32_t step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) override; virtual int32_t step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) override;
@ -119,11 +112,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t do_step_motor_easy_move_to_end_point(); int32_t do_step_motor_easy_move_to_end_point();
private: private:
void inter_inverse_kinematics(int32_t motor_pos, int32_t& x);
void inter_forward_kinematics(int32_t x, int32_t& motor_pos);
int inter_get_pos();
int inter_get_pos(int32_t& x);
int inter_et_pos(int32_t& x);
void befor_motor_move(); void befor_motor_move();
void after_motor_move(); void after_motor_move();
@ -132,7 +122,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
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);
@ -140,8 +129,16 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status); void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
void setErrorFlag(int32_t ecode); void setErrorFlag(int32_t ecode);
int32_t read_enc_val(int32_t& enc_val);
int32_t write_enc_val(int32_t enc_val);
int32_t getnowpos(int32_t& x);
int32_t trysyncpos();
int32_t setnowpos(int32_t x);
int32_t moveTo(int32_t x, int32_t v);
int32_t moveBy(int32_t dx, int32_t v);
int32_t rotate(int32_t direction, int32_t v);
// 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); // virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution);
}; };

5
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -533,4 +533,7 @@ int32_t XYRobotCtrlModule::xymotor_read_inio_index_in_stm32(int32_t ioindex, int
*val = m_gpiotable[ioindex].getPin(); *val = m_gpiotable[ioindex].getPin();
return 0; return 0;
} }
int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) { setnowpos(x, y); }
int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) {
setnowpos(x, y);
return 0;
}

7
components/zcancmder/zcan_protocol_parser.cpp
View File

@ -41,7 +41,6 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
REGFN(step_motor_read_io_index_in_stm32); REGFN(step_motor_read_io_index_in_stm32);
REGFN(step_motor_set_subdevice_reg); REGFN(step_motor_set_subdevice_reg);
REGFN(step_motor_get_subdevice_reg); REGFN(step_motor_get_subdevice_reg);
REGFN(step_motor_read_pos_and_enc_pos);
REGFN(mini_servo_enable); REGFN(mini_servo_enable);
REGFN(mini_servo_read_pos); REGFN(mini_servo_read_pos);
@ -339,12 +338,6 @@ int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) {
cxt->acklen = 4; cxt->acklen = 4;
return module->step_motor_get_subdevice_reg(cxt->params[0], ack); return module->step_motor_get_subdevice_reg(cxt->params[0], ack);
} }
int32_t ZCanProtocolParser::step_motor_read_pos_and_enc_pos(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(0);
int32_t* ack = (int32_t*)cxt->ackbuf;
cxt->acklen = 8;
return module->step_motor_read_pos_and_enc_pos(&ack[0], &ack[1]);
}
#undef MODULE_CLASS #undef MODULE_CLASS
#define MODULE_CLASS ZIMiniServo #define MODULE_CLASS ZIMiniServo

1
components/zcancmder/zcan_protocol_parser.hpp
View File

@ -77,7 +77,6 @@ class ZCanProtocolParser : public IZCanReceiverListener {
CMDFN(step_motor_read_io_index_in_stm32); CMDFN(step_motor_read_io_index_in_stm32);
CMDFN(step_motor_set_subdevice_reg); CMDFN(step_motor_set_subdevice_reg);
CMDFN(step_motor_get_subdevice_reg); CMDFN(step_motor_get_subdevice_reg);
CMDFN(step_motor_read_pos_and_enc_pos);
CMDFN(mini_servo_enable); CMDFN(mini_servo_enable);
CMDFN(mini_servo_read_pos); CMDFN(mini_servo_read_pos);

Write Preview
Loading…
Cancel
Save