峰岹 FU6532 无感FOC学习

峰岹 FU6532 无感FOC

爱吃牛里脊

2054人浏览 · 2025-10-23 17:54:37

爱吃牛里脊 · 2025-10-23 17:54:37 发布

前言

文章主要是我学习的过程，学到哪里记录到哪里，中间说错的会在后续的记录中修改，主要还是看看自己走过哪些弯路，哈哈哈。也不知道自己学习的方式对不对，就喜欢用到啥去看啥，老是觉得都看完了在来搞又不知道之前学的是什么。废话不多说，加油把他搞定。

资料

FAE给我的代码，硬件，调试手册；还有官网下载的参考；后续更新也会放在里面。百度网盘（提取码：1kku），文章中的图片和代码，如有侵权请联系删除。

一、硬件

硬件部分没啥好说的，有不理解的可以看一下峰岹官网FT8132的内容。这里先放着，后续遇到硬件问题即时记录。

二、软件

2.1 调试手册

先来看下调试手册里面的内容：

2.1.1 状态机流程图

在这里插入图片描述
总的电机运行流程就是mcSpeedRamp.FlagONOFF=1启动电机；mcSpeedRamp.FlagONOFF=0关闭电机；还有一个mcFault，故障处理。

2.1.2 程序流程图

请添加图片描述
这是整个代码包含的部分，while循环里的电机状态机就是2.1.1的内容。

2.1.3 调试步骤

先按照它的步骤调试一下吧，本来想直接看代码了。
1.电机参数
厂家提供了最好，我的电机没有参数，但是我有电桥，哇哈哈哈，我是该高兴呢还是该敲桌子呢（焯）。
1）极对数：4
极对数获取方法也很简单，扒拉电机转一圈，感受一下收到几次阻力除以2就是极对数了。或者呢电机条件允许的情况下，可以用一个示波器电压探头分别接入电机任意两相相线(探头接一根相线，地线接一根相线)，然后手动转动转子一圈，示波器截取到电机旋转一圈的反电动势波形，波峰或波谷的个数即电机的极对数（这种方法也是用来测反电动势的）。还有一种方法是拿磁铁吸一圈，吸了几次就是几对极。
2）相电阻Rs：1.875Ω。
不知道精度有没有影响，直接用万用表测的线电阻，没用电桥。
3）相电感Ls：1.049mH。
电桥测：1000Hz；同时测后的时候转动下转子，看下电机是否为凸极性（不知道这个有没有影响，无感初始位置检测的时候凸极性和隐极性的定位方式不一样）
4）反电动势常数 Ke：
示波器的探头接电机的一相，地接电机另外两相中的某一相，转动负载，测出反电动势波形。取中间的一个正弦波，测量其峰峰值Vpp和频率f。
请添加图片描述

规格书上的计算应该也是这个公式的变形。算出来是4.3267，不知道对不对，问了下gpt常见的Ke。
注意参数填的时候里面是周期不是频率，单位是ms。基准速度刚调可以设置小一点，设置为额定值以下就行，防止出错占空比一下太大烧电机，我的额定是3000转，这里设置为4000（速度基准，后面最大转速是它的一半）
请添加图片描述
2. 芯片参数
我就改了PWM频率，10kHz，做有感方波的时候可以满足我的要求，这里就不变。
3. 母线电压上的电阻和采样电阻
母线电压采样可以先关了，之前做方波的时候遇到一个问题，直流源显示24V，但是我设置的24V报过压无法触发，要到24.3V。后来发现是输入的地方有个SS56有0.3V的一个压降，我的采样点在SS56的后面，这个压降加上就可以正常触发。
采样电阻要设置正确，这里会影响硬件过流。硬件过流是没法关闭的，过流之后会直接让ME=0；可以设置不让ME=0，只是进入中断，也不建议关掉，防止烧MOS。放大倍数要配合采样电阻设置。~~我有点忘记这个值应该如何确认了，下次看到的时候再补上。~~ 我设置了0.01Ω，16倍放大（硬件过流为8A，软件过流为4A，方波的设置，软件过流会检测一段时间，硬件过流脉冲就会触发，我的PMOS是最大10A）。
4. 保护参数
刚开始调试，除硬件以外全关。
后续：没有后续，电机能动了也不会按照它的代码写保护的，啊哈哈哈。
5. 启动参数配置
将AlignTestMode置为ALIGN_NOMAL，调速模式选择NONEMODE开机，这个时候电机会跑到预定位状态，此时UVW三相会有固定的PWM波形输出，则硬件驱动电路正常（#define IQ_Start_CURRENT I_Value(0.1) ///< (A) Q轴启动电流。这个也调小一点，不然要1A才会转动）。这样电机应该可以启动，我调试的时候是电流缓慢上升到0.26A，电机转动，不过速度上来之后，电流一下到了2A，直流源报过流（应该是切环的时候挂了，但是不知道是切入闭环还是切入硬件换相）。

