STC-FOC Lite原理剖析-舵机模式自动寻找限位功能讲解

王***

前面说了两个关于STC-FOC Lite的原理剖析，这次来说一说基于之前所说的堵转保护拓展出来的一个有趣的功能——自动寻找限位。



这个FOC电机从功能上分别实现了常见的速度模式，位置模式，舵机模式和三段式旋钮模式。
其中，舵机模式应该算是一种低成本的无刷舵机平替了，因为使用的直驱，可以完美的避免因为外部受力过大造成的齿轮扫齿问题。
并且受到超过抵抗力矩的冲击时，还可以进入堵转保护，有效保护电机。
并且速度极快，可以说没有任何的无刷舵机能比这个更快了（因为是直驱方案，所以也会损失掉力量部分，算是一个小小的缺点吧，毕竟速度和力量总是不能同时拥有的）
不过，单纯的舵机实现，仅仅就是一个带了限位的位置模式而已，如何让这个舵机模式变得更好用呢？


我根据实际的应用场景，做了个自动寻找限位功能，并且带有自动回中。
主要效果就是：上电默认为速度模式，发送模式切换指令到舵机模式，此时电机自行进入不可打断的位置模式寻找机械结构限位，寻找完左右两侧的限位后，自动计算左右限位的中点，自行回正。
并且自动将限位内的范围映射到-1000~+1000的数值上，供舵机模式使用。也就是说，如果限位大小更改，只要两边限位保持对称，那么中点就不会变，而且数值范围仍然是正负1000，不用重新计算匹配问题。
当然，如果完全没有限位，电机会在左转半圈和右转半圈后回正，此时可以为360度舵机。



下面讲解一下原理实现：
首先贴一下代码

// 线程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);
                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);
        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);
                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);
        break;
case 11:
        moto.set_postion = (Servo_Max_Pos + Servo_Min_Pos) / 2; // 自动回中
        Delay(50);
        break;
case 13:
        Servo_Config_Flag = 0; // 完成最大值最小值探测，返回开头
        Task_This[Task] = 0;
        break;
default:
        Get_Delay();
        break;
}
复制代码

这里首先会记忆当前绝对位置信息，然后分别向左和向右探测边界，探测条件是堵转保护被触发。
探测过程中，探测到一个边界以后，会回到第一次记得绝对位置，然后再次向另一边探测。
探测出左右边界以后，剩下就是使用位置模式实现了。
这部分程序是CAN处理舵机模式的数据，对输入数据叠加识别到边界位置后，交给位置环处理

case Servo_Mode:
    if (Last_Mode != Servo_Mode)
    {
        moto.set_postion = _angle_this_dat;
        _postion_add_dat = 0;  // 进入舵机模式清空累计位置
        Servo_Config_Flag = 1; // 启动一次自动位置识别
    }
    if (Can_Value < 1000 && Can_Value > -1000 &&
        (Servo_Max_Pos != Servo_Min_Pos) && Servo_Config_Flag == 0)
    // 范围限定
    {
        moto.set_postion = (long)(Servo_Min_Pos + ((float)(Servo_Max_Pos - Servo_Min_Pos) / 2000.0) *
                                                    (float)(Can_Value + 1000));
    }
    break;
复制代码