Browse Source

update

master
zhaohe 2 years ago
parent
commit
998c09f13e
  1. 2
      components/step_motor_45/step_motor_45.hpp
  2. 235
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  3. 47
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  4. 2
      components/zprotocols/zcancmder_v2

2
components/step_motor_45/step_motor_45.hpp

@ -133,7 +133,7 @@ class StepMotor45 : public ZIModule, public ZIMotor {
virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_read_pos(int32_t *pos);
virtual int32_t motor_read_pos(int32_t *pos) override;
private:
bool getzeropinstate();

235
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@ -252,10 +252,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_
m_thread.stop();
m_thread.start([this, lastforms, speed, status_cb]() {
ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec);
m_stepM1->setAcceleration(m_param.acc);
m_stepM1->setDeceleration(m_param.dec);
m_stepM1->rotate(speed);
_rotate(speed, m_param.acc, m_param.dec);
int32_t startticket = zos_get_tick();
bool reachtime = false;
@ -380,9 +377,9 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
if (!m_Xgpio->getState()) {
{
ZLOGI(TAG, "---------STEP1-------- move to zero first");
_motor_move_by(-m_param.run_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
_rotate(-m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
bool xreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
while (!m_thread.getExitFlag()) {
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
if (m_Xgpio->getState()) {
xreach_zero = true;
@ -414,8 +411,8 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
* @brief
*/
if (m_Xgpio->getState()) {
_motor_move_by(m_param.look_zero_edge_max_d, m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec);
while (!_motor_is_reach_target()) {
_rotate(m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec);
while (true) {
if (m_thread.getExitFlag()) break;
if (!m_Xgpio->getState()) {
_motor_stop(-1);
@ -434,42 +431,6 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
return err::kxymotor_x_find_zero_edge_fail;
}
}
/*******************************************************************************
* X轴到零点 *
*******************************************************************************/
// {
// ZLOGI(TAG, "---------STEP3-------- move to zero");
// _motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
// bool xreach_zero = false;
// while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
// if (m_Xgpio->getState()) {
// xreach_zero = true;
// _motor_stop(-1);
// while (!m_stepM1->isStoped()) { // 等待电机停止
// _motor_stop(-1);
// vTaskDelay(10);
// }
// break;
// }
// vTaskDelay(1);
// }
// if (m_thread.getExitFlag()) {
// ZLOGI(TAG, "break move to zero thread exit");
// return err::kmodule_opeation_break_by_user;
// }
// if (!xreach_zero) {
// // 触发异常回调
// ZLOGE(TAG, "x reach zero failed");
// return err::kxymotor_not_found_x_zero_point;
// }
// }
if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return 0;
@ -480,6 +441,12 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
return 0;
}
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::getnowpos(int32_t& pos) {
int32_t motor_pos = m_stepM1->getXACTUAL();
@ -638,3 +605,183 @@ void StepMotorCtrlModule::active_cfg() {
m_stepM1->setScale(m_param.distance_scale);
m_stepM1->setMotorShaft(m_param.x_shaft);
}
int32_t StepMotorCtrlModule::getid(int32_t* id) {
*id = m_id;
return 0;
}
int32_t StepMotorCtrlModule::module_stop() { return stop(0); }
int32_t StepMotorCtrlModule::module_break() { return stop(0); }
int32_t StepMotorCtrlModule::module_get_last_exec_status(int32_t* status) {
*status = m_lastexecstatus;
return 0;
}
int32_t StepMotorCtrlModule::module_get_status(int32_t* status) {
*status = m_thread.isworking() ? 1 : 0;
return 0;
}
int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) {
*iserror = 0;
return 0;
}
int32_t StepMotorCtrlModule::module_set_param(int32_t param_id, int32_t param_value) {
if (param_id == kcfg_motor_x_shift) {
m_param.shift_x = param_value;
return 0;
}
if (param_id == kcfg_motor_x_shaft) {
m_param.x_shaft = param_value;
return 0;
}
if (param_id == kcfg_motor_x_one_circle_pulse) {
m_param.distance_scale = param_value;
return 0;
}
if (param_id == k_cfg_stepmotor_ihold) {
m_param.ihold = param_value;
return 0;
}
if (param_id == k_cfg_stepmotor_irun) {
m_param.irun = param_value;
return 0;
}
if (param_id == k_cfg_stepmotor_iholddelay) {
m_param.iholddelay = param_value;
return 0;
}
return err::kmodule_not_find_config_index;
}
int32_t StepMotorCtrlModule::module_get_param(int32_t param_id, int32_t* param_value) {
if (param_id == kcfg_motor_x_shift) {
*param_value = m_param.shift_x;
return 0;
}
if (param_id == kcfg_motor_x_shaft) {
*param_value = m_param.x_shaft;
return 0;
}
if (param_id == kcfg_motor_x_one_circle_pulse) {
*param_value = m_param.distance_scale;
return 0;
}
if (param_id == k_cfg_stepmotor_ihold) {
*param_value = m_param.ihold;
return 0;
}
if (param_id == k_cfg_stepmotor_irun) {
*param_value = m_param.irun;
return 0;
}
if (param_id == k_cfg_stepmotor_iholddelay) {
*param_value = m_param.iholddelay;
return 0;
}
return err::kmodule_not_find_config_index;
}
int32_t StepMotorCtrlModule::module_clear_error() { return 0; }
int32_t StepMotorCtrlModule::module_readio(int32_t* io) {
*io = 0;
for (size_t i = 0; i < m_nio; i++) {
if (m_iotable[i].getState()) {
*io |= (0x01 << i);
}
}
return 0;
}
int32_t StepMotorCtrlModule::module_writeio(int32_t io) { return 0; }
int32_t StepMotorCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) {
*adc = 0;
return 0;
}
int32_t StepMotorCtrlModule::module_factory_reset() { return factory_reset(); }
int32_t StepMotorCtrlModule::module_flush_cfg() { return flush(); }
int32_t StepMotorCtrlModule::module_active_cfg() {
active_cfg();
return 0;
}
int32_t StepMotorCtrlModule::motor_enable(int32_t enable) {
zlock_guard l(m_lock);
ZLOGI(TAG, "m%d motor_enable %d", m_id, enable);
if (enable != 0) {
m_stepM1->enable(true);
} else {
m_stepM1->enable(false);
}
return 0;
}
int32_t StepMotorCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) {
ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction);
m_status_cb = nullptr;
m_thread.stop();
_rotate(direction * motor_velocity, acc, acc);
return 0;
}
int32_t StepMotorCtrlModule::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) {
zlock_guard lock(m_lock);
int32_t xnow = 0;
getnowpos(xnow);
int32_t toxnow = xnow + dx;
return motor_move_to(toxnow, motor_velocity, acc);
}
int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox);
m_status_cb = nullptr;
m_thread.stop();
if (m_param.min_x != 0 && tox < m_param.min_x) {
tox = m_param.min_x;
}
if (m_param.max_x != 0 && tox > m_param.max_x) {
tox = m_param.max_x;
}
m_thread.start(
[this, tox, motor_velocity, acc]() {
_motor_move_to(tox, motor_velocity, acc, acc);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(10);
}
call_exec_status_cb(0, nullptr);
},
[this, tox]() { _motor_stop(); });
return 0;
}
int32_t StepMotorCtrlModule::motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; }
int32_t StepMotorCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t StepMotorCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t StepMotorCtrlModule::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
int32_t StepMotorCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; }
int32_t StepMotorCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) {
ZLOGI(TAG, "motor_move_to_zero_backward");
m_param.run_to_zero_speed = findzerospeed;
m_param.look_zero_edge_speed = findzeroedge_speed;
m_param.look_zero_edge_dec = acc;
m_param.run_to_zero_dec = acc;
return move_to_zero(nullptr);
}
int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) {
int32_t xnow = 0;
getnowpos(xnow);
*pos = xnow;
return 0;
}