2.1.4 小结

下面就是具体的环的参数调整，这个后面详细看了代码再说吧，这里的工作主要是确定硬件和采集的电机参数可以使用。我买了一块它的DEMO板，应为之前硬件设计上出了点问题，电机转一会儿就不动了，我就买了块对比了一下。这里做反电动势正好用来验证我的硬件设计对不对。

2.2 软件

软件我就按照程序流程图看吧，可能跳跃的会比较多，因为看着看着会走神，哇哈哈哈。

2.2.1 软件初始化

说到这个就刚开始看stm32的时候，那状态机，一套又一套。对于初学者是真的不友好，看的我真的是想吐，后面看了DengFOC，还好一点，不过他都是浮点数操作，对于我这2块的MCU真的是一言难尽。
代码具体也没啥。
偏置电压就设置成了中间值16383
正转
LED(不管它)。
McStaSet.SetMode = 0这个不知道干嘛的。
mcState：电机状态机设置为mcReady。
mcFaultSource：FaultNoSource。

2.2.2 硬件初始化

2.2.2.1 参考电压

请添加图片描述

参考电压5V；Vhalf是2.5V（这个设置是放大器的偏置电压，设置对之后，后面的偏置电压才会采集正确进入电机运行状态）；使能VREF,VHALF。还配置了P3.2输出这个一定要开，不然没有偏置输出，P3.5输出Vref到P3.5脚，这里可以用示波器看下电压，它的输出有偏差。

2.2.2.2 硬件过流

CMP3_Init()和CMP3_Interrupt_Init()一起看，初始化里面他们只是配置了硬件过流。昨天看无感方波的时候看到他是用来做IPD初始位置检测了，所以本来是想详细看一下它的，这里面没有涉及就带过，等后面有机会再看方波的时候再说。

这里配置了P2.7（AD4），芯片FU6532上没有显示，其实这是内部引脚，它也是用来采集偏置电压的，之前在这里也是头疼了很久；
然后就是设置单比较器模式，15mV迟滞；
再配置DAC，把你设置的过流值转换成电压与AD4采集到的值比较；
配置触发硬件过流自动关闭MOE；
中断配置可有可无，给硬件过流标记位的。

2.2.2.3 ADC初始化

这个比较重要，FOC么，采集电流，无感换相。

P2.0(AD0): 暂时不知
P2.3(AD1): 暂时不知
P2.4(AD2): 电压采集
P2.5(AD3): SREF
P2.7(AD4): 偏置电压
P3.3(AD6): 温度采集
P3.4(AD7): CM3P，AMP0的输出会到这里

这里SetBit(ADC_MASK, 0x3000);使能了CH13、CH14？有点不懂。ADC_SCYCH = 0011 0011，AD2、8~13采样周期为3；SetBit(ADC_CR , ADCALIGN)，这个比较巧妙，直接把adc的采样值左移了3位；4096*8 = 32768，相当于归一化到了Q15。
（2025.10.23，先到这。）

2.2.2.4 Driver初始化

