STC-FOC Lite程序详解-不同控制模式的实现

王***

      本篇文章致力于对STC-FOC Lite的原版代码进行详尽的原理解释，并且提出更改/移植/裁剪建议，方便大家对这个项目进行更加适配自己的个性化更改。

在STC-FOC Lite中，拥有丰富可修改的多种模式。

      速度模式：控制的是速度，单位是rpm（转每分钟）。

      位置模式：控制的增量位置，单位等同编码器分辨率。

      开环模式：可以用来验证驱动电路是否正常，方向和速度不可修改，可控制启停。

      舵机模式：拥有自动探测物理限位的功能，自动映射到-1000~+1000范围，探测到限位后自动归中。

      三段开关模式：可以模拟出三段的开关，并且读取到开关的状态。

      这其中，开环、速度、位置模式都是比较常见的功能，这里就着重介绍一下比较有特色的舵机模式和三段开关模式。

      这里直接上代码来解释，首先是舵机模式：

// 线程1, 舵机模式自动寻找左右限位
Task = 1;
switch (Task_This[Task])
{
case 0:
    // 初始化任务
    if (Servo_Config_Flag && Run_Flag)
    {
        // 读取当前舵机位置
        Servo_This_Pos = Read_Postion_Int_Data();
        Task_This[Task]++; // 进入下一个状态
    }
    break;

case 1:
    // 最大值探测
    if (I_Error_Cnt > I_Error_Dat)
    {
        // 限流保护，如果电流超过阈值
        Servo_Max_Pos = (int)moto.postion; // 记录最大位置
        Servo_Add_Pos = 0; // 自增清零
        Task_This[Task] = 4; // 跳转到回到中心点的任务
    }
    else
    {
        // 逐步增加舵机位置
        Servo_Add_Pos += 10;
        moto.set_postion = Servo_This_Pos + Servo_Add_Pos;
        Delay(1); // 延迟1毫秒

        // 如果自增位置超过编码器的最大范围的一半
        if (Servo_Add_Pos >= (Encode_bit_Max / 2))
        {
            Task_This[Task] = 4; // 跳转到回到中心点的任务
            Servo_Max_Pos = (int)moto.postion; // 记录最大位置
            Servo_Add_Pos = 0; // 强制中断
        }
    }
    break;

case 3:
    // 重新开始最大值探测
    Task_This[Task] = 1;
    break;

case 4:
    // 回到中心点
    moto.set_postion = Servo_This_Pos; // 设置舵机回到初始位置
    if (I_Error_Cnt < I_Error_Dat) // 堵转保护报警消失
        Delay(200); // 延迟200毫秒
    break;

case 6:
    // 最小值探测
    if (I_Error_Cnt > I_Error_Dat)
    {
        // 限流保护，如果电流超过阈值
        Servo_Min_Pos = (int)moto.postion; // 记录最小位置
        Servo_Add_Pos = 0; // 自增清零
        Task_This[Task] = 9; // 跳转到回到中心点的任务
    }
    else
    {
        // 逐步减少舵机位置
        Servo_Add_Pos -= 10;
        moto.set_postion = Servo_This_Pos + Servo_Add_Pos;
        Delay(1); // 延迟1毫秒

        // 如果自增位置低于编码器的最大范围的一半
        if (Servo_Add_Pos <= -(Encode_bit_Max / 2))
        {
            Task_This[Task] = 9; // 跳转到回到中心点的任务
            Servo_Min_Pos = (int)moto.postion; // 记录最小位置
            Servo_Add_Pos = 0; // 强制中断
        }
    }
    break;

case 8:
    // 重新开始最小值探测
    Task_This[Task] = 6;
    break;

case 9:
    // 回到中心点
    moto.set_postion = Servo_This_Pos; // 设置舵机回到初始位置
    if (I_Error_Cnt < I_Error_Dat) // 堵转保护报警消失
        Delay(200); // 延迟200毫秒
    break;

case 11:
    // 自动回中
    moto.set_postion = (Servo_Max_Pos + Servo_Min_Pos) / 2; // 设置舵机到最大和最小位置的中间
    Delay(50); // 延迟50毫秒
    break;

case 13:
    // 完成最大值最小值探测，返回开头
    Servo_Config_Flag = 0; // 标记配置完成
    Task_This[Task] = 0; // 重置任务状态
    break;

default:
    Get_Delay(); // 处理其他状态
    break;
}
复制代码

      以上代码其实是实现了一个舵机位置探测和自动校准的功能。具体步骤如下：

