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

update

master
zhaohe 1 year ago
parent
d0d15a7813
commit
edde3d10c3
6 changed files with 100 additions and 14 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
      a8000_protocol
  2. 97
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  3. 6
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  4. 6
      sdk/components/zcancmder/zcan_protocol_parser.cpp
  5. 1
      sdk/components/zcancmder/zcan_protocol_parser.hpp
  6. 2
      usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h

2
a8000_protocol

@ -1 +1 @@
Subproject commit 5598b81844f8ca2e8373db46455e35aa6a5ae4f4
Subproject commit 826f05973b88e7f311373d9f7d1ce74a2554512f

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

@ -112,6 +112,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc), REG_SET(m_cfg.motor_enable_enc));
PROCESS_REG(kreg_step_motor_dzero_pos, REG_GET(m_cfg.motor_dzero), update_dzero(m_cfg.motor_dzero));
PROCESS_REG(kret_step_motor_pos_devi_tolerance, REG_GET(m_cfg.pos_devi_tolerance), update_dzero(m_cfg.pos_devi_tolerance));
default:
return err::kmodule_not_find_reg;
break;
@ -121,7 +123,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) {
ZLOGI(TAG, "m%d motor_enable %ld", m_id, enable);
m_stepM1->stop();
m_thread.stop();
m_stepM1->enable(enable);
m_state.enable = enable;
return 0;
@ -505,7 +507,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp
ZLOGI(TAG, "reciprocating_motion move pos:%d", x_startpos);
moveTo(x_startpos, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) {
osDelay(50);
osDelay(20);
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
@ -514,7 +516,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp
ZLOGI(TAG, "reciprocating_motion move pos:%d", x_endpos);
moveTo(x_endpos, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) {
osDelay(50);
osDelay(20);
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
@ -527,7 +529,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
osDelay(50);
osDelay(20);
}
after_motor_move();
@ -549,7 +551,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
while (true) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
osDelay(50);
osDelay(20);
}
}
@ -567,15 +569,13 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) {
[this, tox]() {
befor_motor_move();
{
do {
int32_t expectation_dpos = tox - getnowpos();
moveTo(tox, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
if (!waitforstop(nullptr)) break;
osDelay(50);
}
}
checkdpos(expectation_dpos);
} while (false);
after_motor_move();
},
@ -628,6 +628,47 @@ 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); }
void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) {
int32_t dpos = getnowpos() - m_state.before_move_pos;
if (m_cfg.pos_devi_tolerance != 0 && (abs(expect_dpos - dpos) > m_cfg.pos_devi_tolerance)) {
ZLOGE(TAG, "check pos devi tolerance fail.... dpos%d,expectation_dpos:%d", dpos, expect_dpos);
setErrorFlag(err::kstep_motor_lost_step);
} else {
ZLOGE(TAG, "dpos%d,expect_dpos:%d", dpos, expect_dpos);
}
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() {
//
ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", m_id);
m_thread.stop();
creg.module_status = 1;
m_thread.start(
[this]() {
befor_motor_move();
bool moveToZero = false;
do {
// 期望偏差
int32_t expectation_dpos = 0 + m_cfg.motor_dzero - getnowpos();
// 快速移动到零点
moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity);
// 等待移动到目标点
if (!waitforstop([this]() { return m_iotable[0].getState(); })) break;
// 移动到零点
if (!exec_move_to_io_task(0, -1)) break;
moveToZero = true;
// 校验偏差
checkdpos(expectation_dpos);
} while (false);
after_motor_move();
if (moveToZero) setnowpos(0 + m_cfg.motor_dzero);
},
[this]() { m_stepM1->stop(); });
return 0;
}
//
int32_t StepMotorCtrlModule::getnowpos(int32_t& x) {
if (m_cfg.motor_enable_enc == 0) {
@ -637,6 +678,12 @@ int32_t StepMotorCtrlModule::getnowpos(int32_t& x) {
x = m_stepM1->read_enc_val();
return 0;
}
int32_t StepMotorCtrlModule::getnowpos() {
int32_t x;
getnowpos(x);
return x;
}
int32_t StepMotorCtrlModule::trysyncpos() {
if (m_cfg.motor_enable_enc == 0) {
return 0;
@ -670,3 +717,29 @@ int32_t StepMotorCtrlModule::update_dzero(int32_t dzero) {
setnowpos(nowabs + m_cfg.motor_dzero);
return 0;
}
bool StepMotorCtrlModule::waitforstop(function<bool()> checkstop, int checkperiod) {
bool suc = true;
while (true) {
if (m_thread.getExitFlag() || !check_when_run()) {
suc = false;
break;
}
if (checkstop && checkstop()) {
break;
}
if (m_stepM1->isReachTarget()) {
break;
}
osDelay(checkperiod);
}
m_stepM1->stop();
while (m_thread.getExitFlag()) {
if (m_stepM1->isReachTarget()) break;
osDelay(checkperiod);
}
return suc;
}

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

@ -33,6 +33,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t motor_enc_resolution;
int32_t motor_enable_enc;
int32_t motor_dzero; // 零点偏移
int32_t pos_devi_tolerance;
} config_t;
typedef struct {
@ -99,6 +100,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override;
virtual int32_t step_motor_easy_move_to_zero_point_quick() override;
public:
TMC51X0* getMotor() { return m_stepM1; }
config_t* getCfg() { return &m_cfg; }
@ -131,6 +134,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
void setErrorFlag(int32_t ecode);
int32_t getnowpos(int32_t& x);
int32_t getnowpos();
int32_t trysyncpos();
int32_t setnowpos(int32_t x);
int32_t moveTo(int32_t x, int32_t v);
@ -138,6 +142,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t rotate(int32_t direction, int32_t v);
int32_t update_dzero(int32_t dzero);
bool waitforstop(function<bool()> checkstop, int checkperiod = 20);
void checkdpos(int32_t expect_dpos);
// int32_t read_enc_val(int32_t& enc_val);
// int32_t write_enc_val(int32_t enc_val);

6
sdk/components/zcancmder/zcan_protocol_parser.cpp
View File

@ -42,6 +42,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
REGFN(step_motor_read_io_index_in_stm32);
REGFN(step_motor_set_subdevice_reg);
REGFN(step_motor_get_subdevice_reg);
REGFN(step_motor_easy_move_to_zero_point_quick);
REGFN(mini_servo_enable);
REGFN(mini_servo_read_pos);
@ -361,6 +362,11 @@ int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) {
return module->step_motor_get_subdevice_reg(cxt->params[0], ack);
}
int32_t ZCanProtocolParser::step_motor_easy_move_to_zero_point_quick(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(0);
return module->step_motor_easy_move_to_zero_point_quick();
}
#undef MODULE_CLASS
#define MODULE_CLASS ZIMiniServo
// virtual int32_t mini_servo_enable(int32_t enable) = 0;

1
sdk/components/zcancmder/zcan_protocol_parser.hpp
View File

@ -78,6 +78,7 @@ class ZCanProtocolParser : public IZCanReceiverListener {
CMDFN(step_motor_set_subdevice_reg);
CMDFN(step_motor_get_subdevice_reg);
CMDFN(step_motor_easy_reciprocating_motion);
CMDFN(step_motor_easy_move_to_zero_point_quick);
CMDFN(mini_servo_enable);
CMDFN(mini_servo_read_pos);

2
usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h
View File

@ -72,7 +72,7 @@
#define MOTOR2_REFR_MIRROR true
#define MOTOR2_VSTART 100
#define MOTOR2_A1 300
#define MOTOR2_A1 100
#define MOTOR2_V1 800
#define MOTOR2_AMAX 500

Write Preview
Loading…
Cancel
Save