之前做有感的时候梳理过Driver也没记录，就是现在有点忘记了，好记性不如烂笔头。

DRV_ARR = 4800；f = 48M/DRV_ARR,我需要10kHz的PWM波
DRV_DTR ：死区设置，FOC需要设置互补输出，没死区它的上管下管容易在上升下降过程中导通。详细的值需要看MOS的规格书，看导通关闭时间。资料这里讲的挺好的，gpt的回答也类似，就参考这里吧。
DRV_DR：这个就是我们需要多少占空比=DRV_DR/DRV_ARR*100%
DRV_CMR = 1010 1011 1111；UVW上下管输出使能，上管反向输出。
PI_CR：对于FU6532，他是内置预驱的，都是高电平驱动输出。
中断配置：PWM中心触发中断（DRV_COMR = (PWM_VALUE_LOAD >> 3);匹配点对应的占空比 = DRV_COMR4/DRV_ARR100% ）。
DRV_CR：FOC不使能（这个后面运行过程中还会处理）；ME工作在FOC驱动模式；Driver使能，计数器使能；预装载不使能（就是更新PWM占空比之后立即写入寄存器）

2.2.2.5 运放初始化

请添加图片描述
从图上可以看出，只要配置P3.0,P3.1;P2.7为模拟模式；这里AMP_CR0|=(1<<6);代码注释是恒功率，就是把母线电压经过放大器输出到P3.4引脚，这个引脚又是CMP3的正端输入，防止过流的。
过流值这个
这个是FT8132里面的描述，我的要求是2A的额定，按照采样电阻0.01Ω，我的最大采样电流是31.25A，完全满足我的要求。

2.2.2.6 其他

对于时钟初始化这里，1ms的定时处理，没啥好说的。
这里加了个看门狗，之前没遇到，等之后再来看吧，一般都是防死机。

2.2.3 偏置电压采样

这三行代码实现了一个滑动平均滤波的零点偏移校正，相当的哇塞。

mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;

请添加图片描述
这是算法的等价公式，采样1000次，保证采样值在2.5±5%内。如果不在范围内，这里会报错，后续程序是无法运行的。~~还有就是如果不出意外，偏置电压采样的时候Iw_busOffset和IbusOffset应该是一样的。~~ 从下图调试结果来看，还是有细微差别的。
请添加图片描述

2.3 电机控制

2.2.4打的太烦了，直接重开一章，哈哈哈，写博客就是爽，想咋写就咋写。这部分内容也比较多，会把中断部分也插在里面写，整体按照状态机的转移来看，还有就是无感启动的算法学习。

2.3.1 mcReady

if (SPEED_MODE == NONEMODE)
    {
        isCtrlPowOn              = true;  // 开机
        #if (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
        {
          mcFocCtrl.Ref            = MOTOR_SPEED_MIN_RPM;
        }
    }

前面SPEED_MODE设置了NONEMODE，也就是上电运行；这里有个电流校准标记位，MOE=0，不使能FOC，开启了AD4和AD2。从上面看到现在，这里应该是不用处理的，不过可能是为了停机再启动；然后就是有判断是否故障，若是没有故障就等待启动标记和偏置电压采集合适，就进入mcInit。

2.3.2 mcInit

    #if ( Shunt_Resistor_Mode == Single_Resistor)
    {
        ClrBit(ADC_MASK, CH4EN );          // 关闭软件电流采样的ADC  FOC模块会自动调用相应ADC 无需外部使能
    }

这里注释写的FOC模块会自动调用相应ADC 无需外部使能感觉比较重要，先贴这里；下面参数初始化，PI1\2\3分别表示速度、电流、功率环的PI参数设置；30ms的一个预充电，进入mcCharge。
mcCharge放一起吧，就是下三桥轮流充电，芯片里面有预驱，最好执行一下，我的只要24V，其实也不是很必要；预驱完了就是设置预定位角度和时间，然后进入mcAlign。
顺逆风先不看，先实现转起来再说。他提供了两种方式的顺逆风检测，后面可以学习一下。

