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
49aa1c4693
commit
4589e98425
8 changed files with 366 additions and 233 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. 1
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  2. 377
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  3. 33
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  4. 54
      components/tmc/ic/ztmc5130.cpp
  5. 101
      components/tmc/ic/ztmc5130.hpp
  6. 23
      components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp
  7. 8
      components/zcancmder/zcan_protocol_parser.cpp
  8. 2
      components/zcancmder/zcan_protocol_parser.hpp

1
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -176,7 +176,6 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t *
void MiniServoCtrlModule::befor_motor_move() { void MiniServoCtrlModule::befor_motor_move() {
creg.m_module_status = 1; creg.m_module_status = 1;
creg.module_errorcode = 0; creg.module_errorcode = 0;
creg.module_errorbitflag0 = 0;
} }
void MiniServoCtrlModule::after_motor_move() { void MiniServoCtrlModule::after_motor_move() {
if (creg.module_errorcode != 0) { if (creg.module_errorcode != 0) {

377
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -5,6 +5,7 @@
#include "a8000_protocol\protocol.hpp" #include "a8000_protocol\protocol.hpp"
#include "sdk\components\flash\znvs.hpp" #include "sdk\components\flash\znvs.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"
using namespace iflytop; using namespace iflytop;
#define TAG "SMCM" #define TAG "SMCM"
@ -23,6 +24,11 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i
step_motor_active_cfg(); step_motor_active_cfg();
step_motor_enable(true); step_motor_enable(true);
TMC5130* tmcmotor = dynamic_cast<TMC5130*>(m_stepM1);
if (tmcmotor) {
tmcmotor->getGState(); // 读取状态，清空下复位标识
}
} }
void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
memset(&cfg, 0, sizeof(cfg)); memset(&cfg, 0, sizeof(cfg));
@ -64,7 +70,7 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
case kreg_step_motor_iholddelay: case kreg_step_motor_iholddelay:
case kreg_step_motor_max_d: case kreg_step_motor_max_d:
case kreg_step_motor_min_d: case kreg_step_motor_min_d:
step_motor_active_cfg();
return 0;
} }
return suc; return suc;
} }
@ -113,125 +119,6 @@ int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) {
m_stepM1->stop(); m_stepM1->stop();
return 0; return 0;
} }
int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) {
ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction);
m_thread.stop();
if (!m_state.enable) {
return err::kstep_motor_not_enable;
}
m_thread.start(
[this, direction]() {
befor_motor_move();
{
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
int32_t vel = m_cfg.motor_default_velocity;
if (direction <= 0) vel = -vel;
m_stepM1->rotate(vel);
while (true) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
vTaskDelay(5);
}
}
after_motor_move();
},
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) {
ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox);
m_thread.stop();
if (!m_state.enable) {
return err::kstep_motor_not_enable;
}
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;
m_thread.start(
[this, tox]() {
befor_motor_move();
{
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
int32_t motor_pos = 0;
inter_forward_kinematics(tox, motor_pos);
m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
vTaskDelay(5);
}
}
after_motor_move();
},
[this, tox]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) {
ZLOGI(TAG, "m%d motor_move_by %d", m_id, distance);
int32_t motor_pos = 0;
step_motor_read_pos(&motor_pos);
motor_pos += distance;
return step_motor_easy_move_to(motor_pos);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() {
ZLOGI(TAG, "m%d motor_move_to_zero", m_id);
m_thread.stop();
if (!m_state.enable) {
return err::kstep_motor_not_enable;
}
if (m_nio == 0) {
return err::kstep_motor_ioindex_out_of_range;
}
m_thread.start(
[this]() {
befor_motor_move();
exec_move_to_io_task(0, -1);
after_motor_move();
m_stepM1->setXACTUAL(0);
},
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) {
ZLOGI(TAG, "m%d motor_move_to_io %d %d", m_id, ioindex, direction);
m_thread.stop();
if (!m_state.enable) {
return err::kstep_motor_not_enable;
}
if (ioindex < 0 || ioindex >= m_nio) {
return err::kstep_motor_ioindex_out_of_range;
}
m_thread.start(
[this, ioindex, direction]() {
befor_motor_move();
exec_move_to_io_task(ioindex, direction);
after_motor_move();
},
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) {
int32_t motor_pos = 0; int32_t motor_pos = 0;
@ -263,6 +150,11 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t*
return 0; return 0;
} }
int32_t StepMotorCtrlModule::step_motor_read_motoric_status(int32_t* errorflag) {
*errorflag = m_state.motorStatus;
return 0;
}
/*********************************************************************************************************************** /***********************************************************************************************************************
* INTER * * INTER *
***********************************************************************************************************************/ ***********************************************************************************************************************/
@ -279,10 +171,12 @@ int StepMotorCtrlModule::inter_get_pos(int32_t& x) {
return 0; return 0;
} }
void StepMotorCtrlModule::befor_motor_move() { // void StepMotorCtrlModule::befor_motor_move() { //
m_state.before_move_pos = m_stepM1->getXACTUAL();
creg.m_module_status = 1;
creg.module_errorcode = 0;
creg.module_errorbitflag0 = 0;
m_state.before_move_pos = m_stepM1->getXACTUAL();
creg.m_module_status = 1;
creg.module_errorcode = 0;
step_motor_active_cfg();
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_pos = m_stepM1->getXACTUAL();
@ -292,9 +186,53 @@ void StepMotorCtrlModule::after_motor_move() {
} }
creg.m_module_status = 0; creg.m_module_status = 0;
} }
bool StepMotorCtrlModule::check_when_run() { return true; }
int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) {
void StepMotorCtrlModule::dumpTMC5130Status(TMC5130::DevStatusReg_t* status) {
ZLOGE(TAG, "sg_result: %d", status->sg_result);
ZLOGE(TAG, "fsactive: %d", status->fsactive);
ZLOGE(TAG, "cs_actual: %d", status->cs_actual);
ZLOGE(TAG, "stallguard: %d", status->stallguard);
ZLOGE(TAG, "ot: %d", status->ot);
ZLOGE(TAG, "otpw: %d", status->otpw);
ZLOGE(TAG, "s2ga: %d", status->s2ga);
ZLOGE(TAG, "s2gb: %d", status->s2gb);
ZLOGE(TAG, "ola: %d", status->ola);
ZLOGE(TAG, "olb: %d", status->olb);
ZLOGE(TAG, "stst: %d", status->stst);
}
void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) {
creg.module_errorcode = ecode;
m_state.motorStatus = motorStatue;
}
bool StepMotorCtrlModule::check_when_run() {
//
TMC5130* tmc5130 = dynamic_cast<TMC5130*>(m_stepM1);
if (tmc5130) {
auto state = tmc5130->getGState();
auto devStatus = tmc5130->getDevStatus();
if (state.reset) {
ZLOGE(TAG, "motor reset when run");
setErrorFlag(err::kstep_motor_subic_reset, (*(uint32_t*)&devStatus));
return false;
} else if (state.uv_cp) {
ZLOGE(TAG, "motor uv_cp when run");
dumpTMC5130Status(&devStatus);
setErrorFlag(err::kstep_motor_uv_cp, (*(uint32_t*)&devStatus));
return false;
} else if (state.drv_err) {
ZLOGE(TAG, "motor drv_err when run");
setErrorFlag(err::kstep_motor_drv_err, (*(uint32_t*)&devStatus));
dumpTMC5130Status(&devStatus);
return false;
}
}
return true;
}
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_run_to_zero_dec;
@ -304,6 +242,7 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc
direction = direction >= 0 ? 1 : -1; direction = direction >= 0 ? 1 : -1;
ZGPIO* gpio = &m_iotable[ioindex]; ZGPIO* gpio = &m_iotable[ioindex];
if (!check_when_run()) return false;
if (!gpio->getState()) { if (!gpio->getState()) {
ZLOGI(TAG, "---------STEP1-------- move to point"); ZLOGI(TAG, "---------STEP1-------- move to point");
@ -328,10 +267,13 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc
// 失败返回 // 失败返回
if (!xreach_io) { if (!xreach_io) {
ZLOGE(TAG, "x reach io failed"); ZLOGE(TAG, "x reach io failed");
return err::kxymotor_not_found_x_zero_point;
setErrorFlag(err::kstep_motor_not_found_zero_point, 0);
return false;
} }
} }
if (!check_when_run()) return false;
// 如果设备已经在零点，则反向移动一定距离远离零点 // 如果设备已经在零点，则反向移动一定距离远离零点
if (gpio->getState()) { if (gpio->getState()) {
ZLOGI(TAG, "---------STEP2-------- find edge"); ZLOGI(TAG, "---------STEP2-------- find edge");
@ -354,17 +296,20 @@ int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direc
if (!reach_edge) { if (!reach_edge) {
ZLOGE(TAG, "leave away zero failed"); ZLOGE(TAG, "leave away zero failed");
return err::kxymotor_x_find_zero_edge_fail;
setErrorFlag(err::kstep_motor_not_found_point_edge, 0);
return false;
} }
} }
if (!check_when_run()) return false;
if (m_thread.getExitFlag()) { if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return err::kmodule_opeation_break_by_user;
ZLOGW(TAG, "break move to zero thread exit");
return true;
} }
ZLOGI(TAG, "_exec_move_to_io_task_fn success"); ZLOGI(TAG, "_exec_move_to_io_task_fn success");
return 0;
return true;
} }
void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) {
@ -372,4 +317,172 @@ void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) {
m_stepM1->setAcceleration(acc); m_stepM1->setAcceleration(acc);
m_stepM1->setDeceleration(dec); m_stepM1->setDeceleration(dec);
m_stepM1->rotate(vel); m_stepM1->rotate(vel);
}
}
/***********************************************************************************************************************
* 电机控制接口 *
***********************************************************************************************************************/
int32_t StepMotorCtrlModule::module_clear_error() {
/**
* @brief
* 驱动器大部分的错误，在不重启的情况下，是无法清除的。module_clear_error这里不生效
*/
return 0;
}
int32_t StepMotorCtrlModule::check_befor_run() {
if (!m_state.enable) {
return err::kstep_motor_not_enable;
}
if (creg.module_errorcode == err::kstep_motor_subic_reset) {
return creg.module_errorcode;
} else if (creg.module_errorcode == err::kstep_motor_drv_err) {
return creg.module_errorcode;
} else if (creg.module_errorcode == err::kstep_motor_uv_cp) {
return creg.module_errorcode;
} else {
/**
* @brief 其他错误允许设备继续运行
*/
return 0;
}
return 0;
}
int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) {
ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_rotate(direction);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) {
ZLOGI(TAG, "m%d motor_move_by %d", m_id, distance);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_move_by(distance);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) {
ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
return do_step_motor_easy_move_to(tox);
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() {
ZLOGI(TAG, "m%d motor_move_to_zero", m_id);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
if (m_nio == 0) {
return err::kstep_motor_ioindex_out_of_range;
}
return do_step_motor_easy_move_to_zero();
}
int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) {
ZLOGI(TAG, "m%d motor_move_to_io %d %d", m_id, ioindex, direction);
int32_t ecode = check_befor_run();
if (ecode != 0) return ecode;
if (ioindex < 0 || ioindex >= m_nio) {
return err::kstep_motor_ioindex_out_of_range;
}
return do_step_motor_easy_move_to_io(ioindex, direction);
}
//
//
//
int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
m_thread.stop();
m_thread.start(
[this, direction]() {
befor_motor_move();
{
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
int32_t vel = m_cfg.motor_default_velocity;
if (direction <= 0) vel = -vel;
m_stepM1->rotate(vel);
while (true) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
vTaskDelay(5);
}
}
after_motor_move();
},
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::do_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;
m_thread.stop();
m_thread.start(
[this, tox]() {
befor_motor_move();
{
m_stepM1->setAcceleration(m_cfg.motor_default_acc);
m_stepM1->setDeceleration(m_cfg.motor_default_dec);
int32_t motor_pos = 0;
inter_forward_kinematics(tox, motor_pos);
m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity);
while (!m_stepM1->isReachTarget()) {
if (m_thread.getExitFlag()) break;
if (!check_when_run()) break;
vTaskDelay(5);
}
}
after_motor_move();
},
[this, tox]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::do_step_motor_easy_move_by(int32_t distance) {
int32_t motor_pos = 0;
step_motor_read_pos(&motor_pos);
motor_pos += distance;
return step_motor_easy_move_to(motor_pos);
}
int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() {
m_thread.stop();
m_thread.start(
[this]() {
befor_motor_move();
exec_move_to_io_task(0, -1);
after_motor_move();
m_stepM1->setXACTUAL(0);
},
[this]() { m_stepM1->stop(); });
return 0;
}
int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) {
m_thread.stop();
m_thread.start(
[this, ioindex, direction]() {
befor_motor_move();
exec_move_to_io_task(ioindex, direction);
after_motor_move();
},
[this]() { m_stepM1->stop(); });
return 0;
}

