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
5d12a50cd4
commit
99f6aa959a
4 changed files with 154 additions and 5 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. 99
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 10
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  3. 41
      components/tmc/ic/ztmc4361A.cpp
  4. 9
      components/tmc/ic/ztmc4361A.hpp

99
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -4,6 +4,7 @@
#include <string.h>
#include "a8000_protocol\protocol.hpp"
#include "sdk/components/tmc/ic/ztmc4361A.hpp"
#include "sdk\components\flash\znvs.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"
using namespace iflytop;
@ -29,6 +30,14 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i
if (tmcmotor) {
tmcmotor->getGState(); // 读取状态，清空下复位标识
}
TMC4361A* tmc4361motor = dynamic_cast<TMC4361A*>(m_stepM1);
if (tmc4361motor) {
auto tmc4361state = tmc4361motor->getTMC4361AMotorEventState();
auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus();
auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus();
auto tmc2160State = tmc4361motor->getTMC2160MotorGstate();
}
}
void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
memset(&cfg, 0, sizeof(cfg));
@ -163,6 +172,47 @@ int32_t StepMotorCtrlModule::step_motor_read_tmc5130_status(int32_t* status) {
return 0;
}
int32_t StepMotorCtrlModule::step_motor_read_tmc5130_state(int32_t* status) {
TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) {
auto state = tmc5130->getGState();
memcpy(status, &state, sizeof(int32_t));
}
return 0;
};
int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_status(int32_t* status) {
TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
if (tmc4361) {
auto devStatus = tmc4361->getTMC4361AMotorStatus();
memcpy(status, &devStatus, sizeof(int32_t));
}
return 0;
};
int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_state(int32_t* gstate) {
TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
if (tmc4361) {
auto state = tmc4361->getTMC4361AMotorEventState();
memcpy(gstate, &state, sizeof(int32_t));
}
return 0;
};
int32_t StepMotorCtrlModule::step_motor_read_tmc2160_status(int32_t* status) {
TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
if (tmc4361) {
auto devStatus = tmc4361->getTMC2160MotorDriverStatus();
memcpy(status, &devStatus, sizeof(int32_t));
}
return 0;
};
int32_t StepMotorCtrlModule::step_motor_read_tmc2160_state(int32_t* status) {
TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
if (tmc4361) {
auto state = tmc4361->getTMC2160MotorGstate();
memcpy(status, &state, sizeof(int32_t));
}
return 0;
};
/***********************************************************************************************************************
* INTER *
***********************************************************************************************************************/
@ -208,6 +258,55 @@ void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) {
ZLOGE(TAG, "olb: %d", status->olb);
ZLOGE(TAG, "stst: %d", status->stst);
}
void StepMotorCtrlModule::dump(TMC2160MotorDriverStatus_t val) {
ZLOGE(TAG, "sg_result: %d", val.sg_result);
ZLOGE(TAG, "s2vsa: %d", val.s2vsa);
ZLOGE(TAG, "s2vsb: %d", val.s2vsb);
ZLOGE(TAG, "stealth: %d", val.stealth);
ZLOGE(TAG, "fsactive: %d", val.fsactive);
ZLOGE(TAG, "cs_actual: %d", val.cs_actual);
ZLOGE(TAG, "reserved2: %d", val.reserved2);
ZLOGE(TAG, "stallguard: %d", val.stallguard);
ZLOGE(TAG, "ot: %d", val.ot);
ZLOGE(TAG, "otpw: %d", val.otpw);
ZLOGE(TAG, "s2ga: %d", val.s2ga);
ZLOGE(TAG, "s2gb: %d", val.s2gb);
ZLOGE(TAG, "ola: %d", val.ola);
ZLOGE(TAG, "olb: %d", val.olb);
ZLOGE(TAG, "stst: %d", val.stst);
}
void StepMotorCtrlModule::dump(TMC4361AMotorStatus_t val) {
ZLOGE(TAG, "TARGET_REACHED_F: %d", val.TARGET_REACHED_F);
ZLOGE(TAG, "POS_COMP_REACHED_F: %d", val.POS_COMP_REACHED_F);
ZLOGE(TAG, "VEL_REACHED_F: %d", val.VEL_REACHED_F);
ZLOGE(TAG, "VEL_STATE_F: %d", val.VEL_STATE_F);
ZLOGE(TAG, "RAMP_STATE_F: %d", val.RAMP_STATE_F);
ZLOGE(TAG, "STOPL_ACTIVE_F: %d", val.STOPL_ACTIVE_F);
ZLOGE(TAG, "STOPR_ACTIVE_F: %d", val.STOPR_ACTIVE_F);
ZLOGE(TAG, "VSTOPL_ACTIVE_F: %d", val.VSTOPL_ACTIVE_F);
ZLOGE(TAG, "VSTOPR_ACTIVE_F: %d", val.VSTOPR_ACTIVE_F);
ZLOGE(TAG, "ACTIVE_STALL_F: %d", val.ACTIVE_STALL_F);
ZLOGE(TAG, "HOME_ERROR_F: %d", val.HOME_ERROR_F);
ZLOGE(TAG, "FS_ACTIVE_F: %d", val.FS_ACTIVE_F);
ZLOGE(TAG, "ENC_FAIL_F: %d", val.ENC_FAIL_F);
ZLOGE(TAG, "N_ACTIVE_F: %d", val.N_ACTIVE_F);
ZLOGE(TAG, "ENC_LATCH_F: %d", val.ENC_LATCH_F);
ZLOGE(TAG, "MULTI_CYCLE_FAIL_F: %d", val.MULTI_CYCLE_FAIL_F);
ZLOGE(TAG, "___: %d", val.___);
ZLOGE(TAG, "CL_FIT_F: %d", val.CL_FIT_F);
ZLOGE(TAG, "SERIAL_ENC_FLAGS: %d", val.SERIAL_ENC_FLAGS);
ZLOGE(TAG, "SG: %d", val.SG);
ZLOGE(TAG, "OT: %d", val.OT);
ZLOGE(TAG, "OTPW: %d", val.OTPW);
ZLOGE(TAG, "S2GA: %d", val.S2GA);
ZLOGE(TAG, "S2GB: %d", val.S2GB);
ZLOGE(TAG, "OLA: %d", val.OLA);
ZLOGE(TAG, "OLB: %d", val.OLB);
ZLOGE(TAG, "STST: %d", val.STST);
}
void StepMotorCtrlModule::dump(TMC2160MotorGstate_t val) {}
void StepMotorCtrlModule::dump(TMC4361AMotorEventState_t val) {}
void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) {
creg.module_errorcode = ecode;

10
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -84,6 +84,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) 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_tmc4361a_status(int32_t* status) override;
virtual int32_t step_motor_read_tmc4361a_state(int32_t* gstate) override;
virtual int32_t step_motor_read_tmc2160_status(int32_t* status) override;
virtual int32_t step_motor_read_tmc2160_state(int32_t* status) override;
public:
IStepperMotor* getMotor() { return m_stepM1; }
@ -118,5 +123,10 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
private:
void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
void setErrorFlag(int32_t ecode, uint32_t motorStatue);
void dump(TMC2160MotorDriverStatus_t val);
void dump(TMC4361AMotorStatus_t val);
void dump(TMC2160MotorGstate_t val);
void dump(TMC4361AMotorEventState_t val);
};
} // namespace iflytop

