仅供参考
FOC.c
#include <at32f403a_407_conf.h>
#include <math.h>
#include "tmr.h"
//初始变量及函数定义
#define PI 3.14
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
/*宏定义实现的一个约束函数,用于限制一个值的范围。
具体来说,该宏定义的名称为 _constrain,接受三个参数 amt、low 和 high,分别表示要限制的值、最小值和最大值。
该宏定义的实现使用了三元运算符,根据 amt 是否小于 low 或大于 high,返回其中的最大或最小值,或者返回原值。
//换句话说,如果 amt 小于 low,则返回 low;如果 amt 大于 high,则返回 high;否则返回 amt。
这样,_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。*/
float voltage_power_supply=12.6;
float shaft_angle=0,open_loop_timestamp=0;
float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0;
// 电角度求解
float _electricalAngle(float shaft_angle, int pole_pairs) {
return (shaft_angle * pole_pairs);
}
// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle){
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] 的范围内,以确保角度的值始终为正数。
*/
// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
// 计算占空比
// 限制占空比从0到1
dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
//写入PWM到PWM 0 1 2 通道
tmr_x_ch1(TMR1,dc_a*100,TRUE,1);
tmr_x_ch2(TMR1,dc_b*100,TRUE,1);
tmr_x_ch3(TMR1,dc_c*100,TRUE,1);
}
void setPhaseVoltage(float Uq,float Ud, float angle_el) {
angle_el = _normalizeAngle(angle_el + zero_electric_angle);
// 帕克逆变换
Ualpha = -Uq*sin(angle_el);
Ubeta = Uq*cos(angle_el);
// 克拉克逆变换
Ua = Ualpha + voltage_power_supply/2;
Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
setPwm(Ua,Ub,Uc);
}
//开环速度函数
extern long long micros;
float velocityOpenloop(float target_velocity){
unsigned long now_us = micros;
//获取从开启芯片以来的微秒数,它的精度是 4 微秒。 micros() 返回的是一个无符号长整型(unsigned long)的值
//计算当前每个Loop的运行时间间隔
float Ts = (now_us - open_loop_timestamp) * 1e-6f;
/*由于 micros() 函数返回的时间戳会在大约 70 分钟之后重新开始计数,在由70分钟跳变到0时,
TS会出现异常,因此需要进行修正。如果时间间隔小于等于零或大于 0.5 秒,则将其设置为一个较小的默认值,即 1e-3f
*/
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
/*通过乘以时间间隔和目标速度来计算需要转动的机械角度,存储在 shaft_angle 变量中。
在此之前,还需要对轴角度进行归一化,以确保其值在 0 到 2π 之间。*/
shaft_angle = _normalizeAngle(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/3;
setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, 7));
open_loop_timestamp = now_us; //用于计算下一个时间间隔
return Uq;
}
FOC.h
#include <at32f403a_407_conf.h>
#ifndef __FOC_H_
float _electricalAngle(float shaft_angle, int pole_pairs);
float _normalizeAngle(float angle);
void setPwm(float Ua, float Ub, float Uc);
void setPhaseVoltage(float Uq,float Ud, float angle_el);
float velocityOpenloop(float target_velocity);
#endif
tmr.c
#include <at32f403a_407_conf.h>
void tmr1_gpio_init()
{
CRM->apb2en_bit.gpioaen=1;
CRM->apb2en_bit.gpioben=1;
GPIOA->cfghr_bit.iofc8=2;
GPIOA->cfghr_bit.iomc8=1;//ch1
GPIOA->cfghr_bit.iofc9=2;
GPIOA->cfghr_bit.iomc9=1;//ch2
GPIOA->cfghr_bit.iofc10=2;
GPIOA->cfghr_bit.iomc10=1;//ch3
GPIOB->cfghr_bit.iofc13=2;
GPIOB->cfghr_bit.iomc13=1;//ch1c
GPIOB->cfghr_bit.iofc14=2;
GPIOB->cfghr_bit.iomc14=1;//ch2c
GPIOB->cfghr_bit.iofc15=2;
GPIOB->cfghr_bit.iomc15=1;//ch3c
}
void tmr_x_init(tmr_type* tmr_x,int systick,float freq)
{
tmr1_gpio_init();
tmr_x->ctrl1_bit.ocmen=0;//
tmr_x->ctrl1_bit.cnt_dir=0;
tmr_x->pr_bit.pr=(systick*1000000-1)/freq;
tmr_x->div_bit.div=1-1;
tmr_x->brk_bit.oen=1;
tmr_x->ctrl1_bit.tmren=1;
}
void tmr_x_ch1(tmr_type* tmr_x,float ch1_duty,confirm_state new_state,u8 pority)
{
//ch1 congfing
tmr_x->c1dt_bit.c1dt=(int)(ch1_duty/100*tmr_x->pr_bit.pr);
tmr_x->cm1_output_bit.c1octrl=0b110;
tmr_x->cm1_output_bit.c1c=0;
tmr_x->cctrl_bit.c1p=0;
tmr_x->cctrl_bit.c1en=1;
tmr_x->cctrl_bit.c1cp=pority;
tmr_x->cctrl_bit.c1cen=new_state;
tmr_x->cm1_output_bit.c1oien=1;
tmr_x->cm1_output_bit.c1osen=0;
}
void tmr_x_ch2(tmr_type* tmr_x,float ch2_duty,confirm_state new_state,u8 pority)
{
//ch2 congfing
tmr_x->c2dt_bit.c2dt=(int)(ch2_duty/100*tmr_x->pr_bit.pr);
tmr_x->cm1_output_bit.c2octrl=0b110;
tmr_x->cm1_output_bit.c2c=0;
tmr_x->cctrl_bit.c2p=0;
tmr_x->cctrl_bit.c2en=1;
tmr_x->cctrl_bit.c2cp=pority;
tmr_x->cctrl_bit.c2cen=new_state;
tmr_x->cm1_output_bit.c2oien=1;
tmr_x->cm1_output_bit.c2osen=0;
}
void tmr_x_ch3(tmr_type* tmr_x,float ch3_duty,confirm_state new_state,u8 pority)
{
//ch3 congfing
tmr_x->c3dt_bit.c3dt=(int)(ch3_duty/100*tmr_x->pr_bit.pr);
tmr_x->cm2_output_bit.c3octrl=0b110;
tmr_x->cm2_output_bit.c3c=0;
tmr_x->cctrl_bit.c3p=0;
tmr_x->cctrl_bit.c3en=1;
tmr_x->cctrl_bit.c3cp=pority;
tmr_x->cctrl_bit.c3cen=new_state;
tmr_x->cm2_output_bit.c3oien=1;
tmr_x->cm2_output_bit.c3osen=0;
}
void tmr_x_ch4(tmr_type* tmr_x,float ch4_duty)
{
//ch4 congfing
tmr_x->c4dt_bit.c4dt=(int)(ch4_duty/100*tmr_x->pr_bit.pr);
tmr_x->cm2_output_bit.c4octrl=0b110;
tmr_x->cm2_output_bit.c4c=0;
tmr_x->cctrl_bit.c4p=0;
tmr_x->cctrl_bit.c4en=1;
tmr_x->cm2_output_bit.c4oien=1;
tmr_x->cm2_output_bit.c4osen=0;
}
void tmr_x_intrrupt(tmr_type* tmr_x,int div ,int pr)
{
CRM->apb1en_bit.tmr6en=1;
tmr_x->ctrl1_bit.cnt_dir=0;//向上计数
tmr_x->ctrl1_bit.ocmen=0;//单周期使能关闭
tmr_x->ctrl1_bit.ovfen=0;//溢出事件使能
tmr_x->ctrl1_bit.ovfs=1;//溢出事件来源计数器
tmr_x->iden_bit.ovfien=1;
tmr_x->div_bit.div=div;
tmr_x->pr_bit.pr=pr;
//nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
//nvic_irq_enable(TMR6_GLOBAL_IRQn,1 ,0);
tmr_x->ctrl1_bit.tmren=1;
}
tmr.h
#include <at32f403a_407_conf.h>
#ifndef __TMR_H_
void tmr1_gpio_init();
//systick 是当下系统时钟频率
void tmr_x_init(tmr_type* tmr_x,int systick,float freq);
/*new_state TRUE:使能互补通道输出 False 关闭。。。。。 */
/* pority 1:CxCOUT 的有效电平为高 0:CxCOUT 的有效电平为低*/
void tmr_x_ch1(tmr_type* tmr_x,float ch1_duty,confirm_state new_state,u8 pority);
void tmr_x_ch2(tmr_type* tmr_x,float ch2_duty,confirm_state new_state,u8 pority);
void tmr_x_ch3(tmr_type* tmr_x,float ch3_duty,confirm_state new_state,u8 pority);
void tmr_x_ch4(tmr_type* tmr_x,float ch4_duty);
void tmr_x_intrrupt(tmr_type* tmr_x,int div ,int pr);
#endif
main.c
#include<at32f403a_407_conf.h>
#include "tmr.h"
#include "FOC.h"
long long micros=0;
int main()
{
CRM->apb2en_bit.tmr1en=1;
tmr_x_init(TMR1,8,10000);
tmr_x_intrrupt(TMR6,1-1 ,80-1);//1us
nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
nvic_irq_enable(TMR6_GLOBAL_IRQn,1 ,0);
tmr_x_ch1(TMR1,0,TRUE,1);
tmr_x_ch2(TMR1,0,TRUE,1);
tmr_x_ch3(TMR1,0,TRUE,1);
while(1)
{
velocityOpenloop(100);
}
}
void TMR6_GLOBAL_IRQHandler()
{
micros+=1;
TMR6->ists_bit.ovfif=0;//清除TIM1更新中断标志
tmr_flag_clear(TMR6, TMR_OVF_FLAG);
}