2.3.3 mcAlign

这一步是定位，强拉到指定角度。先不探究原理，从代码上看，他是对FOC的寄存器进行配置，先看下它的寄存器。这里我之前也没看，要详细记录下，比较长。

2.3.3.1 FOC规格书简看

请添加图片描述
FOC 模块包含角度估算器，PI 控制器，坐标转换模块，电流采样模块和 PWM波形输出模块，可硬件实现电流闭环。角度估算器利用电机电流信号估算转子位置，实现基于无感 FOC 的电机控制。也可通过MCU处理位置传感器信号获取转子位置，实现基于有感FOC的电机控制。（唉，之前学的算啥哦，这里硬件都集成了，调个参数就结束了，生活不易，猫猫叹气）。下面写的和规格书一起看。

SetBit(DRV_CR, FOCEN);就可以打开FOC模块。
参考输入：Iq_ref由电流环和速度环一起确定；Id_ref一般为0；具体FOC讲解可以参考这篇博客。
PI控制：iq、id分别与自己的ref进行PI运算求出Uq、Ud
Park逆变换：Uq、Ud加当前的电角度，从两相旋转d-q坐标系变换到两相静止α-β坐标系。
Clarke 变换：采样到的A相电流和B相电流从三相静止A-B-C坐标系变换到两相静止α-β坐标系。
Park 变换：将采样电流从两相静止 α-β 坐标系变换到两相旋转 d-q坐标系，获得 d-q轴反馈电流Id和Iq。
SPWM：通过两个相邻矢量的和，可表示任意矢量六边形内的空间电压矢量。这里就已经可以输出正弦波了。
七段、五段式SVPWM：上面已经正弦了，这里还要七段式干嘛。正弦波的上下峰利用率太低，通过平移波形提高效率，（好像是提高了13.4%，这里有点不清楚，感兴趣可以看下dengFOC的视频，有基础FOC算法讲解）
电流电压采样（单双电阻采样的原理参考）
1）单电阻采样（感觉上面的博客写的偏重于要扩大采样间距，问了GPT，它说的还不错，内容保存在这里）
2）双电阻采样（这就是用了KCL公式，测两个求另一个）
电流偏置：初始化执行的电流偏置采样，完成后会写入FOC_CSO寄存器。
角度模式：暂时还没看出来用的那种，先不看，只知道是强拖电机到固定角度之后启动的。
电机参数

2.3.3.2 FOC初始化

FOC寄存器较多，还是有必要要一点一点看过去的，我把他和代码的一个解析放在这个博客里。（写完这里的博客之后，发现还是不要一点点看过去了，用到那个看那个就可以了，太多了，过一遍有点映像就行，大部分都是参数）。
最后整体过一下它初始化的步骤：
1）互补使能（这个前面DRV_INIT里有了）
2）使能FOC：
关闭Drviver计时器；打开FOC；滑膜算法的最大误差选择；速度滤波系数
3）配置FOC寄存器
清零FOC_CR1、FOC_CR2、Id、Iq、角度、爬坡函数的初始加速度、爬坡速度、爬坡次数、启动补偿角度、开环切闭环平滑过渡窗口大小。
4）电流环参数配置
5）位置估算参数配置
6）AO/PLL/SMO
角度估算器模式选择SMO；SMO的滑膜增益值；SMO的最小速度；禁止FOC__EALP/FOC__EBET自动计算；自适应观测器使能
7）FOC_FBASE（估算器的频率基准设定值）
它定义了转速估算与控制算法时间基准之间的比例关系。
$FOC\_FBASE = fbase*Ts*32768$
f_base：基准频率（Hz）；T_s：控制周期（即 PWM 周期）。

#define SAMP_FREQ 		(10.0 * 1000)
#define TPWM_VALUE 	(1.0 / SAMP_FREQ)
#define Pole_Pairs 	(4.0)
#define BASE_FREQ 		((MOTOR_SPEED_BASE / 60) * Pole_Pairs)
#define MOTOR_SPEED_BASE 	(4000.0)
#define OBS_FBASE 				_Q15(BASE_FREQ*TPWM_VALUE)  // 计算值是：874。基准转速4000，4对极。

