2025-03-12 23:04:18 +08:00
|
|
|
|
#include "read_spi.h"
|
|
|
|
|
|
|
|
|
|
float angle_encoder;
|
|
|
|
|
volatile int32_t multi_turn_angle = 0;
|
|
|
|
|
volatile uint16_t last_angle = 0;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 计算奇偶函数
|
|
|
|
|
uint16_t Parity_bit_Calculate(uint16_t data_2_cal)
|
|
|
|
|
{
|
|
|
|
|
uint16_t parity_bit_value=0;
|
|
|
|
|
while(data_2_cal != 0)
|
|
|
|
|
{
|
|
|
|
|
parity_bit_value ^= data_2_cal;
|
|
|
|
|
data_2_cal >>=1;
|
|
|
|
|
}
|
|
|
|
|
return (parity_bit_value & 0x1);
|
|
|
|
|
}
|
|
|
|
|
// SPI发送读取函数
|
|
|
|
|
uint16_t SPI_ReadWrite_OneByte(uint16_t _txdata)
|
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
//AS5407P_CS_L cs拉低
|
|
|
|
|
AS5407P_CS_L();
|
|
|
|
|
uint16_t rxdata;
|
|
|
|
|
if(HAL_SPI_TransmitReceive(&hspi2,(uint8_t *)&_txdata,(uint8_t *)&rxdata,1,1000) !=HAL_OK)
|
|
|
|
|
rxdata=0;
|
|
|
|
|
//AS5407P_CS_H //cs拉高
|
|
|
|
|
AS5407P_CS_H();
|
|
|
|
|
return rxdata;
|
|
|
|
|
}
|
|
|
|
|
uint16_t AS5047_read(uint16_t add)
|
|
|
|
|
{
|
|
|
|
|
uint16_t data;
|
|
|
|
|
add |= 0x4000; //读指令 bit14 置1
|
|
|
|
|
if(Parity_bit_Calculate(add)==1) add=add|0x8000; //如果前15位 1的个数位偶数,则Bit15 置1
|
|
|
|
|
SPI_ReadWrite_OneByte(add); //发送一条指令,不管读回的数据
|
|
|
|
|
data=SPI_ReadWrite_OneByte(NOP|0x4000); //发送一条空指令,读取上一次指令返回的数据
|
|
|
|
|
data &=0x3fff;
|
|
|
|
|
return data;
|
|
|
|
|
}
|
|
|
|
|
|
2025-03-14 20:44:39 +08:00
|
|
|
|
void Encoder_Init(Encoder_t *ptr) {
|
|
|
|
|
ptr->round_cnt = 0;
|
|
|
|
|
ptr->ecd = AS5047_read(ANGLEUNC);
|
|
|
|
|
ptr->last_ecd = ptr->ecd;
|
|
|
|
|
ptr->offset_ecd = ptr->ecd;
|
|
|
|
|
ptr->total_angle = 0.0;
|
|
|
|
|
}
|
2025-03-12 23:04:18 +08:00
|
|
|
|
|
2025-03-14 20:44:39 +08:00
|
|
|
|
void Update_Encoder(Encoder_t *ptr) {
|
|
|
|
|
ptr->ecd = AS5047_read(ANGLEUNC);
|
2025-03-12 23:04:18 +08:00
|
|
|
|
|
2025-03-14 20:44:39 +08:00
|
|
|
|
if (ptr->ecd - ptr->last_ecd > 4096) {
|
|
|
|
|
ptr->round_cnt--;
|
|
|
|
|
} else if (ptr->ecd - ptr->last_ecd < -4096) {
|
|
|
|
|
ptr->round_cnt++;
|
2025-03-12 23:04:18 +08:00
|
|
|
|
}
|
|
|
|
|
|
2025-03-14 20:44:39 +08:00
|
|
|
|
ptr->total_angle = (double)((ptr->round_cnt * 8192 + ptr->ecd - ptr->offset_ecd) );
|
|
|
|
|
ptr->last_ecd = ptr->ecd;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int32_t Get_Encoder_Rounds(Encoder_t *ptr) {
|
|
|
|
|
return ptr->round_cnt;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
double Get_Encoder_Angle(Encoder_t *ptr) {
|
|
|
|
|
return ptr->total_angle;
|
2025-03-12 23:04:18 +08:00
|
|
|
|
}
|