33
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -3,6 +3,7 @@
#include "sdk/os/zos.hpp" #include "sdk/os/zos.hpp"
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" #include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"
namespace iflytop { namespace iflytop {
class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
ENABLE_MODULE(StepMotorCtrlModule, ktmc_step_motor, 0x0001); ENABLE_MODULE(StepMotorCtrlModule, ktmc_step_motor, 0x0001);
@ -30,10 +31,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
} config_t; } config_t;
typedef struct { typedef struct {
int32_t dpos;
int32_t after_move_pos;
int32_t before_move_pos;
int32_t enable;
int32_t dpos;
int32_t after_move_pos;
int32_t before_move_pos;
int32_t enable;
uint32_t motorStatus;
} state_t; } state_t;
private: private:
@ -63,6 +65,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override; virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val) override;
virtual int32_t module_stop() override { return step_motor_stop(0); } virtual int32_t module_stop() override { return step_motor_stop(0); }
virtual int32_t module_active_cfg() override { return step_motor_active_cfg(); } virtual int32_t module_active_cfg() override { return step_motor_active_cfg(); }
virtual int32_t module_clear_error() override;
/*********************************************************************************************************************** /***********************************************************************************************************************
* ZIStepMotor * * ZIStepMotor *
***********************************************************************************************************************/ ***********************************************************************************************************************/
@ -74,16 +77,28 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t step_motor_easy_move_by(int32_t distance) override; virtual int32_t step_motor_easy_move_by(int32_t distance) override;
virtual int32_t step_motor_easy_move_to(int32_t position) override; virtual int32_t step_motor_easy_move_to(int32_t position) override;
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_set_current_pos(int32_t pos) 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_set_current_pos(int32_t pos) override;
virtual int32_t step_motor_active_cfg() override; virtual int32_t step_motor_active_cfg() override;
virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override; virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override;
virtual int32_t step_motor_read_motoric_status(int32_t* errorflag) override;
public: public:
IStepperMotor* getMotor() { return m_stepM1; } IStepperMotor* getMotor() { return m_stepM1; }
config_t* getCfg() { return &m_cfg; } config_t* getCfg() { return &m_cfg; }
private: 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_move_to_zero();
int32_t do_step_motor_easy_move_to_io(int32_t ioindex, int32_t direction);
private:
void inter_inverse_kinematics(int32_t motor_pos, int32_t& x); void inter_inverse_kinematics(int32_t motor_pos, int32_t& x);
void inter_forward_kinematics(int32_t x, int32_t& motor_pos); void inter_forward_kinematics(int32_t x, int32_t& motor_pos);
@ -94,9 +109,13 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
void after_motor_move(); void after_motor_move();
bool check_when_run(); bool check_when_run();
int32_t exec_move_to_io_task(int32_t ioindex, int32_t direction);
void _rotate(int32_t vel, int32_t acc, int32_t dec);
bool exec_move_to_io_task(int32_t ioindex, int32_t direction);
void _rotate(int32_t vel, int32_t acc, int32_t dec);
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);
private:
void dumpTMC5130Status(TMC5130::DevStatusReg_t* status);
void setErrorFlag(int32_t ecode, uint32_t motorStatue);
}; };
} // namespace iflytop } // namespace iflytop

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

