[STM32+HAL]DengFOC移植之闭环速度控制

一、前言：

有关闭环位置控制，详见[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.28
	return 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.6V
	DFOC_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 */
  }