【CubeMX-HAL库】STM32F407—无刷电机闭环控制

目录

一、闭环理论知识

二、闭环代码编写

1.位置闭环控制

①FOC.c

②FOC.h

③main.c 

④运行效果

3.问题解决

①开环电机能转动但闭环电机不转

②开启FPU提高运行速率

---

一、闭环理论知识

在【CubeMX-HAL库】STM32F407—无刷电机开环驱动我们自己编写了一个软件角度生成器来实现[0-2PI]的电角度输入。

①很容易我们就可以推算出在无刷电机闭环控制中，我们需要读取实际中无刷电机编码器的角度传入帕克逆变换中。

②编码器的读取是有方向的，顺时针转动编码器读取的角度增加减小需要我们调整。

所以在闭环控制无刷电机中，电角度求解，角度归一为[0-2PI]，帕克逆变换，克拉克逆变换，设置PWM函数还是我们FOC控制无刷电机的基础代码。

二、闭环代码编写

1.位置闭环控制

①FOC.c

#include "FOC.h"

//初始变量及函数定义
float voltage_limit=12;//输出电压最大限幅
float voltage_power_supply = 12;//转化为电压比
float open_loop_shaft_angle = 0,open_loop_timestamp = 0;//机械角度,开环时间间隔
float zero_electric_angle = 0;//零电角度值
float Ualpha,Ubeta = 0;//二位坐标向量
float Ua = 0,Ub = 0,Uc = 0;//三相电压
char M1_PP = 7,M1_DIR = 1;//极对数,方向

float FOC_ElectricalAngle(float machine_angle,int pole_pairs_num)//电角度求解
{
    return FOC_Conversion_Angle_0_2PI((float)(M1_DIR *  pole_pairs_num) * machine_angle - zero_electric_angle);//电角度=机械角度×极对数
}