41
components/tmc/ic/ztmc4361A.cpp
View File

@ -1,4 +1,4 @@
#if 1
#include "ztmc4361A.hpp"
#ifdef HAL_SPI_MODULE_ENABLED
@ -18,7 +18,6 @@ using namespace iflytop;
pin->setState(val); \
}
#if 1
void TMC4361A::tmc4361_readWriteArray(uint8_t *data, size_t length) {
m_csgpio->setState(false);
// zchip_clock_early_delayus(10);
@ -203,6 +202,11 @@ uint8_t TMC4361A::reset() {
zchip_clock_early_delayus(1000);
SET_PIN(m_resetPin, true);
// 软件复位
tmc4361_writeInt(TMC4361A_RESET_REG, 0x525354);
zchip_clock_early_delayus(1000);
tmc4361_writeInt(TMC4361A_RESET_REG, 0);
/**
* @brief 重置芯片镜像寄存??
*
@ -299,7 +303,6 @@ bool TMC4361A::isReachTarget() {
}
}
/***********************************************************************************************************************
* DRIVERIC_OPERATION *
***********************************************************************************************************************/
@ -391,7 +394,35 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
return 0xFFFFFFFF - last + now;
}
}
#endif
#endif
TMC2160MotorDriverStatus_t TMC4361A::getTMC2160MotorDriverStatus() {
static_assert(sizeof(TMC2160MotorDriverStatus_t) == 4, "TMC2160MotorDriverStatus_t size not match");
TMC2160MotorDriverStatus_t status;
uint32_t ret = driverIC_readInt(TMC2160_DRV_STATUS);
memcpy(&status, &ret, sizeof(status));
return status;
}
TMC4361AMotorStatus_t TMC4361A::getTMC4361AMotorStatus() {
static_assert(sizeof(TMC4361AMotorStatus_t) == 4, "TMC4361AMotorStatus_t size not match");
TMC4361AMotorStatus_t status;
uint32_t ret = tmc4361_readInt(TMC4361A_STATUS);
memcpy(&status, &ret, sizeof(status));
return status;
}
TMC2160MotorGstate_t TMC4361A::getTMC2160MotorGstate() {
static_assert(sizeof(TMC2160MotorGstate_t) == 4, "TMC2160MotorGstate_t size not match");
TMC2160MotorGstate_t status;
uint32_t ret = driverIC_readInt(TMC2160_GSTAT);
memcpy(&status, &ret, sizeof(status));
return status;
}
TMC4361AMotorEventState_t TMC4361A::getTMC4361AMotorEventState() {
static_assert(sizeof(TMC4361AMotorEventState_t) == 4, "TMC4361AMotorEventState_t size not match");
TMC4361AMotorEventState_t status;
uint32_t ret = tmc4361_readInt(TMC4361A_EVENTS);
memcpy(&status, &ret, sizeof(status));
return status;
}
#endif

9
components/tmc/ic/ztmc4361A.hpp
View File

@ -7,6 +7,7 @@
#include <stdlib.h>
#include "../basic/tmc_ic_interface.hpp"
#include "a8000_protocol\api\zi_motor.hpp"
#include "sdk/os/zos.hpp"
extern "C" {
#include "TMC2160\TMC2160.h"
@ -15,6 +16,7 @@ extern "C" {
#ifdef HAL_SPI_MODULE_ENABLED
namespace iflytop {
#define TMC4361A_LISTENER_MAX 5
class TMC4361A : public IStepperMotor {
public:
@ -124,6 +126,13 @@ class TMC4361A : public IStepperMotor {
void driverIC_setMotorShaft(bool reverse);
uint32_t driverIC_readICVersion();
public:
TMC2160MotorDriverStatus_t getTMC2160MotorDriverStatus();
TMC4361AMotorStatus_t getTMC4361AMotorStatus();
TMC2160MotorGstate_t getTMC2160MotorGstate();
TMC4361AMotorEventState_t getTMC4361AMotorEventState();
private:
int32_t to_motor_acc(int32_t acc /* rpm/s^2*/);
int32_t to_motor_vel(int32_t vel /*rpm*/);

Write Preview
Loading…
Cancel
Save