215 lines
6.1 KiB
C
215 lines
6.1 KiB
C
//
|
||
// Created by hu123456 on 2024/1/16.
|
||
//
|
||
|
||
#include "tle5012b.h"
|
||
long cpr =32767;
|
||
float motory_full_rotation_offset=0,motorx_full_rotation_offset=0;
|
||
long motory_angle_data_prev =0,motorx_angle_data_prev = 0;
|
||
|
||
|
||
//void tle5012b_init(void)
|
||
//{
|
||
// // ³õʼ»¯TLE5012B´«¸ÐÆ÷
|
||
// GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
// __HAL_RCC_GPIOA_CLK_ENABLE();
|
||
//
|
||
// GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5;
|
||
// GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||
// GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5, GPIO_PIN_RESET);
|
||
//
|
||
// HAL_Delay(10);
|
||
// getAngle(motory);
|
||
// getAngle(motorx);
|
||
//}
|
||
//
|
||
//void mosi()
|
||
//{
|
||
// GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
// __HAL_RCC_GPIOA_CLK_ENABLE();
|
||
//
|
||
// GPIO_InitStruct.Pin = GPIO_PIN_7;
|
||
// GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||
// GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
//
|
||
// /*Configure GPIO pins : PBPin PB14 */
|
||
// GPIO_InitStruct.Pin = GPIO_PIN_6;
|
||
// GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||
// GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
//}
|
||
//
|
||
//void miso()
|
||
//{
|
||
// GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
// __HAL_RCC_GPIOA_CLK_ENABLE();
|
||
//
|
||
// GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
|
||
// GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||
// GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
//}
|
||
//
|
||
//static void Write_16bit(unsigned short dat)
|
||
//{
|
||
// mosi();
|
||
// for(uint8_t i = 0; i < 16; i++)
|
||
// {
|
||
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
|
||
//
|
||
// if(dat&0x8000) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,1);
|
||
// else HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,0);
|
||
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
|
||
// dat <<= 1;
|
||
// }
|
||
//}
|
||
//
|
||
//static void Read_16bit(unsigned short *dat)
|
||
//{
|
||
// miso();
|
||
// *dat = 0;
|
||
// for(uint8_t i = 0; i < 16; i++) {
|
||
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
|
||
// *dat <<= 1;
|
||
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
|
||
// *dat |= HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_6) ? 1 : 0;
|
||
// }
|
||
//}
|
||
//
|
||
//uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
|
||
//{
|
||
// uint16_t u16Data;
|
||
// Write_16bit(u16RegValue);
|
||
// Read_16bit(&u16Data);
|
||
// return u16Data;
|
||
//}
|
||
//
|
||
//double getAngle(unsigned char motor)
|
||
//{
|
||
// float angle_data,d_angle;
|
||
// if(motor == motory)
|
||
// {
|
||
// TLE_CS1_ENABLE;
|
||
// angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
|
||
// d_angle = angle_data - motory_angle_data_prev;
|
||
// if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
|
||
// motory_angle_data_prev = angle_data;
|
||
// TLE_CS1_DISABLE;
|
||
// return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
|
||
// }
|
||
// else
|
||
// {
|
||
// TLE_CS2_ENABLE;
|
||
// angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
|
||
// d_angle = angle_data - motorx_angle_data_prev;
|
||
// if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
|
||
// motorx_angle_data_prev = angle_data;
|
||
// TLE_CS2_DISABLE;
|
||
// return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
|
||
// }
|
||
//}
|
||
/******************************************************************************/
|
||
|
||
|
||
|
||
///////Ó²¼þ
|
||
|
||
|
||
void tle5012b_init(void)
|
||
{
|
||
// ³õʼ»¯TLE5012B´«¸ÐÆ÷
|
||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||
|
||
GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4, GPIO_PIN_RESET);
|
||
|
||
HAL_Delay(10);
|
||
getAngle(motory);
|
||
getAngle(motorx);
|
||
FOCStruct_X.Angle_Prev = getAngle(motorx);
|
||
FOCStruct_Y.Angle_Prev = getAngle(motory);
|
||
}
|
||
|
||
void SPI2_TX_OFF()
|
||
{
|
||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||
/**SPI1 GPIO Configuration
|
||
PA5 ------> SPI1_SCK
|
||
PA6 ------> SPI1_MISO
|
||
PA7 ------> SPI1_MOSI
|
||
*/
|
||
GPIO_InitStruct.Pin = GPIO_PIN_7;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
}
|
||
|
||
void SPI2_TX_ON()
|
||
{
|
||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||
GPIO_InitStruct.Pin = GPIO_PIN_7;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
|
||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
}
|
||
|
||
|
||
double getAngle(unsigned char motor)
|
||
{
|
||
long angle_data,d_angle;
|
||
if(motor == motory)
|
||
{
|
||
TLE_CS1_ENABLE;
|
||
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
|
||
d_angle = angle_data - motory_angle_data_prev;
|
||
if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
|
||
motory_angle_data_prev = angle_data;
|
||
TLE_CS1_DISABLE;
|
||
return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
|
||
}
|
||
else
|
||
{
|
||
TLE_CS2_ENABLE;
|
||
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
|
||
d_angle = angle_data - motorx_angle_data_prev;
|
||
if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
|
||
motorx_angle_data_prev = angle_data;
|
||
TLE_CS2_DISABLE;
|
||
return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
|
||
}
|
||
}
|
||
|
||
|
||
uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
|
||
{
|
||
uint16_t u16Data;
|
||
HAL_SPI_Transmit( &hspi1, (uint8_t *)(&u16RegValue), sizeof(u16RegValue)/sizeof(uint16_t), 0xff );
|
||
SPI2_TX_OFF();
|
||
HAL_SPI_Receive( &hspi1,(uint8_t *)(&u16Data), sizeof(u16Data)/sizeof(uint16_t), 0xff );
|
||
SPI2_TX_ON();
|
||
return (u16Data & 0x7FFF);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
//uint16_t ReadSpeed(void)
|
||
//{
|
||
// return ReadTLE5012B_1(READ_SPEED_VALUE);
|
||
//}
|