float FOC_Conversion_Angle_0_2PI(float angle)//归一化角度到[0,2PI]
{
    float a = fmod(angle, 2 * PI);//取余运算可以用于归一化,列出特殊值例子算便知
    return a >= 0 ? a : (a + 2 * PI);
    //三目运算符。格式：condition ? expr1 : expr2
    //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。
    //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。
    //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}

void FOC_SetPWM(float Ua,float Ub,float Uc)//设置PWM到控制器输出
{
    Ua = LIMIT(Ua, 0.0f, voltage_limit);//限制电压上限
    Ub = LIMIT(Ub, 0.0f, voltage_limit);
    Uc = LIMIT(Uc, 0.0f, voltage_limit);

    float dc_a = LIMIT(Ua / voltage_power_supply, 0.0f, 1.0f);//计算占空比
    float dc_b = LIMIT(Ub / voltage_power_supply, 0.0f, 1.0f);//限制占空比从0到1
    float dc_c = LIMIT(Uc / voltage_power_supply, 0.0f, 1.0f);

    TIM1->CCR1 = dc_a * 255.0f;//写入PWM到PWM 1 2 3通道
    TIM1->CCR2 = dc_b * 255.0f;
    TIM1->CCR3 = dc_c * 255.0f;
//    printf("%d,%d,%d\n",TIM1->CCR1,TIM1->CCR2,TIM1->CCR3);
}

void FOC_M_Init(int PP,int DIR)//电机初始化获得零电角度
{ 
    M1_PP = PP;
    M1_DIR = DIR;
    FOC_Parker_Clark(3,_3PI_2);
    HAL_Delay(3000);
    zero_electric_angle = FOC_ElectricalAngle(AS5600_2_GetAngle_0_2PI(),M1_PP);//电角度0位偏差校准
    FOC_Parker_Clark(0,_3PI_2);
    printf("Zero_Electric_Angle:%f\r\n",zero_electric_angle);
}

void FOC_Parker_Clark(float Uq,float angle_el)//FOC核心算法,克拉克逆变换/帕克逆变换
{
    //Ud等于零,用Uq控制
    Uq = LIMIT(Uq,-voltage_power_supply/2.0f,voltage_power_supply/2.0f);
    angle_el = FOC_Conversion_Angle_0_2PI(angle_el);
    Ualpha = -Uq * sin(angle_el);//帕克逆变换
    Ubeta = Uq * cos(angle_el);

    Ua = Ualpha + voltage_power_supply / 2.0f;//克拉克逆变换,+V/2整体将数值挪到正数
    Ub = (sqrt(3) * Ubeta - Ualpha) / 2.0f + voltage_power_supply / 2.0f;
    Uc = (-Ualpha - sqrt(3) * Ubeta) / 2.0f + voltage_power_supply / 2.0f;
    
    FOC_SetPWM(Ua, Ub, Uc);
}

uint32_t micros;//芯片运行时基
float FOC_Open_Speed_Start(float target_velocity)//开环速度函数
{ 
    uint32_t now_us = micros;//获取从开启芯片以来的微秒数，它的精度是 4 微秒。micros() 返回的是一个无符号长整型（unsigned long）的值

    float Ts = (now_us - open_loop_timestamp) * 1e-6f;//计算当前每个Loop的运行时间间隔

    //由于 micros() 函数返回的时间戳会在大约 70 分钟之后重新开始计数，在由70分钟跳变到0时，TS会出现异常，因此需要进行修正。
    //如果时间间隔小于等于零或大于 0.5 秒，则将其设置为一个较小的默认值，即 1e-3f
    if (Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;

    // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，存储在 shaft_angle 变量中。
    //在此之前，还需要对轴角度进行归一化，以确保其值在 0 到 2π 之间。
    open_loop_shaft_angle = FOC_Conversion_Angle_0_2PI(open_loop_shaft_angle + target_velocity * Ts);
    //以目标速度为 10 rad/s 为例，如果时间间隔是 1 秒，则在每个循环中需要增加 10 * 1 = 10 弧度的角度变化量，才能使电机转动到目标速度。
    //如果时间间隔是 0.1 秒，那么在每个循环中需要增加的角度变化量就是 10 * 0.1 = 1 弧度，才能实现相同的目标速度。
    //因此，电机轴的转动角度取决于目标速度和时间间隔的乘积。

    // 使用早前设置的voltage_power_supply的1/3作为Uq值，这个值会直接影响输出力矩
    // 最大只能设置为Uq = voltage_power_supply/2，否则ua,ub,uc会超出供电电压限幅
    float Uq = voltage_power_supply / 5;

    FOC_Parker_Clark(Uq,FOC_ElectricalAngle(open_loop_shaft_angle, 7));

    open_loop_timestamp = now_us;//用于计算下一个时间间隔
    return Uq;
}

②FOC.h

#ifndef __FOC_H_
#define __FOC_H_

#include "main.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "AS5600.h"

#define PI       3.14159265359f
#define _2PI     6.28318530718f
#define _3PI_2   4.71238898f
#define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f

#define LIMIT(amt,low,high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))//限幅函数

extern uint32_t micros;//芯片运行时基

float FOC_ElectricalAngle(float machine_angle,int pole_pairs_num);//电角度求解
float FOC_Conversion_Angle_0_2PI(float angle);//归一化角度到[0,2PI]
void FOC_SetPWM(float Ua,float Ub,float Uc);//设置PWM到控制器输出
void FOC_M_Init(int PP,int DIR);//电机初始化获得零电角度
void FOC_Parker_Clark(float Uq,float angle_el);//FOC核心算法,克拉克逆变换/帕克逆变换
float FOC_Open_Speed_Start(float target_velocity);//开环速度函数

#endif

③main.c 

#define Sensor_DIR 1//传感器方向
#define Motor_PP 7//电机极对数

#define Kp 0.005f//0.133->45°输出最大力矩

FOC_M_Init(Motor_PP,Sensor_DIR);//极对数,传感器方向

a = AS5600_2_GetAngle_0_2PI();
//FOC_Open_Speed_Start(10);//Open speed
//FOC_Parker_Clark(1,FOC_ElectricalAngle(a,7));//电压力矩0-6V
//10K（有点超了可能最高才5.1K）无声 5K无声 1K优秀 500HZ较粗 100HZ粗狂 10HZ卡顿
FOC_Parker_Clark(Kp * (0 - Sensor_DIR * AS5600_2_GetAngle_Cycles()) * 180.0f / PI,
                 FOC_ElectricalAngle(a,Motor_PP));//位置闭环 /PI*180转换成角度 然后*Kp获得输出力矩

④运行效果

2.速度闭环控制 

3.力矩位置闭环控制

4.问题解决

①开环电机能转动但闭环电机不转

在调试开环代码时可以正常转动，但是调试闭环代码时发现电机不转，手动转起来感觉像齿轮一样在每个线圈之间都卡卡的。将编码器方向反向之后电机兹拉兹拉的鸣叫。

突然悟了一下：开环转动时电机逆时针转动，软件模拟的角度范围不断增大[0-2PI]。

但是闭环之后逆时针转动角度减小，改变编码器方向也不行，然后试着任意调换电机的两相，发现电机可以正常，调换两相之后开环闭环电机随着软件模拟和实际编码器测得的正向增长的角度开始顺时针旋转。

②开启FPU提高运行速率

在stm32f407xx.h中我们可以看到M4内核有如下资源。

然后在core_cm4.h中我们看到需要定义两个宏然才能使能FPU。 所以我们在Target中define两个宏定义。

,__CC_ARM,__TARGET_FPU_VFP

使能之后system_stm32f4xx.c中就会开启FPU的设置。

然后我们需要引用#include “arm_math.h”这个STM32自带的数学处理函数。

直接#include "arm_math.h"发现找不到头文件路径，所以我们在Manage中添加DSP库再编译。

原始计算速度

 修改了两个三角函数计算的运行效果

但是用帕克逆变换库效果反而降低一点，克拉克逆变换库也不太适配当前函数。