153 lines
4.6 KiB
C
153 lines
4.6 KiB
C
#include "AS5600.h"
|
||
#include "math.h"
|
||
#include "LowPass_Filter.h"
|
||
#include "foc.h"
|
||
|
||
extern int DIR; // 无刷电机纠偏旋转方向
|
||
|
||
float full_rotations; // 当前旋转圈数
|
||
|
||
float angle_prev; // 上次角度(用于位置环)
|
||
float vel_angle_prev; // 上次角度(用于速度环)
|
||
|
||
float angle_prev_Velocity; // 上次角度(用于速度环)
|
||
float vel_angle_prev_Velocity; // 上次角度(用于速度环)
|
||
|
||
unsigned long angle_prev_ts; // 上次的运行时间
|
||
unsigned long vel_angle_prev_ts; // 上次的运行时间
|
||
|
||
float raw_angle; // 传感器原始值
|
||
float angle; // 传感器角度缩减值
|
||
|
||
// 读取0x0c地址原始数据
|
||
HAL_StatusTypeDef AS5600_ReadRawAngle(I2C_HandleTypeDef *hi2c1, float *raw_angle)
|
||
{
|
||
uint8_t data[2]; // 用于存储读取的数据
|
||
HAL_StatusTypeDef ret; // 存储函数返回状态
|
||
|
||
// 从AS5600传感器的0x0c地址读取2字节数据
|
||
ret = HAL_I2C_Mem_Read(hi2c1, AS5600_I2C_ADDR << 1, AS5600_REG_RAW_ANGLE, I2C_MEMADD_SIZE_8BIT, data, 2, HAL_MAX_DELAY);
|
||
if (ret != HAL_OK)
|
||
{
|
||
return ret; // 如果读取失败，则返回错误状态
|
||
}
|
||
|
||
*raw_angle = (data[0] << 8) | data[1]; // 将读取的数据合成为一个16位数
|
||
return HAL_OK; // 返回成功状态
|
||
}
|
||
|
||
// 读取角度数据
|
||
HAL_StatusTypeDef AS5600_ReadAngle(I2C_HandleTypeDef *hi2c1, float *angle)
|
||
{
|
||
uint8_t data[2]; // 用于存储读取的数据
|
||
HAL_StatusTypeDef ret; // 存储函数返回状态
|
||
|
||
// 从AS5600传感器的0x0e地址读取2字节数据
|
||
ret = HAL_I2C_Mem_Read(hi2c1, AS5600_I2C_ADDR << 1, AS5600_REG_ANGLE, I2C_MEMADD_SIZE_8BIT, data, 2, HAL_MAX_DELAY);
|
||
if (ret != HAL_OK)
|
||
{
|
||
return ret; // 如果读取失败，则返回错误状态
|
||
}
|
||
|
||
*angle = (data[0] << 8) | data[1]; // 将读取的数据合成为一个16位数
|
||
return HAL_OK; // 返回成功状态
|
||
}
|
||
|
||
// 把原始值解算成编码器360°值
|
||
float AS5600_GetAngle360(void)
|
||
{
|
||
return raw_angle * 0.08789f; // 将原始值转换为0-360°的角度
|
||
}
|
||
|
||
// 读取磁编码器归一化弧度值:(0-6.28)
|
||
float AS5600_GetAngle2PI(void)
|
||
{
|
||
return raw_angle * 0.08789f / 57.32484f; // 将原始值转换为0-2π的弧度
|
||
}
|
||
|
||
// 磁编码器弧度制角度累计计算:(0-∞)
|
||
float AS5600_GetAngle(void)
|
||
{
|
||
float val = AS5600_GetAngle2PI(); // 获取当前角度
|
||
float d_angle = val - angle_prev; // 计算角度变化
|
||
|
||
// 计算旋转的圈数
|
||
// 通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出
|
||
// 如果发生了溢出,则将full_rotations增加1(如果d_angle小于0)或减少1(如果d_angle大于0)
|
||
if (fabs(d_angle) > (0.8f * 6.28318530718f))
|
||
full_rotations += (d_angle > 0) ? -1 : 1; // 根据角度变化方向调整旋转圈数
|
||
|
||
angle_prev = val; // 更新上次角度
|
||
return (float)full_rotations * 6.28318530718f + angle_prev; // 返回累计角度
|
||
}
|
||
|
||
// 磁编码器速度计算:(0-∞)
|
||
float AS5600_GetVelocity(void)
|
||
{
|
||
float Ts, vel = 0.0f; // Ts为采样时间，vel为速度
|
||
|
||
// 计算采样时间
|
||
angle_prev_ts = SysTick->VAL; // 获取当前时间戳
|
||
if (angle_prev_ts < vel_angle_prev_ts)
|
||
Ts = (float)(vel_angle_prev_ts - angle_prev_ts); // 计算时间差
|
||
else
|
||
Ts = (float)(0xFFFFFF - angle_prev_ts + vel_angle_prev_ts); // 处理时间戳溢出情况
|
||
|
||
// 快速修复微小溢出
|
||
if (Ts == 0 || Ts > 0.5f)
|
||
Ts = 1e-3f; // 防止采样时间为0或过大
|
||
|
||
angle_prev_Velocity = AS5600_GetAngle(); // 获取当前角度
|
||
vel = (angle_prev_Velocity - vel_angle_prev_Velocity) / Ts; // 计算速度
|
||
vel_angle_prev_Velocity = angle_prev_Velocity; // 更新上次角度
|
||
vel_angle_prev_ts = angle_prev_ts; // 更新上次时间戳
|
||
|
||
return vel; // 返回速度
|
||
}
|
||
|
||
//float AS5600_GetVelocity(void)
|
||
//{
|
||
// float Ts, vel = 0.0f; // Ts为采样时间，vel为速度
|
||
|
||
// // 获取当前时间戳
|
||
// uint32_t current_ts = SysTick->VAL;
|
||
|
||
// // 计算采样时间，SysTick->LOAD的值为系统定时器重装值
|
||
// if (current_ts < vel_angle_prev_ts)
|
||
// Ts = (float)(vel_angle_prev_ts - current_ts) / SysTick->LOAD * 1e-6f; // 计算时间差
|
||
// else
|
||
// Ts = (float)(0xFFFFFF - current_ts + vel_angle_prev_ts) / SysTick->LOAD * 1e-6f; // 处理时间戳溢出情况
|
||
|
||
// // 快速修复微小溢出
|
||
// if (Ts == 0 || Ts > 0.5f)
|
||
// Ts = 1e-3f; // 防止采样时间为0或过大
|
||
|
||
// // 获取当前角度
|
||
// float current_angle = AS5600_GetAngle();
|
||
|
||
// // 计算角度差值，处理角度溢出情况（假设角度范围是0到360度）
|
||
// float angle_diff = current_angle - vel_angle_prev_Velocity;
|
||
// if (angle_diff > 180.0f)
|
||
// angle_diff -= 360.0f;
|
||
// else if (angle_diff < -180.0f)
|
||
// angle_diff += 360.0f;
|
||
|
||
// // 计算速度
|
||
// vel = angle_diff / Ts;
|
||
|
||
// // 更新上次角度和时间戳
|
||
// vel_angle_prev_Velocity = current_angle;
|
||
// vel_angle_prev_ts = current_ts;
|
||
|
||
// return vel; // 返回速度
|
||
//}
|
||
|
||
|
||
// 磁编码器速度低通滤波计算:(0-∞)
|
||
float AS5600_Get_Speed(void)
|
||
{
|
||
float vel_M0_ori = AS5600_GetVelocity(); // 速度原始数据采集
|
||
float vel_M0_flit = LowPass_Filter(DIR * vel_M0_ori); // 原始数据低通滤波
|
||
return vel_M0_flit; // 返回滤波后的速度
|
||
}
|