步骤一：逐步增加舵机位置

通过逐步增加舵机的绝对位置，每次增加10个单位。如果自增位置超过编码器最大范围的一半，则跳转到步骤四，记录当前最大位置，并将舵机位置重置到初始位置。超过范围意味着半圈内并没有机械限位，所以要放弃搜索，转去搜索另一个边界。

步骤三：重新开始最大值探测

重新开始最大值探测的任务，和任务1进行配合，完成延迟后重复的功能，类似没有死循环的for语句。

步骤四：回到中心点

舵机回到初始位置。如果堵转保护报警消失，则延迟200毫秒。

步骤六：最小值探测

进行最小值探测。如果电流超过阈值，则记录最小位置，并跳转到步骤九。否则，逐步减少舵机位置，每次减少10个单位。如果自增位置低于编码器最大范围的一半，则跳转到步骤九，记录当前最小位置，并将舵机位置重置到初始位置。

步骤八：重新开始最小值探测

重新开始最小值探测的任务。

步骤九：回到中心点

舵机回到初始位置（这个位置可能并不是中间）。如果堵转保护报警消失，则延迟200毫秒。

步骤十一：自动回中

舵机自动回到最大和最小位置的中间位置，并延迟50毫秒，等待转动稳定。

步骤十三：完成最大值最小值探测

完成最大值和最小值探测后，标记配置完成，并重置任务状态。

默认处理：

处理其他状态。

      以上步骤中的“Task_This[Task]”变量用于控制任务的跳转，而“Servo_Add_Pos”变量则用于逐步增加或减少舵机的位置。此外，“Servo_Max_Pos”和“Servo_Min_Pos”变量分别用于记录舵机的最大和最小位置，“Servo_Config_Flag”变量用于标记配置是否完成。

      这部分代码的精髓就是巧妙利用了堵转保护功能来实现边界检测，因为在较为灵敏的堵转保护和较低的转速驱动时，电机驱动过程中并不会出现大电流的情况，十分安全。通过记忆堵转后的位置，也可以获得十分精确的位置信息。这部分边界位置信息会在后面的舵机模式中，充当电子限位的功能。并且，电子限位会对边界位置信息进行内缩，保证运行过程中尽量不与机械边界进行碰撞。

      讲完了舵机模式，再讲一下这个三段开关（更改一下也可以是多段开关）

//                 线程4，多档开关DEMO,挡位分别为0,4000,8000
// 如果超出范围，默认回到4000挡位。每个挡位的极限变化值是1500
Task = 4;
switch (Task_This[Task])
{
case 0:
    if (Mode == Set_ID_Mode)
    {
        if (moto.postion > 4500 || moto.postion < 3500)
            moto.set_postion = 4000;
        else
            Task_This[Task]++;
    }
    break;
case 1:
#define Up_Gear 4000
#define Zero_Gear 0
#define Down_Gear -4000
#define Gear_Offset 2500
    if (moto.postion > (Up_Gear - Gear_Offset) && moto.postion < (Up_Gear + Gear_Offset) && moto.set_postion != Up_Gear)
        moto.set_postion = (Up_Gear);
    if (moto.postion > (Zero_Gear - Gear_Offset) && moto.postion < (Zero_Gear + Gear_Offset) && moto.set_postion != Zero_Gear)
        moto.set_postion = (Zero_Gear);
    if (moto.postion > (Down_Gear - Gear_Offset) && moto.postion < (Down_Gear + Gear_Offset) && moto.set_postion != Down_Gear)
        moto.set_postion = (Down_Gear);
    if (Mode != Set_ID_Mode)
        Task_This[Task] = 0; // 返回等待状态
    break;
default:
    Get_Delay();
    break;
}
复制代码

      这部分代码中，定义了几个位置作为开关的中心位置。然后，通过使用一个较小力矩的PID（即欠调参数，人能拧动，但是保持有力矩），在超过边界后将设定位置切换到下一个开关中心位置，以实现阻拦->牵引的变化过程，通过调节PID的参数，即可实现一个多段开关的效果。

      这个也是电机中的一种交互输入模式，类似旋钮开关。其他的模式多是一些输出场景，这个模式按道理来说是可以用来设定参数的，不过应用上我并没有深入开发，这等待大家自行探索了。

晚***