FOC_FBASE 告诉估算器：“当电机达到基准速度时（BASE_FREQ Hz），每个控制周期的角度增量是多少。”
8）FOC_OMEKLPF（估算器估算速度的低通滤波系数）

一阶离散低通滤波公式：
$ω_{filtered}(k) = ω_{filtered}(k-1) + K_{LPF} \times (ω_{est}(k) - ω_{filtered}(k-1))$

其中 $K_{LPF}$ 就是 FOC_OMEKLPF 对应的系数。
9）FOC_TGLI（小PWM脉冲消除）
10）SVPWM模式设置
11）对于FOC模式，正反转的时候需要通过DRV_CR中DDIR进行控制
12）过调制使能。
13）单电阻采样配置
单电阻模式；采样时刻延迟配置；7段式SVPWM；电流采样最小窗口（采样时间+死区）设置；
14）电流偏置校准
校准itrip和ic；写入偏置
15）使能DRV计数器并设置DRV的比较值来源于FOC模块。

FOC_Init()的内容到这里就结束了，花的时间有点久了，感觉还没到看算法的地方都用了挺久的时间。不过不要心急，后面算法不理解也没关系，不理解算法不会影响电机转动的，哈哈哈哈。

2.3.3.3 FOC预定位配置

这里就是配置预定位的电流、KP、KI
1）配置 $I D REF = 0$ 和 $I QREF = 2 A$ （这里2A太大了吧，空载应该0.5A差不多，刚调试的时候。这个值是我猜的，不知道对不对。）
2）D和Q的PI参数
3）估算器的PI
4）起始角度设置：这里设置的预定位角度是-30°；设置初始估算器的角度输出为-30°。
5）使能DRV输出，开始找初始角。
程序设定了500ms来找初始角，时间到了之后就会进入静止启动配置函数。

ps：自写代码调试

一开始的时候死活没有电流输出，后面检查了Driver配置，没有选择FOC源输出给driver_out；第二个问题是预定位的IQ_REF没有Q15转换（PID的目标都没有怎么会有输出呢，自己写的代码一定要自己仔细看）。
调试的时候嗡鸣声音比较大。这个问题其实就是设置了IQ_REF，FOC中IQ控制的是力矩（会产生周期性的电磁力变化，就会出现嗡鸣），ID控制的是磁场（产生恒定的吸附力）。所以改成设置ID_REF的值就可以解决这个问题。
对于2中设置的ID值，设置0.5A时出现了启动缓慢的问题，可以设置IQ值为0.1，给较小Q轴电流，可以加快电机启动且预定位不会发出声响。

2.3.3.4 静止启动配置

