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

v1101| 步进电机支持多个默认速度

master
zhaohe 4 months ago
parent
5acf652aec
commit
1290762877
6 changed files with 94 additions and 40 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. 72
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  3. 23
      sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  4. 30
      sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp
  5. 5
      sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp
  6. 2
      usrc/version.h

2
a8000_protocol

@ -1 +1 @@
Subproject commit 590c89179513d49d36fd9ad9cb678e3d9e34be7a
Subproject commit ffd944277ed3f6f63e01d3ab2ea1c14d01fec752

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

@ -108,6 +108,9 @@ int32_t StepMotorCtrlModule::module_set_reg(int32_t regindex, int32_t val) {
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_one_circle_pulse, (m_cfg.motor_one_circle_pulse));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_one_circle_pulse_denominator, (m_cfg.motor_one_circle_pulse_denominator));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_default_velocity, (m_cfg.motor_default_velocity));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_low_velocity, (m_cfg.motor_low_velocity));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_mid_velocity, (m_cfg.motor_mid_velocity));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_high_velocity, (m_cfg.motor_high_velocity));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_run_to_zero_speed, (m_cfg.motor_run_to_zero_speed));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_look_zero_edge_speed, (m_cfg.motor_look_zero_edge_speed));
MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_ihold, (m_cfg.stepmotor_ihold));
@ -183,6 +186,9 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t regindex, int32_t* val) {
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_one_circle_pulse, m_cfg.motor_one_circle_pulse);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_one_circle_pulse_denominator, m_cfg.motor_one_circle_pulse_denominator);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_default_velocity, m_cfg.motor_default_velocity);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_low_velocity, (m_cfg.motor_low_velocity));
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_mid_velocity, (m_cfg.motor_mid_velocity));
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_high_velocity, (m_cfg.motor_high_velocity));
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_run_to_zero_speed, m_cfg.motor_run_to_zero_speed);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_look_zero_edge_speed, m_cfg.motor_look_zero_edge_speed);
MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_ihold, m_cfg.stepmotor_ihold);
@ -573,21 +579,26 @@ int32_t StepMotorCtrlModule::check_befor_run() {
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) {
int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { return step_motor_rotate(direction, 0); }
int32_t StepMotorCtrlModule::step_motor_rotate(int32_t direction, int32_t speedlevel) {
ZLOGI(TAG, "m%d motor_rotate %d", module_id, direction);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_rotate(direction);
return do_step_motor_easy_rotate(direction, speedlevel);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) {
int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { return step_motor_move_by(distance, 0); }
int32_t StepMotorCtrlModule::step_motor_move_by(int32_t distance, int32_t speedlevel) {
ZLOGI(TAG, "m%d motor_move_by %d", module_id, distance);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_move_by(distance);
return do_step_motor_easy_move_by(distance, speedlevel);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) {
int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { return step_motor_move_to(tox, 0); }
int32_t StepMotorCtrlModule::step_motor_move_to(int32_t tox, int32_t speedlevel) {
ZLOGI(TAG, "m%d motor_move_to %d", module_id, tox);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
@ -597,8 +608,9 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) {
}
if (m_cfg.min_d != 0 && tox < m_cfg.min_d) tox = m_cfg.min_d;
if (m_cfg.max_d != 0 && tox > m_cfg.max_d) tox = m_cfg.max_d;
return do_step_motor_easy_move_to(tox);
return do_step_motor_easy_move_to(tox, speedlevel);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() {
ZLOGI(TAG, "m%d motor_move_to_zero", module_id);
int32_t ecode = check_befor_run();
@ -696,15 +708,15 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp
return 0;
}
int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction, int32_t speedlevel) {
m_thread.stop();
module_status = 1;
m_thread.start(
[this, direction]() {
[this, direction, speedlevel]() {
befor_motor_move();
{
// rotate(direction, m_cfg.motor_default_velocity);
rotateInVelocityMode(direction >= 0 ? m_cfg.motor_default_velocity : -m_cfg.motor_default_velocity);
int32_t speed = getspeed(speedlevel);
rotateInVelocityMode(direction >= 0 ? speed : -speed);
while (true) {
if (m_thread.getExitFlag()) break;
@ -718,7 +730,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) {
int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance, int32_t speedlevel) {
int32_t motor_pos = 0;
step_motor_read_pos(&motor_pos);
motor_pos += distance;
@ -726,18 +738,18 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) {
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_move_to(motor_pos);
return do_step_motor_easy_move_to(motor_pos, speedlevel);
}
int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) {
int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox, int32_t speedlevel) {
m_thread.stop();
module_status = 1;
m_thread.start(
[this, tox]() {
[this, tox, speedlevel]() {
befor_motor_move();
do {
int32_t expectation_dpos = tox - getnowpos();
moveTo(tox, m_cfg.motor_default_velocity);
moveTo(tox, getspeed(speedlevel));
if (!waitforstop(nullptr)) break;
checkdpos(expectation_dpos);
@ -807,8 +819,8 @@ void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) {
ZLOGI(TAG, "dpos%d,expect_dpos:%d", dpos, expect_dpos);
}
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() {
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { return step_motor_move_to_zero_point_quick(0); }
int32_t StepMotorCtrlModule::step_motor_move_to_zero_point_quick(int32_t speedlevel) {
//
ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", module_id);
int32_t ecode = check_befor_run();
@ -821,20 +833,21 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() {
m_thread.stop();
module_status = 1;
m_thread.start(
[this]() {
[this, speedlevel]() {
befor_motor_move();
bool moveToZero = false;
bool moveToZero = false;
int32_t speed = getspeed(speedlevel);
do {
// 期望偏差
int32_t expectation_dpos = -m_cfg.io_trigger_append_distance + 0 + m_cfg.motor_dzero - getnowpos();
if (!m_iotable[0].getState()) {
// 快速移动到零点
moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity);
moveTo(0 + m_cfg.motor_dzero, speed);
// 等待移动到目标点
if (!waitforstop([this]() { return m_iotable[0].getState(); })) break;
} else {
// 快速移动到零点
moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity);
moveTo(0 + m_cfg.motor_dzero, speed);
// 等待移动到目标点
if (!waitforstop([this]() { return !m_iotable[0].getState(); })) break;
}
@ -853,6 +866,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() {
return 0;
}
//
int32_t StepMotorCtrlModule::getnowpos(int32_t& x) {
if (m_cfg.motor_enable_enc == 0) {
@ -933,3 +947,19 @@ bool StepMotorCtrlModule::waitforstop(function<bool()> checkstop, int checkperio
return suc;
}
int32_t StepMotorCtrlModule::getspeed(int32_t speedlevel) {
int32_t speed = m_cfg.motor_default_velocity;
if (speedlevel == 0) {
speed = m_cfg.motor_default_velocity;
} else if (speedlevel == 1) {
speed = m_cfg.motor_low_velocity;
} else if (speedlevel == 2) {
speed = m_cfg.motor_mid_velocity;
} else if (speedlevel == 3) {
speed = m_cfg.motor_high_velocity;
} else {
speed = m_cfg.motor_default_velocity;
}
return speed;
}

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

@ -22,6 +22,9 @@ class StepMotorCtrlModule : public ZIModule {
int32_t stepmotor_iglobalscaler;
int32_t stepmotor_mres;
int32_t motor_default_velocity;
int32_t motor_low_velocity;
int32_t motor_mid_velocity;
int32_t motor_high_velocity;
int32_t min_d;
int32_t max_d;
int32_t motor_run_to_zero_speed;
@ -72,8 +75,6 @@ class StepMotorCtrlModule : public ZIModule {
* ZIModule *
***********************************************************************************************************************/
virtual int32_t module_set_reg(int32_t regindex, int32_t val) override;
virtual int32_t module_get_reg(int32_t regindex, int32_t* val) override;
virtual int32_t module_reset_reg() override;
@ -93,6 +94,13 @@ class StepMotorCtrlModule : public ZIModule {
virtual int32_t step_motor_easy_rotate(int32_t direction);
virtual int32_t step_motor_easy_move_by(int32_t distance);
virtual int32_t step_motor_easy_move_to(int32_t position);
virtual int32_t step_motor_easy_move_to_zero_point_quick();
virtual int32_t step_motor_move_by(int32_t distance, int32_t speedlevel);
virtual int32_t step_motor_move_to(int32_t position, int32_t speedlevel);
virtual int32_t step_motor_move_to_zero_point_quick(int32_t speedlevel);
virtual int32_t step_motor_rotate(int32_t direction, int32_t speedlevel);
virtual int32_t step_motor_easy_move_to_zero();
virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction);
virtual int32_t step_motor_easy_move_to_end_point();
@ -110,8 +118,6 @@ class StepMotorCtrlModule : public ZIModule {
virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32);
virtual int32_t step_motor_easy_move_to_zero_point_quick();
public:
TMC51X0* getMotor() { return m_stepM1; }
config_t* getCfg() { return &m_cfg; }
@ -119,9 +125,9 @@ class StepMotorCtrlModule : public ZIModule {
private:
int32_t check_befor_run();
int32_t do_step_motor_easy_rotate(int32_t direction);
int32_t do_step_motor_easy_move_by(int32_t distance);
int32_t do_step_motor_easy_move_to(int32_t position);
int32_t do_step_motor_easy_rotate(int32_t direction, int32_t speedlevel);
int32_t do_step_motor_easy_move_by(int32_t distance, int32_t speedlevel);
int32_t do_step_motor_easy_move_to(int32_t position, int32_t speedlevel);
int32_t do_step_motor_easy_move_to_zero();
int32_t do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction);
int32_t do_step_motor_easy_move_to_end_point();
@ -137,7 +143,6 @@ class StepMotorCtrlModule : public ZIModule {
// void _rotate(int32_t vel, int32_t acc, int32_t dec);
private:
void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
void setErrorFlag(int32_t ecode);
@ -155,6 +160,8 @@ class StepMotorCtrlModule : public ZIModule {
bool waitforstop(function<bool()> checkstop, int checkperiod = 20);
void checkdpos(int32_t expect_dpos);
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);

30
sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp
View File

@ -50,8 +50,6 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) {
REGFN(module_get_type);
REGFN(module_get_status);
REGFN(step_motor_enable);
REGFN(step_motor_read_pos);
REGFN(step_motor_read_enc_pos);
@ -73,6 +71,10 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) {
REGFN(step_motor_set_subdevice_reg);
REGFN(step_motor_get_subdevice_reg);
REGFN(step_motor_easy_move_to_zero_point_quick);
REGFN(step_motor_move_by);
REGFN(step_motor_move_to);
REGFN(step_motor_move_to_zero_point_quick);
REGFN(step_motor_rotate);
REGFN(mini_servo_enable);
REGFN(mini_servo_read_pos);
@ -176,9 +178,7 @@ void ZCanProtocolParser::initialize(ZCanReceiver* cancmder) {
REGFN(pipette_zmotor_read_dev_status_cache);
}
void ZCanProtocolParser::_registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; }
void ZCanProtocolParser::registerModule(ZIModule* module) {
_registerModule(module->getid(), module);
}
void ZCanProtocolParser::registerModule(ZIModule* module) { _registerModule(module->getid(), module); }
void ZCanProtocolParser::processRxPacket(ZcanRxframe* rxframe) {
// printf("processRxPacket cmdid:%d moduleid:%d\n", rxcmd->cmdid, rxcmd->moduleId);
@ -285,9 +285,6 @@ void ZCanProtocolParser::_processRxPacket(ZIModule* module, zcr_cmd_header_t* rx
#define MODULE_CLASS ZIModule
int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(0);
return module->module_ping();
@ -351,7 +348,6 @@ int32_t ZCanProtocolParser::module_get_type(cmdcontxt_t* cxt) {
return module->module_get_type(&ack[0]);
}
// int32_t ZCanProtocolParser::module_active_cfg(cmdcontxt_t* cxt) {
// CHECK_AND_GET_MODULE(0);
// return module->module_active_cfg();
@ -470,6 +466,22 @@ int32_t ZCanProtocolParser::step_motor_easy_move_to_zero_point_quick(cmdcontxt_t
return module->step_motor_easy_move_to_zero_point_quick();
}
int32_t ZCanProtocolParser::step_motor_move_by(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(2);
return module->step_motor_move_by(cxt->params[0], cxt->params[1]);
}
int32_t ZCanProtocolParser::step_motor_move_to(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(2);
return module->step_motor_move_to(cxt->params[0], cxt->params[1]);
}
int32_t ZCanProtocolParser::step_motor_move_to_zero_point_quick(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(2);
return module->step_motor_move_to_zero_point_quick(cxt->params[0]);
}
int32_t ZCanProtocolParser::step_motor_rotate(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(2);
return module->step_motor_rotate(cxt->params[0], cxt->params[1]);
}
#undef MODULE_CLASS
#define MODULE_CLASS MiniServoCtrlModule
// virtual int32_t mini_servo_enable(int32_t enable) = 0;

5
sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp
View File

@ -88,6 +88,11 @@ class ZCanProtocolParser : public IZCanRxProcesser {
CMDFN(step_motor_easy_reciprocating_motion);
CMDFN(step_motor_easy_move_to_zero_point_quick);
CMDFN(step_motor_move_by);
CMDFN(step_motor_move_to);
CMDFN(step_motor_move_to_zero_point_quick);
CMDFN(step_motor_rotate);
CMDFN(mini_servo_enable);
CMDFN(mini_servo_read_pos);
CMDFN(mini_servo_active_cfg);

2
usrc/version.h
View File

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