cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

FOC开环调试成功

Browse Source
This commit is contained in:
cbb
2025-10-16 21:43:19 +08:00
parent 2d807260c5
commit 35801d6f48
13 changed files with 704 additions and 509 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/tle5012b.cpp
+137 -112
View File
@@ -1,6 +1,9 @@
#include "tle5012b.h"
#include "core_cmInstr.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "stdbool.h"
#include "systick.h"
#define SPI_TX_OFF() \
do { \
@@ -64,21 +67,19 @@ static constexpr uint16_t MOD4_VALUE = 0x0080; /**< MOD4 register value:
uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
{
uint32_t to;
uint16_t data = 0xFFFF;
bool ok = true;
uint16_t data = 0xFFFF;
bool ok = true;
/* ---------- Send data ---------- */
to = 200;
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to);
if (to == 0) ok = false;
if (ok)
{
if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE))
if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE))
{
(void)spi_i2s_data_receive(spi_periph); /* throw Dummy */
(void)spi_i2s_data_receive(spi_periph); /* throw Dummy */
}
spi_i2s_data_transmit(spi_periph, i_HalfWord);
/* wait rx succ*/
@@ -89,7 +90,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
data = spi_i2s_data_receive(spi_periph); /* throw Dummy */
}
/* ---------- wait bus idle ---------- */
/* ---------- wait bsu idle ---------- */
if (ok)
{
to = 200;
@@ -100,6 +101,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
return ok ? data : 0xFFFF;
}
/**
* @brief Write data to TLI5012B register
* @param i_Cmd Command word to send
@@ -111,12 +113,16 @@ uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data)
{
uint16_t result = 0xFFFF; /* Default error code */
SPI_CS_ENABLE();
SPI_TX_ON();
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
result = spi_rw_half_word(SPI0, i_Cmd);
result = spi_rw_half_word(SPI0, i_Data);
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
// result = spi_rw_half_word(SPI0, result);
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
result = spi_rw_half_word(SPI0, DUMMY_BYTE);
SPI_CS_DISABLE();
SPI_TX_ON(); /* Restore MOSI as output */
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
return result;
}
@@ -131,12 +137,16 @@ uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data)
{
uint16_t result = 0xFFFF; /* Default error code */
SPI_CS_ENABLE();
SPI_TX_ON();
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
*i_Data = spi_rw_half_word(SPI0, i_Cmd);
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
// *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);
result = spi_rw_half_word(SPI0, result);
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
*i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);
result = spi_rw_half_word(SPI0, DUMMY_BYTE);
SPI_CS_DISABLE();
SPI_TX_ON(); /* Restore MOSI as output */
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
return result;
}
@@ -146,16 +156,17 @@ void tle5012b_gpio_init(void)
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_SPI0);
rcu_periph_clock_enable(RCU_AF);
/* JTAG-DP disabled and SW-DP enabled, so SPI0 can use PB3 and PB4 */
/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
/* SPI0 GPIO config: NSS/PA15, SCK/PB3, MOSI/PB5 */
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
/* PA15 → NSS, 推挽输出 */
gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5);
/* SPI0 GPIO config: MISO/PB4 */
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4);
/* PB4 → MISO, 浮空输入 */
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4 | GPIO_PIN_10);
}
/**
@@ -168,21 +179,20 @@ void tle5012b_spi_init(void)
spi_parameter_struct spi_init_str;
spi_struct_para_init(&spi_init_str);
/* SPI0 parameter config */
/* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */
spi_init_str.device_mode = SPI_MASTER;
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
spi_init_str.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;
spi_init_str.nss = SPI_NSS_SOFT;
spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
spi_init_str.nss = SPI_NSS_SOFT;
spi_init_str.endian = SPI_ENDIAN_MSB;
spi_init_str.prescale = SPI_PSC_128;
spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */
spi_init(SPI0, &spi_init_str);
spi_nss_output_enable(SPI0);
spi_nss_output_enable(SPI0);
spi_nssp_mode_enable(SPI0);
spi_enable(SPI0);
}
spi_enable(SPI0);
}
@@ -196,119 +206,134 @@ void cdd_TLI5012_Init(void)
uint16_t u16Command = 0xFFFF;
uint16_t u16Data = 0x0000;
uint16_t u16Safe = 0x0000;
/* ---------- Step 1: Read and configure STAT register ---------- */
/* Read STAT register to get current status */
u16Command = READ_STAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write STAT register, set Slave_Number = 00 */
u16Command = 0x0001;
u16Data &= 0x9FFF; /* Clear bits 13-14 (Slave_Number) */
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Read STAT register again to verify write operation */
u16Command = READ_STAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 2: Read ACSTAT register ---------- */
u16Command = READ_ACSTAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 3: Configure MOD1 register ---------- */
/* Read current MOD1 register value */
u16Command = RD_REG | WRITE_MOD1_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD1 register with configuration value */
u16Command = WRITE_MOD1_VALUE;
u16Data = MOD1_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD1 register write operation */
u16Command = RD_REG | WRITE_MOD1_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 4: Configure MOD2 register ---------- */
/* Read current MOD2 register value */
u16Command = RD_REG | WRITE_MOD2_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD2 register with configuration value */
u16Command = WRITE_MOD2_VALUE;
u16Data = MOD2_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD2 register write operation */
u16Command = RD_REG | WRITE_MOD2_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD1 register with configuration value */
// u16Command = WRITE_MOD1_VALUE;
// u16Data = MOD1_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD1 register write operation */
// u16Command = RD_REG | WRITE_MOD1_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 4: Configure MOD2 register ---------- */
// /* Read current MOD2 register value */
// u16Command = RD_REG | WRITE_MOD2_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD2 register with configuration value */
// u16Command = WRITE_MOD2_VALUE;
// u16Data = MOD2_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD2 register write operation */
// u16Command = RD_REG | WRITE_MOD2_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 5: Configure MOD3 register ---------- */
// /* Read current MOD3 register value */
// u16Command = RD_REG | WRITE_MOD3_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD3 register with configuration value */
// u16Command = WRITE_MOD3_VALUE;
// u16Data = MOD3_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD3 register write operation */
// u16Command = RD_REG | WRITE_MOD3_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Optional: Configure MOD3 register with SSC_OD = 1 (commented out) */
// u16Command = 0x5091;
// u16Data |= 0x0004; // Set SSC_OD bit
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 6: Configure MOD4 register ---------- */
// /* Read current MOD4 register value */
// u16Command = RD_REG | WRITE_MOD4_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD4 register with configuration value */
// u16Command = WRITE_MOD4_VALUE;
// u16Data = MOD4_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD4 register write operation */
// u16Command = RD_REG | WRITE_MOD4_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 5: Configure MOD3 register ---------- */
/* Read current MOD3 register value */
u16Command = RD_REG | WRITE_MOD3_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD3 register with configuration value */
u16Command = WRITE_MOD3_VALUE;
u16Data = MOD3_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD3 register write operation */
u16Command = RD_REG | WRITE_MOD3_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 6: Configure MOD4 register ---------- */
/* Read current MOD4 register value */
u16Command = RD_REG | WRITE_MOD4_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD4 register with configuration value */
u16Command = WRITE_MOD4_VALUE;
u16Data = MOD4_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD4 register write operation */
u16Command = RD_REG | WRITE_MOD4_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
}
void Test_TLI5012B(void)
{
uint16_t u16Command = READ_ANGLE_VALUE;
uint16_t u16Command = 0xFFFF;
uint16_t u16Data = 0x0000;
uint16_t u16Safe = 0x0000;
float angle = 0;
float angle = 0;
u16Command = READ_ANGLE_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// 输出原始读取的数据，用于诊断
printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// 检查是否有错误标志位
if (u16Safe != 0xFFFF && u16Safe != 0x0000) {
printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n");
}
// 检查错误位（bit 15）
if (u16Data & 0x8000) {
printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n");
}
// 提取角度值(15位)
u16Data = u16Data & 0x7FFF;
printf ( "\r\n AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// angle = 360*( ((int16_t)(u16Data << 1)) >> 1)/32768;
angle = ( 360*( ( (int16_t)(u16Data << 1) ) >> 1 ) ) >> 15;
printf ( "\r\n angle = %f, AVAL = %d \r\n", angle, (((int16_t)(u16Data << 1)) >> 1));
}
printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// 将15位角度值转换为角度(0-360度)
// 0x7FFF = 32767 对应 360度
angle = ((float)u16Data * 360.0f) / 32767.0f;
printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data);
// 添加额外的传感器状态检查
uint16_t status_data, status_safe;
u16Command = READ_STAT_VALUE;
status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data);
printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe);
}