1）刚刚预定位配置过D和Q、KP、KI。这里重新给它初始化一下，进入接下来的启动配置。
2）这里的MOE在FOC初始化之后就打开了，不是应该配置好了之后再开么。MOE是控制PWM输出的，开关它就可以实现PWM的输出与否。
3）配置启动的 $I D REF = 0$ 和 $I QREF = 0.1 A$ 。这里是我改了这个IQREF值，原始值我忘记了，好像也是2A，太大了我的直流源报过流了，改小之后可以正常启动，但是还是没有能进去速度环，应该还有问题，先往下面看。
4）D和Q的PI参数：D的无所谓，IDref都是0，目标值都没有就不用管PI了。Q的设置是P是0.5，I是0.02。
5）估算器的PI
6）启动方式选择：Omega_Start。
这里应该先来说一下Omega_Start，内容较多放个博客里。这里有一篇博客对他简单介绍并调试了，可以借鉴一下。
看完这两篇博客回来应该也就知道这里需要配置什么了。FOC_EFREQACC为Omega的增量；FOC_EFREQMIN为Omega的启动判断转速；FOC_EFREQHOLD为Omega的限制转速；再配置FOC_CR1中使能估算器强制角度和估算器模式。
7）mcFocCtrl.State_Count：设置Omega的启动时间1500ms，时间到了之后进入mcRun。
8）设置Q轴启动电流；这里设置这个电流值应该也可以控制电机启停，所以ME=1在前面应该是没事的。
——————
到这里我设置了FOC_EFREQHOLD为500，启动时间为20s后，速度先上升随后停止，应该就是强拖速度到了500，但是估算器输出速度为0，没有进入FOC闭环，先不着急解决，接下来看一下估算器是如何计算速度的。
mcFocCtrl.SpeedFlt = LPFFunction(FOC__EOME, mcFocCtrl.SpeedFlt, 90);
这里是一个对FOC__EOME低通滤波之后的速度值，那这里的速度就是估算器直接输出的（内部不可见）。
然后我就调试了一下：
请添加图片描述
调试上来看，速度已经到了300转，电机状态机也正常到了运行状态，说明问题不是在omega启动这里了，图上可以看到，ADCCurrent的值是0，这不是不是意味着是电流采样的问题。

2.3.3.5 电流采样相关代码

代码在DRV中断中。
从上图中，看到Power_Currt的采样值是22。根据电流公式计算请添加图片描述
得到当前母线电流是0.1678A。
不过这里有个疑问，中断里没有触发adc采样，只是等待ADC忙状态结束，在1ms的系统时钟里面是有adc采样开始的。
看了规格书里面对单电阻采样的描述，是在配置了单电阻模式之后，会在4个采样点对母线电流进行采样。所以这里也是不可见的。
这一部分代码就是采样，减去偏置，滤波。后面就把这个值送去算功率了。
算了，先不纠结，往下看。

2.3.4 mcRun

2.3.4.1 速度环

调试之后确定是速度环切入闭环的时候，电流增大，但是转速没有多少变化。先看一下速度环的代码。

速度没有超过MOTOR_LOOP_RPM，先做软化电流。

else
{
	mcFocCtrl.Mode0HoldCnt = 0;
	
	mcFocCtrl.IqRef = FOC_IQREF;                       //启动未切环前进行软化电流处理
	if(mcFocCtrl.IqRef + IQ_Start_CURRENT_ACC < IQ_Start_CURRENT_END)
	{
		mcFocCtrl.IqRef += IQ_Start_CURRENT_ACC;
	}
	else if(mcFocCtrl.IqRef - IQ_Start_CURRENT_ACC > IQ_Start_CURRENT_END)
	{
		mcFocCtrl.IqRef -= IQ_Start_CURRENT_ACC;
	}
	else
	{
		mcFocCtrl.IqRef  = IQ_Start_CURRENT_END;
	}
	FOC_IQREF = mcFocCtrl.IqRef;
}

开环电流启动，从0.1A每1ms加0.01A，最大到0.5A。

速度达到设定值，进入闭环配置

if (mcFocCtrl.SpeedFlt > MOTOR_LOOP_RPM)
{
	mcFocCtrl.Mode0HoldCnt++;
	if (mcFocCtrl.Mode0HoldCnt > 1)
	{
		FOC_THECOMP = RunThetaComp;                   // 切环时刻，运行角度补正
        mcFocCtrl.mcFOC_THECOMP = RunThetaComp;
		mcFocCtrl.CtrlMode = 1;
		FOC_QKP = QKP;
		FOC_QKI = QKI;
		FOC_DKP = DKP;
		FOC_DKI = DKI;
		ClrBit(FOC_CR2, UDD);
		
		// 启动电流环与外环给定衔接
		#if (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
		{
			if (mcFocCtrl.Start_Mode == TAILWIND_START)
			{
				mcRefRamp.OutValue_float = mcBemf.BEMFSpeed;
			}
			else
			{
				mcRefRamp.OutValue_float = mcFocCtrl.SpeedFlt;
			}
		}
		#elif (MOTOR_CTRL_MODE == POWER_LOOP_CONTROL)
		{
			mcRefRamp.OutValue_float = mcFocCtrl.PowerFlt;
		}
		#elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
		{
			mcRefRamp.OutValue_float = mcFocCtrl.UqFlt;
		}
		#endif
		//
		mcFocCtrl.LoopTime = LOOP_TIME;
		mcRefRamp.IncValue = RAMP_INC;
		mcRefRamp.DecValue = RAMP_DEC;
		mcFocCtrl.IqRef = FOC_IQREF;
		FOC_IDREF = ID_RUN_CURRENT;     // D轴启动电流
		PI1_UKH = mcFocCtrl.IqRef;
	}
}