47
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@ -5,7 +5,7 @@
#include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp"
namespace iflytop {
class StepMotorCtrlModule : public I_StepMotorCtrlModule {
class StepMotorCtrlModule : public I_StepMotorCtrlModule, public ZIModule, public ZIMotor {
public:
private:
IStepperMotor* m_stepM1;
@ -33,7 +33,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
action_cb_status_t m_status_cb;
public:
void initialize(int id, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark);
void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark);
static void create_default_cfg(flash_config_t& cfg);
virtual bool isbusy() override;
@ -99,6 +99,48 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
int32_t do_motor_action_block_2(function<int32_t()> action, function<int32_t(bool&, int periodcount)> break_condition);
/*******************************************************************************
* OVERRIDE MODULE *
*******************************************************************************/
virtual int32_t getid(int32_t* id) override;
virtual int32_t module_stop() override;
virtual int32_t module_break() override;
virtual int32_t module_get_last_exec_status(int32_t* status) override;
virtual int32_t module_get_status(int32_t* status) override;
virtual int32_t module_get_error(int32_t* iserror) override;
virtual int32_t module_clear_error() override;
virtual int32_t module_set_param(int32_t param_id, int32_t param_value) override;
virtual int32_t module_get_param(int32_t param_id, int32_t* param_value) override;
virtual int32_t module_readio(int32_t* io) override;
virtual int32_t module_writeio(int32_t io) override;
virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc) override;
virtual int32_t module_factory_reset() override;
virtual int32_t module_flush_cfg() override;
virtual int32_t module_active_cfg() override;
/*******************************************************************************
* Motor *
*******************************************************************************/
virtual int32_t motor_enable(int32_t enable) override;
virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override;
virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_read_pos(int32_t* pos) override;
private:
void active_cfg();
@ -114,6 +156,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
void _motor_move_by(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec);
void _motor_stop(int32_t dec = -1);
bool _motor_is_reach_target();
void _rotate(int32_t vel, int32_t acc, int32_t dec);
void inverse_kinematics(int32_t motor_pos, int32_t& x);
void forward_kinematics(int32_t x, int32_t& motor_pos);

2
components/zprotocols/zcancmder_v2

@ -1 +1 @@
Subproject commit 95626e25193bdb17428bf4edca04af30f1306c04
Subproject commit 87d1a547ef95f5739c1177063dfb8b3db6b6df81
Loading…
Cancel
Save