369 lines
11 KiB
C
369 lines
11 KiB
C
#include "tmc2240.h"
|
||
|
||
tmc2240_t _tmc2240[TMC2240_MAX];
|
||
|
||
static void tmc2240_write(tmc2240_index_e index, uint8_t *data)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
|
||
|
||
tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
|
||
for (uint16_t i = 0; i < 5; i++)
|
||
{
|
||
tmc->spi->interface.u.normal.spi_send(tmc->spi, data[i]);
|
||
}
|
||
tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);
|
||
}
|
||
|
||
static void tmc2240_read(tmc2240_index_e index, uint8_t *wdata, uint8_t *rdata)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
|
||
|
||
tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
|
||
for (uint16_t i = 0; i < 5; i++)
|
||
{
|
||
rdata[i] = tmc->spi->interface.u.normal.spi_send(tmc->spi, wdata[i]);
|
||
}
|
||
tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);
|
||
|
||
__NOP();
|
||
__NOP();
|
||
__NOP();
|
||
|
||
tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
|
||
for (uint16_t i = 0; i < 5; i++)
|
||
{
|
||
rdata[i] = tmc->spi->interface.u.normal.spi_send(tmc->spi, wdata[i]);
|
||
}
|
||
tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);
|
||
}
|
||
|
||
static void tmc2240_reg_write(tmc2240_index_e index, uint8_t reg, uint32_t data)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
|
||
|
||
uint8_t wdata[5] = {0};
|
||
wdata[0] = TMC2240_HIGHT_BIT | reg;
|
||
wdata[1] = (data >> 24) & 0xFF;
|
||
wdata[2] = (data >> 16) & 0xFF;
|
||
wdata[3] = (data >> 8) & 0xFF;
|
||
wdata[4] = data & 0xFF;
|
||
tmc2240_write(index, wdata);
|
||
}
|
||
|
||
static uint32_t tmc2240_reg_read(tmc2240_index_e index, uint8_t reg)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
|
||
|
||
uint8_t wdata[5] = {0};
|
||
uint8_t rdata[5] = {0};
|
||
wdata[0] = reg;
|
||
tmc2240_read(index, wdata, rdata);
|
||
return (rdata[1] << 24) | (rdata[2] << 16) | (rdata[3] << 8) | rdata[4];
|
||
}
|
||
|
||
/**
|
||
* @brief 配置TMC2240步进电机驱动器
|
||
*
|
||
* 该函数用于配置TMC2240步进电机驱动器的各种参数，包括步进电机的工作模式、加减速曲线等。
|
||
*
|
||
* @param index TMC2240步进电机驱动器的索引，用于指定要配置的驱动器。
|
||
*/
|
||
static void tmc2240_config_write(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
tmc->config.gconf.data = 0x00000000;
|
||
|
||
tmc->config.chopconf.data = 0x00410153;
|
||
tmc->config.chopconf.bits.mres = TMC2240_MRES_8;
|
||
|
||
tmc->config.drvconf.data = 0x00000021;
|
||
tmc->config.global_scaler.data = 0x00000000;
|
||
|
||
tmc->config.ihold_irun.bits.ihold = 31;
|
||
tmc->config.ihold_irun.bits.irun = 31;
|
||
tmc->config.ihold_irun.bits.iholddelay = 0;
|
||
tmc->config.ihold_irun.bits.irundelay = 0;
|
||
|
||
tmc->config.pwmconf.data = 0xC44C261E;
|
||
tmc->config.gstat.data = 0x00000007;
|
||
tmc2240_reg_write(index, TMC2240_GCONF, tmc->config.gconf.data);
|
||
tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
|
||
tmc2240_reg_write(index, TMC2240_DRV_CONF, tmc->config.drvconf.data);
|
||
tmc2240_reg_write(index, TMC2240_GLOBAL_SCALER, tmc->config.global_scaler.data);
|
||
tmc2240_reg_write(index, TMC2240_IHOLD_IRUN, tmc->config.ihold_irun.data);
|
||
tmc2240_reg_write(index, TMC2240_PWMCONF, tmc->config.pwmconf.data);
|
||
tmc2240_reg_write(index, TMC2240_GSTAT, tmc->config.gstat.data);
|
||
}
|
||
|
||
static void _tmc2240_motor_update(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
switch (tmc->config.chopconf.bits.mres)
|
||
{
|
||
case TMC2240_MRES_256:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 256;
|
||
break;
|
||
case TMC2240_MRES_128:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 128;
|
||
break;
|
||
case TMC2240_MRES_64:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 64;
|
||
break;
|
||
case TMC2240_MRES_32:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 32;
|
||
break;
|
||
case TMC2240_MRES_16:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 16;
|
||
break;
|
||
case TMC2240_MRES_8:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 8;
|
||
break;
|
||
case TMC2240_MRES_4:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 4;
|
||
break;
|
||
case TMC2240_MRES_2:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 2;
|
||
break;
|
||
case TMC2240_MRES_1:
|
||
tmc->motor.step_angle = MOTOR_42_STEP_ANGLE;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
tmc->motor.circle_pulse = 360 / tmc->motor.step_angle;
|
||
}
|
||
|
||
static void _tmc2240_enable(tmc2240_index_e index, BOOL enable)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
BOOL state = tmc->en->read(*tmc->en) == 0 ? TRUE : FALSE;
|
||
|
||
if (state == enable)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (enable == TRUE)
|
||
{
|
||
PWM_START(tmc->timer, tmc->time_ch);
|
||
tmc->en->reset(*tmc->en);
|
||
tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
|
||
}
|
||
else
|
||
{
|
||
PWM_STOP(tmc->timer, tmc->time_ch);
|
||
tmc->en->set(*tmc->en);
|
||
chopconf_u chopconf;
|
||
osel_memset((uint8_t *)&chopconf, 0, sizeof(chopconf_u));
|
||
chopconf.bits.mres = TMC2240_MRES_1;
|
||
tmc2240_reg_write(index, TMC2240_CHOPCONF, chopconf.data);
|
||
}
|
||
}
|
||
|
||
static void _tmc2240_direction(tmc2240_index_e index, tmc2240_direction_e dir)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
if (dir == TMC2240_FORWARD)
|
||
{
|
||
tmc->dir->reset(*tmc->dir);
|
||
}
|
||
else
|
||
{
|
||
tmc->dir->set(*tmc->dir);
|
||
}
|
||
}
|
||
|
||
void tmc2240_init(tmc2240_index_e index, SPI_TypeDef *SPIx, TIM_TypeDef *timer, uint32_t time_ch, spi_gpio_group_t *gpios)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
osel_memset((uint8_t *)tmc, 0, sizeof(tmc2240_t));
|
||
|
||
tmc->timer = timer;
|
||
tmc->time_ch = time_ch;
|
||
tmc->spi = spi_create(SPI_TYPE_NORMAL, *gpios, 0);
|
||
DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
|
||
tmc->spi->interface.hardware_enable(tmc->spi, SPIx);
|
||
{
|
||
tmc->default_tm.sysclk = SystemCoreClock / 1000;
|
||
tmc->default_tm.psc = PWM_GET_PSC(tmc->timer);
|
||
tmc->default_tm.arr = PWM_GET_ARR(tmc->timer);
|
||
tmc->default_tm.freq = PWM_GET_FREQ(tmc->timer);
|
||
}
|
||
tmc->params.percent = TMC2240_PWM_DUTY_DEFAULT;
|
||
tmc->params.arr = 0;
|
||
tmc->params.freq = 0;
|
||
tmc->params.enable = FALSE;
|
||
tmc->params.direction = TMC2240_FORWARD;
|
||
tmc2240_config_write(index);
|
||
_tmc2240_motor_update(index);
|
||
}
|
||
|
||
tmc2240_t *tmc2240_get(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
return &_tmc2240[index];
|
||
}
|
||
|
||
void tmc2240_percent(tmc2240_index_e index, float32 percent)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
PWM_SET_DUTY(tmc->timer, tmc->time_ch, ABS(percent));
|
||
}
|
||
|
||
void tmc2240_config_read(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
tmc->read_config.gconf.data = tmc2240_reg_read(index, TMC2240_GCONF);
|
||
tmc->read_config.chopconf.data = tmc2240_reg_read(index, TMC2240_CHOPCONF);
|
||
tmc->read_config.drvconf.data = tmc2240_reg_read(index, TMC2240_DRV_CONF);
|
||
tmc->read_config.global_scaler.data = tmc2240_reg_read(index, TMC2240_GLOBAL_SCALER);
|
||
tmc->read_config.ihold_irun.data = tmc2240_reg_read(index, TMC2240_IHOLD_IRUN);
|
||
tmc->read_config.pwmconf.data = tmc2240_reg_read(index, TMC2240_PWMCONF);
|
||
tmc->read_config.gstat.data = tmc2240_reg_read(index, TMC2240_GSTAT);
|
||
tmc->data.tmc2240_adc_temp = tmc2240_reg_read(index, TMC2240_ADC_TEMP);
|
||
tmc->data.tmc2240_temperature = (float32)(tmc->data.tmc2240_adc_temp - 2038) / 7.7;
|
||
}
|
||
|
||
void tmc2240_test(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
_tmc2240_enable(index, tmc->params.enable);
|
||
|
||
if (tmc->params.enable == TRUE)
|
||
{
|
||
_tmc2240_direction(index, tmc->params.direction);
|
||
|
||
if (PWM_GET_ARR(tmc->timer) != tmc->params.arr)
|
||
{
|
||
if (tmc->params.arr == 0)
|
||
{
|
||
tmc->params.arr = tmc->default_tm.arr;
|
||
}
|
||
PWM_SET_ARR(tmc->timer, tmc->params.arr);
|
||
tmc->params.freq = PWM_GET_FREQ(tmc->timer);
|
||
}
|
||
|
||
tmc2240_percent(index, tmc->params.percent);
|
||
|
||
if (tmc->config.chopconf.bits.mres != tmc->read_config.chopconf.bits.mres)
|
||
{
|
||
tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
|
||
_tmc2240_motor_update(index);
|
||
}
|
||
}
|
||
}
|
||
|
||
void tmc2240_motor_set_angle(tmc2240_index_e index, int32_t angle)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
if (angle == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
tmc->params.enable = FALSE;
|
||
_tmc2240_enable(index, tmc->params.enable);
|
||
tmc->motor.pulse_count = 0;
|
||
|
||
if (angle > 0)
|
||
{
|
||
tmc->params.direction = TMC2240_FORWARD;
|
||
}
|
||
else
|
||
{
|
||
tmc->params.direction = TMC2240_BACKWARD;
|
||
}
|
||
|
||
tmc->motor.pulse_count = ABS(angle) / tmc->motor.step_angle;
|
||
tmc->motor.step_angle_count = 0;
|
||
|
||
tmc->params.enable = TRUE;
|
||
|
||
_tmc2240_direction(index, tmc->params.direction);
|
||
if (PWM_GET_ARR(tmc->timer) != tmc->params.arr)
|
||
{
|
||
if (tmc->params.arr == 0)
|
||
{
|
||
tmc->params.arr = tmc->default_tm.arr;
|
||
}
|
||
PWM_SET_ARR(tmc->timer, tmc->params.arr);
|
||
tmc->params.freq = PWM_GET_FREQ(tmc->timer);
|
||
}
|
||
|
||
tmc2240_percent(index, tmc->params.percent);
|
||
|
||
if (tmc->config.chopconf.bits.mres != tmc->read_config.chopconf.bits.mres)
|
||
{
|
||
tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
|
||
_tmc2240_motor_update(index);
|
||
}
|
||
_tmc2240_enable(index, tmc->params.enable);
|
||
}
|
||
|
||
void tmc2240_motor_update(tmc2240_index_e index)
|
||
{
|
||
DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
|
||
tmc2240_t *tmc = &_tmc2240[index];
|
||
DBG_ASSERT(tmc != NULL __DBG_LINE);
|
||
|
||
if (tmc->motor.pulse_count > 0)
|
||
{
|
||
tmc->motor.pulse_count--; // 脉冲数
|
||
tmc->motor.step_angle_count++; // 步距个数
|
||
if (tmc->params.direction == TMC2240_FORWARD)
|
||
{
|
||
tmc->motor.add_pulse_count++; /* 绝对位置++ */
|
||
}
|
||
else
|
||
{
|
||
tmc->motor.add_pulse_count--; /* 绝对位置-- */
|
||
}
|
||
}
|
||
|
||
/* 当脉冲数等于0的时候 代表需要发送的脉冲个数已完成，停止输出 */
|
||
|
||
if (tmc->motor.pulse_count <= 0)
|
||
{
|
||
tmc->params.enable = FALSE;
|
||
_tmc2240_enable(index, tmc->params.enable);
|
||
}
|
||
}
|