一、前言：
有关闭环位置控制，详见[STM32+HAL]DengFOC移植之闭环位置控制
二、Keil填写代码：
1、AS5600读取角位移
#include "AS5600.h"
#include "math.h"
#include "FOC2.h"float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度
long angle_prev_ts=0; // 上次调用 getAngle 的时间戳
float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度
long vel_angle_prev_ts=0; // 最后速度计算时间戳
int32_t full_rotations=0; // 总圈数计数
int32_t vel_full_rotations=0; // 用于速度计算的先前完整旋转圈数/* IIC读多字节 */
void AS5600_Read_Reg(uint16_t reg, uint8_t* buf, uint8_t len)
{HAL_I2C_Mem_Read(&hi2c1, AS5600_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100);
}/* 得到弧度制的角度，范围在0-6.28 */
float GetAngle_Without_Track(void)
{float angle_d; // GetAngle_Without_Track()的返回值int16_t in_angle;uint8_t temp[DATA_SIZE]={0};AS5600_Read_Reg( Angle_Hight_Register_Addr, temp, DATA_SIZE);in_angle = ((int16_t)temp[0] <<8) | (temp[1]);angle_d = (float)in_angle * (2.0f*PI) / 4096;//angle_d为弧度制，范围在0-6.28return angle_d;
}/* 得到弧度制的带圈数角度 */
float GetAngle(void)
{float angle_cd; // GetAngle()的返回值float val = GetAngle_Without_Track();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*2.0f*PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;angle_prev = val;angle_cd = full_rotations * (2.0f*PI) + angle_prev;return angle_cd;
}/* 初始化AS5600 */
void AS5600_Init(void) {vel_angle_prev = GetAngle_Without_Track();vel_angle_prev_ts = micros();HAL_Delay(1);GetAngle_Without_Track();angle_prev = GetAngle_Without_Track();angle_prev_ts = micros();
}/* 更新当前角度值 */
void AS5600_Update(void) {float val = GetAngle_Without_Track();angle_prev_ts = micros();float d_angle = val - angle_prev; //前一次循环的角度-当前的角度// 圈数检测if(fabs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; //判断是否大于80%的360度；判断转向angle_prev = val;
}/* 上一次的度数 */
float AS5600_GetMechanicalAngle(void) {return angle_prev;
}/*角度输出函数圈数转化为角度的弧度值，再加上未转满一圈的度数
*/
float AS5600_GetAngle(void){printf("%.2f\r\n",(float)full_rotations * _2PI + angle_prev);return (float)full_rotations * _2PI + angle_prev;
}/* 获取编码器速度 */
float AS5600_GetVelocity(void) {// 计算采样时间float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;// 快速修复奇怪的情况（微溢出）if(Ts <= 0) Ts = 1e-3f;// 速度计算float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;// 保存变量以待将来使用vel_angle_prev = angle_prev;vel_full_rotations = full_rotations;vel_angle_prev_ts = angle_prev_ts;printf("%.2f\r\n",vel);return vel;
}
2、pid.c
#include "pid.h"
#include "FOC2.h"void PIDController_Init(PIDController* pid, float P, float I, float D, float ramp, float limit)
{pid->P = P;pid->I = I;pid->D = D;pid->output_ramp = ramp; // PID控制器加速度限幅pid->limit = limit; // PID控制器输出限幅pid->error_prev = 0.0f;pid->output_prev = 0.0f;pid->integral_prev = 0.0f;pid->timestamp_prev = micros(); // 假设时间戳以某种形式初始化
}/* PID控制器函数输入结构体及偏差值，输出相应的电压 */
float PIDController_Operator(PIDController* pid, float error)
{// 计算两次循环中间的间隔时间unsigned long timestamp_now = micros();float Ts = (timestamp_now - pid->timestamp_prev) * 1e-6f;if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;// P环float proportional = pid->P * error;// Tustin 散点积分（I环）float integral = pid->integral_prev + pid->I*Ts*0.5f*(error + pid->error_prev);integral = _constrain(integral, -pid->limit, pid->limit);// D环（微分环节）float derivative = pid->D*(error - pid->error_prev)/Ts;// 将P,I,D三环的计算值加起来float output = proportional + integral + derivative;output = _constrain(output, -pid->limit, pid->limit);if(pid->output_ramp > 0){// 对PID的变化速率进行限制float output_rate = (output - pid->output_prev)/Ts;if (output_rate > pid->output_ramp)output = pid->output_prev + pid->output_ramp*Ts;else if (output_rate < - pid->output_ramp)output = pid->output_prev - pid->output_ramp*Ts;}// 保存值（为了下一次循环）pid->integral_prev = integral;pid->output_prev = output;pid->error_prev = error;pid->timestamp_prev = timestamp_now;return output;
}/*
// 使用示例
int main() {PIDController pid;PIDController_Init(&pid, 1.0f, 0.0f, 0.0f, 0.1f, 100.0f);float output = PIDController_Update(&pid, 0.5f);printf("PID output: %f\n", output);return 0;
}
*/3、简易低通滤波
#include "lowpass_filter.h"
#include <stdint.h>void LowPassFilter_Init(LowPassFilter *filter, float time_constant) {filter->Tf = time_constant;filter->y_prev = 0.0f;filter->timestamp_prev = micros();
}float LowPassFilter_process(LowPassFilter *filter, float x) {unsigned long timestamp = micros();float dt = (float)(timestamp - filter->timestamp_prev) * 1e-6f; //计算两次循环的时间间隔并转换为秒if (dt < 0.0f) dt = 1e-3f; //处理因为micros跳变引起的时间间隔跳变else if (dt > 0.3f) { //处理时间间隔大于0.3s的情况filter->y_prev = x;filter->timestamp_prev = timestamp;return x;}float alpha = filter->Tf / (filter->Tf + dt);float y = alpha * filter->y_prev + (1.0f - alpha) * x;filter->y_prev = y;filter->timestamp_prev = timestamp;return y;
}
4、main.c
/* USER CODE BEGIN PV */
float motor_target=2; //目标速度
float angle_target=2; //目标角度
extern int PP,DIR;
/* USER CODE END PV */ /* USER CODE BEGIN 2 */printf("Hello World\r\n");HAL_Delay(500);DFOC_Vbus(12.6); // 输入电压12.6VDFOC_alignSensor(PP,DIR); // 设置转轴数和方向HAL_Delay(100);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){DFOC_M0_set_Force_Angle(angle_target); //设置位置
// DFOC_M0_SetVelocity(motor_target); //设置速度/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}