@ -46,39 +46,8 @@ void TMC5130::initialize(cfg_t *cfg) {
} }
m_hspi = cfg->spi; m_hspi = cfg->spi;
enableIC(false);
// tmc5130_init(&m_TMC5130, channel, &m_tmc_api_config, &tmc5130_defaultRegisterResetState[0]);
// tmc5130_setCallback(&m_TMC5130, pri_tmc4361A_callback);
enable(false);
reset(); reset();
writeInt(TMC5130_PWMCONF, 0x000500C8);
// writeInt( TMC5130_GCONF, 0x00000004);
writeInt(TMC5130_CHOPCONF, 0x000100c3);
writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
writeInt(TMC5130_PWMCONF, 0x000401c8);
writeInt(TMC5130_XTARGET, 0);
writeInt(TMC5130_XACTUAL, 0x00000000);
writeInt(TMC5130_VACTUAL, 0x00000000);
writeInt(TMC5130_VSTART, 100);
writeInt(TMC5130_A1, 1000);
writeInt(TMC5130_V1, 0);
writeInt(TMC5130_D1, 1000);
writeInt(TMC5130_VSTOP, 100);
writeInt(TMC5130_TZEROWAIT, 0);
// writeInt(TMC5130_VSTART, 100);
// writeInt(TMC5130_V1, 0);
// writeInt(TMC5130_VSTOP, 100);
// writeInt(TMC5130_TZEROWAIT, 0);
setAcceleration(100);
setDeceleration(100);
setIHOLD_IRUN(2, 10, 1);
enableIC(true);
} }
void TMC5130::setMRES(mres_type_t value) { void TMC5130::setMRES(mres_type_t value) {
@ -118,7 +87,7 @@ void TMC5130::setNoAccLimit(bool enable) {
} }
} }
void TMC5130::enableIC(bool enable) {
void TMC5130::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);
} }
@ -137,6 +106,23 @@ uint8_t TMC5130::reset() {
} }
writeInt(add, m_defaultRegisterResetState[add]); writeInt(add, m_defaultRegisterResetState[add]);
} }
writeInt(TMC5130_PWMCONF, 0x000500C8);
writeInt(TMC5130_CHOPCONF, 0x000100c3);
writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
writeInt(TMC5130_PWMCONF, 0x000401c8);
writeInt(TMC5130_XTARGET, 0);
writeInt(TMC5130_XACTUAL, 0x00000000);
writeInt(TMC5130_VACTUAL, 0x00000000);
writeInt(TMC5130_VSTART, 100);
writeInt(TMC5130_A1, 1000);
writeInt(TMC5130_V1, 0);
writeInt(TMC5130_D1, 1000);
writeInt(TMC5130_VSTOP, 100);
writeInt(TMC5130_TZEROWAIT, 0);
setAcceleration(100);
setDeceleration(100);
setIHOLD_IRUN(2, 10, 1);
enable(false);
return 0; return 0;
} }
int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); }
@ -181,8 +167,6 @@ void TMC5130::rotate(int32_t velocity) {
writeInt(TMC5130_VMAX, abs(velocity)); writeInt(TMC5130_VMAX, abs(velocity));
writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
} }
void TMC5130::right(int32_t velocity) { rotate(velocity); }
void TMC5130::left(int32_t velocity) { rotate(-velocity); }
void TMC5130::moveTo(int32_t position, uint32_t velocityMax) { void TMC5130::moveTo(int32_t position, uint32_t velocityMax) {
// position *= m_scale; // position *= m_scale;
// velocityMax *= m_scale; // velocityMax *= m_scale;

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

@ -46,6 +46,28 @@ class Tmc5130RampStat {
class TMC5130 : public IStepperMotor { class TMC5130 : public IStepperMotor {
public: public:
typedef struct { typedef struct {
uint32_t sg_result : 10;
uint32_t reserved0 : 5;
uint32_t fsactive : 1;
uint32_t cs_actual : 5;
uint32_t reserved1 : 3;
uint32_t stallguard : 1;
uint32_t ot : 1;
uint32_t otpw : 1;
uint32_t s2ga : 1;
uint32_t s2gb : 1;
uint32_t ola : 1;
uint32_t olb : 1;
uint32_t stst : 1;
} DevStatusReg_t;
typedef struct {
uint32_t reset : 1;
uint32_t drv_err : 1; //
uint32_t uv_cp : 1;
} GState_t;
typedef struct {
SPI_HandleTypeDef *spi; SPI_HandleTypeDef *spi;
Pin_t csgpio = PinNull; // Pin_t csgpio = PinNull; //
Pin_t resetPin = PinNull; // Pin_t resetPin = PinNull; //
@ -55,9 +77,6 @@ class TMC5130 : public IStepperMotor {
} cfg_t; } cfg_t;
protected: protected:
// TMC5130Port *m_port;
// TMC5130Config_t *m_config;
cfg_t m_cfg; cfg_t m_cfg;
ZGPIO *m_csnpin = NULL; ZGPIO *m_csnpin = NULL;
@ -81,69 +100,59 @@ class TMC5130 : public IStepperMotor {
public: public:
TMC5130(/* args */); TMC5130(/* args */);
// static void createDeafultTMC5130Config(TMC5130Config_t *config, TMC5130Port *m_port);
void initialize(cfg_t *cfg); void initialize(cfg_t *cfg);
void enableIC(bool enable);
virtual void enable(bool enable) { enableIC(enable); }
uint8_t reset();
virtual void enable(bool enable);
/*******************************************************************************
* 常用寄存器读写方法 *
*******************************************************************************/
uint32_t readICVersion(); // 5130:0x11 5160:0x30
virtual int32_t getXACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
virtual void setXACTUAL(int32_t value); // 设置电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
virtual void rotate(int32_t velocity);
virtual void moveTo(int32_t position, uint32_t velocityMax);
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax);
virtual void stop();
virtual int32_t getVACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
uint32_t readXTARGET(); // 读取驱动器目标位置
virtual void setXACTUAL(int32_t value); // 设置当前位置
virtual int32_t getXACTUAL(); // 当前位置
virtual int32_t getVACTUAL(); // 当前速度
virtual void setAcceleration(float accelerationpps2); // 设置最大加速度
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度
virtual void setMotorShaft(bool reverse);
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
virtual void setGlobalScale(uint8_t globalscale); // ONLY for 5160
virtual void setScale(int32_t scale); virtual void setScale(int32_t scale);
virtual void setScaleDenominator(int32_t scale); virtual void setScaleDenominator(int32_t scale);
virtual void setAcceleration(float accelerationpps2);
virtual void setDeceleration(float accelerationpps2);
void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
void setGlobalScale(uint8_t globalscale);
uint8_t getGlobalScale();
// void setSubdivision(uint8_t subdivision);
virtual bool isReachTarget(); // 是否到达目标位置
virtual bool isStoped() { return isReachTarget(); }
virtual void setNoAccLimit(bool enable) override;
void setMotorShaft(bool reverse); // 设置电机旋转方向
public:
DevStatusReg_t getDevStatus() {
static_assert(sizeof(DevStatusReg_t) == 4);
uint32_t value = readInt(TMC5130_DRVSTATUS);
return *(DevStatusReg_t *)&value;
}
GState_t getGState() {
static_assert(sizeof(GState_t) == 4);
uint32_t value = readInt(TMC5130_GSTAT);
return *(GState_t *)&value;
}
public:
uint8_t reset();
uint32_t readICVersion(); // 5130:0x11 5160:0x30
uint32_t readXTARGET(); // 读取驱动器目标位置
uint8_t getGlobalScale(); //
uint32_t getTMC5130_RAMPSTAT(); // 这个寄存器读取的是TMC5130的状态寄存器 uint32_t getTMC5130_RAMPSTAT(); // 这个寄存器读取的是TMC5130的状态寄存器
Tmc5130RampStat getTMC5130_RAMPSTAT2(); Tmc5130RampStat getTMC5130_RAMPSTAT2();
virtual void rotate(int32_t velocity);
virtual void right(int32_t velocity);
virtual void left(int32_t velocity);
virtual void moveTo(int32_t position, uint32_t velocityMax);
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax);
virtual void stop();
virtual bool isReachTarget(); // 是否到达目标位置
virtual bool isStoped() { return isReachTarget(); }
/******************************************************************************* /*******************************************************************************
* 驱动器寄存器读写方法 * * 驱动器寄存器读写方法 *
*******************************************************************************/ *******************************************************************************/
// void writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value);
void readWriteArray(uint8_t *data, size_t length); void readWriteArray(uint8_t *data, size_t length);
void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
void writeInt(uint8_t address, int32_t value); void writeInt(uint8_t address, int32_t value);
int32_t readInt(uint8_t address); int32_t readInt(uint8_t address);
void setNoAccLimit(bool enable) override;
/**
* @brief 设置电机细分
*
* @param value
*/
void setMRES(mres_type_t value);
void setMRES(mres_type_t value);
private: private:
uint32_t haspassedms(uint32_t now, uint32_t last); uint32_t haspassedms(uint32_t now, uint32_t last);

23
components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp
View File

@ -1,7 +1,7 @@
#include "water_cooling_temperature_control_module.hpp" #include "water_cooling_temperature_control_module.hpp"
#include "sdk\components\flash\znvs.hpp"
#include "a8000_protocol\protocol.hpp" #include "a8000_protocol\protocol.hpp"
#include "sdk\components\flash\znvs.hpp"
using namespace iflytop; using namespace iflytop;
@ -14,10 +14,10 @@ using namespace iflytop;
#define ACTION_TEST_ENABLE_LOG 5 #define ACTION_TEST_ENABLE_LOG 5
#define ACTION_TEST_DISABLE_LOG 6 #define ACTION_TEST_DISABLE_LOG 6
static int32_t prvabs(int32_t v){
if(v<0){
static int32_t prvabs(int32_t v) {
if (v < 0) {
return -v; return -v;
}else{
} else {
return v; return v;
} }
} }
@ -53,8 +53,7 @@ int32_t WaterCoolingTemperatureControlModule::getid(int32_t* id) {
return 0; return 0;
} }
int32_t WaterCoolingTemperatureControlModule::module_clear_error() { int32_t WaterCoolingTemperatureControlModule::module_clear_error() {
creg.module_errorcode = 0;
creg.module_errorbitflag0 = 0;
creg.module_errorcode = 0;
m_fan_ctrl->clearError(); m_fan_ctrl->clearError();
m_pelter_ctrl->clearError(); m_pelter_ctrl->clearError();
m_pump_ctrl->clearError(); m_pump_ctrl->clearError();
@ -195,13 +194,13 @@ void WaterCoolingTemperatureControlModule::workloop() {
#define BIT(x, n) ((x >> n) & 0x01) #define BIT(x, n) ((x >> n) & 0x01)
int32_t WaterCoolingTemperatureControlModule::checkdevice() { int32_t WaterCoolingTemperatureControlModule::checkdevice() {
creg.module_errorbitflag0 = geterrorbitflag0();
if (creg.module_errorbitflag0 != 0) {
uint32_t module_errorbitflag0 = geterrorbitflag0();
if (module_errorbitflag0 != 0) {
ZLOGE(TAG, "checkdevice errorbitflag0:%d %d %d %d", // ZLOGE(TAG, "checkdevice errorbitflag0:%d %d %d %d", //
BIT(creg.module_errorbitflag0, 0), //
BIT(creg.module_errorbitflag0, 1), //
BIT(creg.module_errorbitflag0, 2), //
BIT(creg.module_errorbitflag0, 3));
BIT(module_errorbitflag0, 0), //
BIT(module_errorbitflag0, 1), //
BIT(module_errorbitflag0, 2), //
BIT(module_errorbitflag0, 3));
m_pelter_ctrl->dumpErrorInfo(); m_pelter_ctrl->dumpErrorInfo();
creg.module_errorcode = err::khwardware_error; creg.module_errorcode = err::khwardware_error;
return 1; return 1;

8
components/zcancmder/zcan_protocol_parser.cpp
View File

@ -33,6 +33,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
REGFN(step_motor_active_cfg); REGFN(step_motor_active_cfg);
REGFN(step_motor_stop); REGFN(step_motor_stop);
REGFN(step_motor_read_io_state); REGFN(step_motor_read_io_state);
REGFN(step_motor_read_motoric_status);
REGFN(mini_servo_enable); REGFN(mini_servo_enable);
REGFN(mini_servo_read_pos); REGFN(mini_servo_read_pos);
@ -246,6 +247,13 @@ int32_t ZCanProtocolParser::step_motor_read_io_state(cmdcontxt_t* cxt) {
return module->step_motor_read_io_state(cxt->params[0], ack); return module->step_motor_read_io_state(cxt->params[0], ack);
} }
int32_t ZCanProtocolParser::step_motor_read_motoric_status(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(0);
int32_t* ack = (int32_t*)cxt->ackbuf;
cxt->acklen = 4;
return module->step_motor_read_motoric_status(&ack[0]);
}
#undef MODULE_CLASS #undef MODULE_CLASS
#define MODULE_CLASS ZIMiniServo #define MODULE_CLASS ZIMiniServo
// virtual int32_t mini_servo_enable(int32_t enable) = 0; // virtual int32_t mini_servo_enable(int32_t enable) = 0;

2
components/zcancmder/zcan_protocol_parser.hpp
View File

@ -68,6 +68,8 @@ class ZCanProtocolParser : public IZCanReceiverListener {
static int32_t step_motor_easy_move_to_io(cmdcontxt_t* cxt); static int32_t step_motor_easy_move_to_io(cmdcontxt_t* cxt);
static int32_t step_motor_active_cfg(cmdcontxt_t* cxt); static int32_t step_motor_active_cfg(cmdcontxt_t* cxt);
static int32_t step_motor_read_io_state(cmdcontxt_t* cxt); static int32_t step_motor_read_io_state(cmdcontxt_t* cxt);
static int32_t step_motor_read_motoric_status(cmdcontxt_t* cxt);
static int32_t mini_servo_enable(cmdcontxt_t* cxt); static int32_t mini_servo_enable(cmdcontxt_t* cxt);
static int32_t mini_servo_read_pos(cmdcontxt_t* cxt); static int32_t mini_servo_read_pos(cmdcontxt_t* cxt);

Write Preview
Loading…
Cancel
Save