379 lines
12 KiB
C
379 lines
12 KiB
C
#include <string.h>
|
|
#include <stdio.h>
|
|
#include "cmsis_os.h"
|
|
#include "TMC2240_EVAL.h"
|
|
#include "motor.h"
|
|
#include "spi.h"
|
|
#include "communication.h"
|
|
|
|
uint8_t motor_X = MOTOR_A;
|
|
|
|
encoder_t encoder_A;
|
|
encoder_t encoder_B;
|
|
|
|
motor_t motor_A;
|
|
motor_t motor_B;
|
|
|
|
uint8_t TxData[4];
|
|
uint8_t RxData[4];
|
|
|
|
void fun_ini_encoder(TIM_HandleTypeDef *htim, encoder_t *encoder)
|
|
{
|
|
if ((!htim) || (!encoder))
|
|
return;
|
|
|
|
memset(encoder, 0, sizeof(encoder_t));
|
|
HAL_TIM_Encoder_Start(htim, TIM_CHANNEL_ALL);
|
|
if (htim == &htim3)
|
|
encoder->index = 1;
|
|
else if (htim == &htim1)
|
|
encoder->index = 2;
|
|
}
|
|
|
|
void fun_get_encoder(TIM_HandleTypeDef *htim, encoder_t *encoder)
|
|
{
|
|
if (!encoder)
|
|
return;
|
|
|
|
uint8_t *direction = &encoder->direction;
|
|
uint16_t *enc1 = &encoder->enc1;
|
|
uint16_t *enc1_old = &encoder->enc1_old;
|
|
int16_t *enc2 = &encoder->enc2;
|
|
int32_t *enc = &encoder->enc;
|
|
|
|
*direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim);
|
|
*enc1 = (uint32_t)(__HAL_TIM_GET_COUNTER(htim));
|
|
if ((*direction == 0) && (*enc1 < *enc1_old))
|
|
{
|
|
(*enc2)++;
|
|
}
|
|
if ((*direction == 1) && (*enc1 > *enc1_old))
|
|
{
|
|
(*enc2)--;
|
|
}
|
|
*enc1_old = *enc1;
|
|
|
|
*enc = *enc2 << 16 | *enc1;
|
|
}
|
|
|
|
static void fun_ctr_tmc2240a_nss(uint8_t index, BOOL en)
|
|
{
|
|
if (index == MOTOR_A)
|
|
{
|
|
if (en == TRUE)
|
|
TMC2240A_A_NSS_L();
|
|
else if (en == FALSE)
|
|
TMC2240A_A_NSS_H();
|
|
}
|
|
else if (index == MOTOR_B)
|
|
{
|
|
if (en == TRUE)
|
|
TMC2240A_B_NSS_L();
|
|
else if (en == FALSE)
|
|
TMC2240A_B_NSS_H();
|
|
}
|
|
}
|
|
|
|
static void fun_read_tmc2240a(uint8_t index, uint8_t reg, uint32_t *dat)
|
|
{
|
|
uint8_t dat1, dat2, dat3, dat4;
|
|
|
|
dat1 = 0x00;
|
|
dat2 = 0x00;
|
|
dat3 = 0x00;
|
|
dat4 = 0x00;
|
|
|
|
fun_ctr_tmc2240a_nss(index, ENABLE);
|
|
HAL_SPI_Transmit(&hspi2, ®, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat1, 1, TMC2240A_TIMEOUT_VALUE);
|
|
fun_ctr_tmc2240a_nss(index, DISABLE);
|
|
|
|
fun_ctr_tmc2240a_nss(index, ENABLE);
|
|
HAL_SPI_Transmit(&hspi2, ®, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_TransmitReceive(&hspi2, &dat4, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_TransmitReceive(&hspi2, &dat3, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_TransmitReceive(&hspi2, &dat2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_TransmitReceive(&hspi2, &dat1, &dat1, 1, TMC2240A_TIMEOUT_VALUE);
|
|
|
|
*dat = (dat4 << 24) + (dat3 << 16) + (dat2 << 8) + dat1;
|
|
//*dat = dat2 & 0x1f;
|
|
//*dat = dat1 & 0x1f;
|
|
|
|
fun_ctr_tmc2240a_nss(index, DISABLE);
|
|
}
|
|
|
|
static void fun_write_tmc2240a(uint8_t index, uint8_t reg, uint32_t dat)
|
|
{
|
|
uint8_t dat1, dat2, dat3, dat4;
|
|
|
|
dat1 = dat;
|
|
dat2 = dat >> 8;
|
|
dat3 = dat >> 16;
|
|
dat4 = dat >> 24;
|
|
|
|
fun_ctr_tmc2240a_nss(index, TRUE);
|
|
vTaskDelay(10);
|
|
reg |= 0x80;
|
|
HAL_SPI_Transmit(&hspi2, ®, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
|
|
HAL_SPI_Transmit(&hspi2, &dat1, 1, TMC2240A_TIMEOUT_VALUE);
|
|
fun_ctr_tmc2240a_nss(index, FALSE);
|
|
vTaskDelay(10);
|
|
}
|
|
/*
|
|
0x00 = 0x00002108 ;; writing GCONF @ address 0=0x00 with 0x00002108=8456=0.0
|
|
0x03 = 0x00000000 ;; writing SLAVECONF @ address 1=0x03 with 0x00000000=0=0.0
|
|
0x04 = 0x4001682C ;; writing IOIN @ address 2=0x04 with 0x4001682C=1073834028=0.0
|
|
0x0A = 0x00000021 ;; writing DRV_CONF @ address 3=0x0A with 0x00000021=33=0.0
|
|
0x0B = 0x00000000 ;; writing GLOBAL_SCALER @ address 4=0x0B with 0x00000000=0=0.0
|
|
0x10 = 0x00001208 ;; writing IHOLD_IRUN @ address 5=0x10 with 0x00001208=4616=0.0
|
|
0x11 = 0x00000000 ;; writing TPOWERDOWN @ address 6=0x11 with 0x00000000=0=0.0
|
|
0x13 = 0x00000000 ;; writing TPWMTHRS @ address 7=0x13 with 0x00000000=0=0.0
|
|
0x14 = 0x000003BE ;; writing TCOOLTHRS @ address 8=0x14 with 0x000003BE=958=0.0
|
|
0x15 = 0x00000000 ;; writing THIGH @ address 9=0x15 with 0x00000000=0=0.0
|
|
0x2D = 0x00000000 ;; writing DIRECT_MODE @ address 10=0x2D with 0x00000000=0=0.0
|
|
0x38 = 0x00000000 ;; writing ENCMODE @ address 11=0x38 with 0x00000000=0=0.0
|
|
0x39 = 0x00000000 ;; writing X_ENC @ address 12=0x39 with 0x00000000=0=0.0
|
|
0x3A = 0x00010000 ;; writing ENC_CONST @ address 13=0x3A with 0x00010000=65536=0.0
|
|
0x52 = 0x0B920F25 ;; writing OTW_OV_VTH @ address 14=0x52 with 0x0B920F25=194121509=0.0
|
|
0x60 = 0xAAAAB554 ;; writing MSLUT[0] @ address 15=0x60 with 0xAAAAB554=0=0.0
|
|
0x61 = 0x4A9554AA ;; writing MSLUT[1] @ address 16=0x61 with 0x4A9554AA=1251300522=0.0
|
|
0x62 = 0x24492929 ;; writing MSLUT[2] @ address 17=0x62 with 0x24492929=608774441=0.0
|
|
0x63 = 0x10104222 ;; writing MSLUT[3] @ address 18=0x63 with 0x10104222=269500962=0.0
|
|
0x64 = 0xFBFFFFFF ;; writing MSLUT[4] @ address 19=0x64 with 0xFBFFFFFF=0=0.0
|
|
0x65 = 0xB5BB777D ;; writing MSLUT[5] @ address 20=0x65 with 0xB5BB777D=0=0.0
|
|
0x66 = 0x49295556 ;; writing MSLUT[6] @ address 21=0x66 with 0x49295556=1227445590=0.0
|
|
0x67 = 0x00404222 ;; writing MSLUT[7] @ address 22=0x67 with 0x00404222=4211234=0.0
|
|
0x68 = 0xFFFF8056 ;; writing MSLUTSEL @ address 23=0x68 with 0xFFFF8056=0=0.0
|
|
0x69 = 0x00F70000 ;; writing MSLUTSTART @ address 24=0x69 with 0x00F70000=16187392=0.0
|
|
|
|
0x6C = 0x00410153 ;; writing CHOPCONF @ address 25=0x6C with 0x00410153=4260179=0.0
|
|
0x6D = 0x00040000 ;; writing COOLCONF @ address 26=0x6D with 0x00040000=262144=0.0
|
|
0x70 = 0xC44C001E ;; writing PWMCONF @ address 27=0x70 with 0xC44C001E=0=0.0
|
|
0x74 = 0x00000000 ;; writing SG4_THRS @ address 28=0x74 with 0x00000000=0=0.0
|
|
*/
|
|
/*
|
|
static void fun_config_tmc2240a(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
uint8_t *index = &motor->index;
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_GCONF, 0x00002108);
|
|
fun_write_tmc2240a(*index, TMC2240_SLAVECONF, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_IOIN, 0x4001682C);
|
|
fun_write_tmc2240a(*index, TMC2240_DRV_CONF, 0x00000021);
|
|
fun_write_tmc2240a(*index, TMC2240_GLOBAL_SCALER, 0x00000000);
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_IHOLD_IRUN, 0x00001208);
|
|
fun_write_tmc2240a(*index, TMC2240_TPOWERDOWN, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_TPWMTHRS, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_TCOOLTHRS, 0x000003BE);
|
|
fun_write_tmc2240a(*index, TMC2240_THIGH, 0x00000000);
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_DIRECT_MODE, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_ENCMODE, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_XENC, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_ENC_CONST, 0x00010000);
|
|
fun_write_tmc2240a(*index, TMC2240_OTW_OV_VTH, 0x0B920F25);
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT0, 0xAAAAB554);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT1, 0x4A9554AA);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT2, 0x24492929);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT3, 0x10104222);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT4, 0xFBFFFFFF);
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT5, 0xB5BB777D);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT6, 0x49295556);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUT7, 0x00404222);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUTSEL, 0xFFFF8056);
|
|
fun_write_tmc2240a(*index, TMC2240_MSLUTSTART, 0x00F70000);
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_CHOPCONF, 0x00410153);
|
|
fun_write_tmc2240a(*index, TMC2240_COOLCONF, 0x00040000);
|
|
fun_write_tmc2240a(*index, TMC2240_PWMCONF, 0xC44C001E);
|
|
fun_write_tmc2240a(*index, TMC2240_SG4_THRS, 0x00000000);
|
|
}
|
|
*/
|
|
|
|
static void fun_config_tmc2240a(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
uint8_t *index = &motor->index;
|
|
|
|
fun_write_tmc2240a(*index, TMC2240_GCONF, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_CHOPCONF, 0x00410153); // bit27 - bit24 配置细分
|
|
fun_write_tmc2240a(*index, TMC2240_DRV_CONF, 0x00000021); // bit1-bit0 配置电流x1:0.5A
|
|
fun_write_tmc2240a(*index, TMC2240_GLOBAL_SCALER, 0x00000000);
|
|
fun_write_tmc2240a(*index, TMC2240_IHOLD_IRUN, 0x00071f1f);
|
|
fun_write_tmc2240a(*index, TMC2240_PWMCONF, 0xC44C261E);
|
|
fun_write_tmc2240a(*index, TMC2240_GSTAT, 0x00000007);
|
|
}
|
|
|
|
void fun_ini_tmc2240a(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
memset(motor, 0, sizeof(motor_t));
|
|
|
|
if (motor == &motor_A)
|
|
motor->index = MOTOR_A;
|
|
else if (motor == &motor_B)
|
|
motor->index = MOTOR_B;
|
|
|
|
motor->en = FALSE;
|
|
motor->Sn = 2.0f;
|
|
motor->ctr_cmd = CMD_NO_PROC;
|
|
motor->pwm = PWM_DEFAULT;
|
|
motor->freq = FREQ_DEFAULT;
|
|
motor->load_sv = LOAD_DEFAULT;
|
|
|
|
fun_config_tmc2240a(motor);
|
|
}
|
|
|
|
static void fun_set_motor_enable(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
uint8_t *index = &motor->index;
|
|
BOOL *en = &motor->en;
|
|
uint32_t *pwm = &motor->pwm;
|
|
|
|
if (*en == TRUE)
|
|
{
|
|
__HAL_TIM_SET_COUNTER(&htim4, 0);
|
|
HAL_TIM_Base_Start_IT(&htim4);
|
|
if (*index == MOTOR_A)
|
|
{
|
|
TMC2240A_A_EN_L();
|
|
// TMC2240A_B_EN_H();
|
|
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, *pwm);
|
|
HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_3);
|
|
}
|
|
|
|
if (*index == MOTOR_B)
|
|
{
|
|
// TMC2240A_A_EN_H();
|
|
TMC2240A_B_EN_L();
|
|
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, *pwm);
|
|
HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_4);
|
|
}
|
|
}
|
|
else if (*en == FALSE)
|
|
{
|
|
if (*index == MOTOR_A)
|
|
{
|
|
HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_3);
|
|
TMC2240A_A_EN_H();
|
|
}
|
|
|
|
else if (*index == MOTOR_B)
|
|
{
|
|
HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_4);
|
|
TMC2240A_B_EN_H();
|
|
}
|
|
HAL_TIM_Base_Stop_IT(&htim4);
|
|
}
|
|
}
|
|
|
|
static void fun_set_motor_direction(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
uint8_t *index = &motor->index;
|
|
uint8_t *direction = &motor->direction;
|
|
|
|
if (*direction == DIR_FORWARD)
|
|
{
|
|
if (*index == MOTOR_A)
|
|
TMC2240A_A_DIR_H();
|
|
else if (*index == MOTOR_B)
|
|
TMC2240A_B_DIR_H();
|
|
}
|
|
else if (*direction == DIR_REVERSED)
|
|
{
|
|
if (*index == MOTOR_A)
|
|
TMC2240A_A_DIR_L();
|
|
else if (*index == MOTOR_B)
|
|
TMC2240A_B_DIR_L();
|
|
}
|
|
}
|
|
|
|
static void fun_set_motor_speed(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *en = &motor->en;
|
|
uint8_t *index = &motor->index;
|
|
uint32_t freq = motor->freq;
|
|
uint32_t pwm = motor->pwm;
|
|
|
|
uint32_t arr = 1000000 / freq - 1;
|
|
uint32_t ccr = pwm * arr / 100;
|
|
uint32_t *load_pv = &motor->load_pv;
|
|
|
|
if (*en == FALSE)
|
|
return;
|
|
|
|
fun_read_tmc2240a(*index, TMC2240_SG4_RESULT, load_pv);
|
|
|
|
__HAL_TIM_SET_AUTORELOAD(&htim4, arr);
|
|
|
|
if (*index == MOTOR_A)
|
|
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, ccr);
|
|
else if (*index == MOTOR_B)
|
|
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, ccr);
|
|
}
|
|
|
|
void fun_set_motor(motor_t *motor)
|
|
{
|
|
fun_set_motor_enable(motor);
|
|
fun_set_motor_direction(motor);
|
|
fun_set_motor_speed(motor);
|
|
}
|
|
|
|
void fun_pwm_it_callback(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *en = &motor->en;
|
|
BOOL force_near_flag = motor->force_near_flag;
|
|
BOOL force_far_flag = motor->force_far_flag;
|
|
uint32_t *step = &motor->step;
|
|
uint32_t *step_count = &motor->step_count;
|
|
BOOL *end_flag = &motor->end_flag;
|
|
uint8_t ctr_cmd = motor->ctr_cmd;
|
|
|
|
if (!motor->position_zero)
|
|
return;
|
|
|
|
if ((ctr_cmd == CMD_ZERO_DIST) || (ctr_cmd == CMD_RATED_DIST) || (ctr_cmd == CMD_GET_TIME))
|
|
//if ((ctr_cmd == CMD_ZERO_DIST) || (ctr_cmd == CMD_RATED_DIST))
|
|
return;
|
|
|
|
if (*en == FALSE)
|
|
return;
|
|
|
|
if ((*step_count)++ > *step)
|
|
{
|
|
*en = FALSE;
|
|
*step_count = 0;
|
|
*step = 0;
|
|
if ((!force_near_flag) && (!force_far_flag))
|
|
*end_flag = TRUE;
|
|
}
|
|
}
|