上面就是大致参数配置，基本没啥改动。结果发现自己蠢了，前面采集电机参数的时候，代码上的电感是H，不是mH，修改之后就可以正常运行。
修改：

#define DKP                            _Q12(0.01)
#define SKP                            _Q12(0.05)
#define SOUTMAX                        I_Value(1.0)
#define Motor_Max_ADCCurrentbus        I_Value(2.0)

速度闭环

case 1:
	{
		if(mcFocCtrl.mcFOC_THECOMP + 1 < RunThetaComp_2)    // 让角度补偿FOC_THECOMP逐步逼近目标补偿RunThetaComp_2
		{
			mcFocCtrl.mcFOC_THECOMP += 1;
		}else if(mcFocCtrl.mcFOC_THECOMP - 1> RunThetaComp_2)
		{
			mcFocCtrl.mcFOC_THECOMP -= 1;
		}
		else
		{
			mcFocCtrl.mcFOC_THECOMP = RunThetaComp_2;
		}
		FOC_THECOMP = mcFocCtrl.mcFOC_THECOMP;      //切环后进行角度校准（可重写对速度进行角度补偿，增加低速力矩，但空载时效率降低）
		mcFocCtrl.LoopTime++;
		if (mcFocCtrl.LoopTime >= LOOP_TIME)
		{
			mcFocCtrl.LoopTime = 0;
			refRampOut = Motor_Ramp(mcFocCtrl.Ref);             // 控制命令爬坡函数，用于实现调速信号之间平滑过渡
			#if (MOTOR_CTRL_MODE == CURRENT_LOOP_CONTROL)
			{
				mcFocCtrl.IqRef = refRampOut;
				FOC_IQREF = mcFocCtrl.IqRef;
			}
			#elif (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
			{
				mcFocCtrl.IqSpeedRef = HW_One_PI(refRampOut - mcFocCtrl.SpeedFlt);
					mcFocCtrl.IqRef = mcFocCtrl.IqSpeedRef;
				FOC_IQREF = mcFocCtrl.IqRef;
			}
			#elif (MOTOR_CTRL_MODE == POWER_LOOP_CONTROL)
			{

				#if (OverSpeedLimitEnable)//限速
				{
					PI1_UKMAX= HW_One_PI2(MOTOR_SPEED_LIMIT_RPM - mcFocCtrl.SpeedFlt);
				}
				#endif

				mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.PowerFlt);

				#if (Weak_MagneticEn)
					Weak_Magnetic();
				#else
					FOC_IQREF = mcFocCtrl.IqRef;
				#endif
			}
			#elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
			{
				mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.UqFlt);
				FOC_IQREF = mcFocCtrl.IqRef;
			}
			#endif
		}
	}
	break;

代码解析：
第一部分就是让角度补偿FOC_THECOMP逐步逼近目标补偿RunThetaComp_2；
第二部分是爬坡函数refRampOut = Motor_Ramp(mcFocCtrl.Ref);代码中并不是直接把当前速度与目标速度进行PID运算的，而是先判断当前转速和目标值的关系，这样就避免了PI处理突变输入，比如说0转到2000转，PI输出直接让Iqref的值变成了10（假设限制为10），就会一下子输出最大电压，电机工作不平滑。
第三部分就是速度环的PI输出Iqref。