236 lines
7.8 KiB
C
236 lines
7.8 KiB
C
#include "tim.h"
|
||
#include "ABZ.h"
|
||
#include "foc.h"
|
||
#include "math.h"
|
||
#include "LowPass_Filter.h"
|
||
|
||
#define RAD_TO_DEG (57.2957795131f) // 弧度转角度常量
|
||
#define FULL_ROTATION_RAD (6.28318530718f) // 2π
|
||
|
||
extern int DIR; // 无刷电机纠偏旋转方向
|
||
|
||
//此驱动按照AS5600驱动中的函数编写
|
||
|
||
// ABZ 正交编码器的每转脉冲数
|
||
#define ENCODER_PULSES_PER_REV 4096
|
||
|
||
// 定时器周期 (秒)
|
||
#define TIMER_PERIOD 1.0f
|
||
|
||
int realCnt = 0; // 真实编码器计数值
|
||
|
||
ABZ_Encoder encoderDef; //encoderDef结构体声明
|
||
|
||
// 变量声明
|
||
volatile int32_t ABZ_pulse_count = 0;
|
||
volatile int32_t ABZ_prev_pulse_count = 0;
|
||
volatile float ABZ_speed = 0.0f;
|
||
volatile float ABZ_angle = 0.0f;
|
||
|
||
/////////////////////////////////////////////////////////////////////
|
||
|
||
//此处适用的定时器时间戳计算速度
|
||
float ABZ_full_rotations; // 当前旋转圈数
|
||
|
||
unsigned long ABZ_angle_prev_ts; // 上次的运行时间
|
||
unsigned long ABZ_vel_angle_prev_ts; // 上次的运行时间
|
||
|
||
float ABZ_angle_prev_Velocity; // 上次角度(用于速度环)
|
||
float ABZ_vel_angle_prev_Velocity; // 上次角度(用于速度环)
|
||
|
||
float ABZ_encoder_count = 0; // 读取当前编码器计数值
|
||
float ABZ_prev_encoder_count = 0; // 读取上次编码器计数值
|
||
|
||
|
||
void TIM2_M1_ABZ(void)
|
||
{
|
||
// 启动编码器模式
|
||
HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
|
||
|
||
// 启动索引中断,检测到ABZ编码器Z信号就重置计数器。
|
||
HAL_TIMEx_EnableEncoderIndex(&htim2);
|
||
}
|
||
|
||
void TIM3_M2_ABZ(void)
|
||
{
|
||
// 启动编码器模式
|
||
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
|
||
|
||
// 启动索引中断,检测到ABZ编码器Z信号就重置计数器。
|
||
HAL_TIMEx_EnableEncoderIndex(&htim3);
|
||
}
|
||
|
||
/**
|
||
* @brief 编码器调试函数
|
||
*
|
||
*/
|
||
void Count_M1_ABZ(void)
|
||
{
|
||
// Debug：读取编码器计数值
|
||
volatile int32_t encoder_count = __HAL_TIM_GET_COUNTER(&htim2);
|
||
}
|
||
|
||
void Count_M2_ABZ(void)
|
||
{
|
||
// Debug：读取编码器计数值
|
||
volatile int32_t encoder_count = __HAL_TIM_GET_COUNTER(&htim3);
|
||
}
|
||
|
||
// // 电机速度和角度计算
|
||
// void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
|
||
// {
|
||
// if (htim->Instance == TIM3)
|
||
// {
|
||
// // 获取当前脉冲计数
|
||
// ABZ_pulse_count = __HAL_TIM_GET_COUNTER(&htim3);
|
||
|
||
// // 计算脉冲增量
|
||
// int32_t pulse_delta = ABZ_pulse_count - ABZ_prev_pulse_count;
|
||
|
||
// // 计算速度 (PPS: 脉冲每秒)
|
||
// // 假设定时器周期为1秒，则速度为脉冲增量
|
||
// ABZ_speed = (float)pulse_delta / TIMER_PERIOD;
|
||
|
||
// // 计算角度 (单位：度)
|
||
// // 角度 = 当前计数值 / 每转脉冲数 * 360度
|
||
// ABZ_angle = (float)((ABZ_pulse_count % ENCODER_PULSES_PER_REV + ENCODER_PULSES_PER_REV) % ENCODER_PULSES_PER_REV) / ENCODER_PULSES_PER_REV * 360.0f;
|
||
|
||
// // 更新上次脉冲计数
|
||
// ABZ_prev_pulse_count = ABZ_pulse_count;
|
||
|
||
// // 清除定时器更新中断标志
|
||
// __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
|
||
// }
|
||
// }
|
||
|
||
//定时器中断方法计算速度
|
||
// int UpdateEncoderCnt(void)
|
||
// {
|
||
// // 启动编码器模式
|
||
// HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
|
||
|
||
// // 启动索引中断
|
||
// HAL_TIMEx_EnableEncoderIndex(&htim3);
|
||
|
||
// // 获取电机读数
|
||
// encoderDef.cnt = (int32_t)__HAL_TIM_GET_COUNTER(&htim3);
|
||
|
||
// // 计算电机偏移量
|
||
// encoderDef.incCnt = ModifyIncCnt(encoderDef.cnt - encoderDef.preCnt);
|
||
// // 更新上次脉冲计数
|
||
// encoderDef.preCnt = encoderDef.cnt;
|
||
|
||
// // 获取真实的编码器计数值
|
||
// realCnt = encoderDef.cnt - encoderDef.offsetCnt;
|
||
|
||
// // 当计数值为负数时，加上一圈的计数数
|
||
// while (realCnt < 0)
|
||
// {
|
||
// realCnt += ENCODER_PULSES_PER_REV * 4;
|
||
// }
|
||
|
||
// // 计算真实的角度值
|
||
// encoderDef.angle = realCnt * 2 * PI / ENCODER_PULSES_PER_REV / 4;
|
||
|
||
// // 计算电角度
|
||
// encoderDef.elecAngle = _normalizeAngle(encoderDef.angle);
|
||
|
||
// // 返回编码器读数
|
||
// return encoderDef.cnt;
|
||
// }
|
||
|
||
// int16_t ModifyIncCnt(int16_t delta)
|
||
// {
|
||
// // 如果增量超过了一半计数范围，说明是溢出，需要进行调整
|
||
// if (delta > ENCODER_HALF_COUNT)
|
||
// {
|
||
// delta -= (ENCODER_MAX_COUNT + 1);
|
||
// }
|
||
// else if (delta < -ENCODER_HALF_COUNT)
|
||
// {
|
||
// delta += (ENCODER_MAX_COUNT + 1);
|
||
// }
|
||
// return delta;
|
||
// }
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
|
||
// 把原始值解算成编码器360°值
|
||
//float ABZ_GetAngle360(void)
|
||
//{
|
||
// ABZ_encoder_count = __HAL_TIM_GET_COUNTER(&htim3);
|
||
// float ABZ_raw_angle = ABZ_encoder_count;
|
||
// return ABZ_raw_angle * 0.08789f; // 将原始值转换为0-360°的角度,4096*0.08789≈360°
|
||
//}
|
||
|
||
float debug_ABZ_encoder_count;//debug 参数
|
||
// 读取磁编码器归一化弧度值:(0-6.28)
|
||
float ABZ_GetAngle2PI(void)
|
||
{
|
||
//读取脉冲数
|
||
ABZ_encoder_count = __HAL_TIM_GET_COUNTER(&htim3);
|
||
//debug 参数
|
||
debug_ABZ_encoder_count = ABZ_encoder_count * 0.08789f / 57.32484f;
|
||
|
||
return ABZ_encoder_count * 0.08789f / 57.32484f; // 将原始值转换为0-2π的弧度
|
||
}
|
||
|
||
|
||
float ABZ_val; //Debug：当前角度
|
||
float ABZ_current_angle;//当前角度变化值
|
||
float ABZ_full_rotations_Debug;//累计旋转角度
|
||
|
||
// 磁编码器弧度制角度累计计算:(0-∞)
|
||
float ABZ_GetAngle(void)
|
||
{
|
||
ABZ_val = ABZ_GetAngle2PI(); // 获取当前弧度
|
||
ABZ_current_angle = ABZ_val - ABZ_prev_encoder_count; // 计算角度变化
|
||
|
||
// 计算旋转的圈数
|
||
// 通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出
|
||
// 如果发生了溢出,则将full_rotations增加1(如果d_angle小于0)或减少1(如果d_angle大于0)
|
||
if (fabs(ABZ_current_angle) > (0.8f * 6.28318530718f))
|
||
ABZ_full_rotations += (ABZ_current_angle > 0) ? -1 : 1; // 根据角度变化方向调整旋转圈数
|
||
|
||
ABZ_prev_encoder_count = ABZ_val; // 更新上次角度
|
||
ABZ_full_rotations_Debug=ABZ_full_rotations * 6.28318530718f + ABZ_prev_encoder_count;
|
||
return (float)ABZ_full_rotations * 6.28318530718f + ABZ_prev_encoder_count; // 返回累计角度
|
||
}
|
||
|
||
|
||
float ABZ_Ts, ABZvel = 0.0f; // Ts为采样时间，vel为速度
|
||
|
||
// 磁编码器速度计算:(0-∞)
|
||
float ABZ_GetVelocity(void)
|
||
{
|
||
//float Ts, vel = 0.0f; // Ts为采样时间，vel为速度
|
||
|
||
// 计算采样时间
|
||
ABZ_angle_prev_ts = SysTick->VAL; // 获取当前时间戳
|
||
|
||
if (ABZ_angle_prev_ts < ABZ_vel_angle_prev_ts)
|
||
ABZ_Ts = (float)(ABZ_vel_angle_prev_ts - ABZ_angle_prev_ts); // 计算时间差
|
||
else
|
||
ABZ_Ts = (float)(0xFFFFFF - ABZ_angle_prev_ts + ABZ_vel_angle_prev_ts); // 处理时间戳溢出情况
|
||
|
||
// 快速修复微小溢出
|
||
if (ABZ_Ts == 0 || ABZ_Ts > 0.5f)
|
||
ABZ_Ts = 1e-3f; // 防止采样时间为0或过大
|
||
|
||
ABZ_angle_prev_Velocity = ABZ_GetAngle(); // 获取当前角度
|
||
ABZvel = (ABZ_angle_prev_Velocity - ABZ_vel_angle_prev_Velocity) / ABZ_Ts; // 计算速度
|
||
ABZ_vel_angle_prev_Velocity = ABZ_angle_prev_Velocity; // 更新上次角度
|
||
ABZ_vel_angle_prev_ts = ABZ_angle_prev_ts; // 更新上次时间戳
|
||
|
||
return ABZvel; // 返回速度
|
||
}
|
||
|
||
float ABZ_vel_M0_flit;
|
||
// 磁编码器速度低通滤波计算:(0-∞)
|
||
float ABZ_Get_Speed(void)
|
||
{
|
||
float vel_M0_ori = ABZ_GetVelocity(); // 速度原始数据采集
|
||
ABZ_vel_M0_flit = LowPass_Filter(DIR * vel_M0_ori); // 原始数据低通滤波
|
||
return ABZ_vel_M0_flit; // 返回滤波后的速度
|
||
}
|