玄铁K230实现双电机FOC云台控制：基于RT-Smart与MicroPython的人脸跟踪实战

项目背景及功能

目前大多数基于玄铁K230实现的FOC（磁场定向控制）云台控制方案，往往将K230作为上位机使用，通过串口等通讯协议向其他单片机发送控制信号，再由这些单片机来驱动无刷电机。这种方案在架构上增加了一层，并非完全的原生驱动。

而本文分享的方案，其核心在于直接在玄铁K230上运行FOC控制算法。算法基于RT-Smart（RT-Thread的一个分支）实现，并利用了硬件定时器来实时更新输出力矩，相比于单纯使用MicroPython实现，其实时性表现要优异得多。

与此同时，我们还将这套FOC驱动算法封装成了MicroPython的库，这样就可以在MicroPython环境中直接调用。这样一来，我们既享受到了MicroPython便捷的代码编写与调试特性，又保证了底层控制的高实时性。开发者可以轻松地结合MicroPython官方提供的各种AI库与我们自定义的FOC库，快速实现诸如物体自动跟踪等高级功能。

重要提示：本项目的主要代码和核心功能都是在RT-Smart上使用C语言完成的，MicroPython层主要起封装和调用作用，并非仅用MicroPython实现。

效果演示

我们在官方提供的人脸识别例程基础上，集成了电机控制功能，将识别出的人脸位置坐标作为闭环控制的输入。

自动跟踪的流畅度很大程度上取决于AI模型的推理效率。本例中使用的人脸检测模型处理一帧图像大约需要40ms，跟踪效果尚可。我也尝试过例程中提供的YOLO模型，其推理速度约为120ms一帧，此时跟踪效果就会显得比较卡顿。后续会尝试集成更快的模型进行优化。

另外需要说明的是，CanMV IDE的帧缓冲区显示存在一定延迟。如果将脚本保存到玄铁K230开发板上离线运行，实际的跟踪效果会比在IDE的帧缓冲区中看到的更加流畅。

外设使用情况概述

本项目中，玄铁K230的片上资源得到了充分利用：

• PWM：使用了全部6路PWM输出，用于驱动两个FOC电机（每个电机需3路PWM）。
• I2C：使用了两路I2C总线，分别用于读取两个AS5600磁编码器的角度值。
• 定时器：使用了硬件定时器0，以固定频率中断来实时更新电机的输出力矩。

硬件设计

玄铁K230芯片本身支持6路PWM输出，这正好满足驱动一个双轴FOC云台（两个电机）的需求。然而，在笔者使用的立创·庐山派K230-CanMV开发板上，默认只引出了5路PWM引脚，这让我们面临一点小挑战。

起初，笔者以为只能驱动一个电机，几乎要放弃双电机方案。但在查阅原理图后，发现了一个非常巧妙的解决方案：板上用户按钮（USR）所使用的引脚是GPIO53，而该引脚正好可以复用为PWM5的输出功能。并且这个焊盘的位置相对独立，便于焊接，修改后也不会影响按钮原有的功能。

焊接完成后，如果担心焊点较小容易脱焊，可以适量点一些热熔胶或涂覆绿油进行加固。

为了方便接线，笔者设计了一块简单的驱动板，其上集成MS8313电机驱动芯片。此板设计较为简易，仅供参考。

电机采用常见的2804无刷电机，驱动芯片为MS8313，位置反馈使用AS5600磁编码器。整个FOC系统的硬件连接框图如下：

系统组装后的实物如下图所示。由于撰写本文时定制电路板尚未到货，图中暂时使用杜邦线进行连接，因此线路看起来有些凌乱。

引脚连接对应表：

云台Y轴电机编码器
IIC1_SCL → GPIO34
IIC1_SDA → GPIO35

云台X轴电机编码器
IIC0_SCL → GPIO48
IIC0_SDA → GPIO49

Y轴电机三路PWM
PWM0 → GPIO42
PWM2 → GPIO46
PWM4 → GPIO52
GPIO_OUTPUT → GPIO40 用作Y轴电机的EN引脚

X轴电机三路PWM
PWM1 → GPIO61
PWM3 → GPIO47
PWM5 → GPIO53
GPIO_OUTPUT → GPIO04 用作X轴电机的EN引脚

软件设计

下图展示了本项目的软件总体架构。MicroPython本质上是运行在RT-Smart之上的一个应用程序，它对芯片底层的一些驱动函数进行了封装和抽象，极大地简化了上层应用的开发。

然而，MicroPython在实时性和灵活性方面相对较低，并且玄铁K230平台上的MicroPython目前不支持硬件定时器。FOC控制对实时性有较高要求，因此我选择先用C语言实现FOC的核心库，这样就可以直接调用硬件定时器，确保控制循环的准时性，然后将C库封装成MicroPython模块供上层调用。

具体实现过程

6.1 搭建CanMV K230开发环境

玄铁K230是一款大小核异构芯片，官方提供了四种主要的开发环境：

• CanMV：大核运行RT-Smart，无Linux系统。上电后自动进入MicroPython环境，可连接CanMV IDE进行开发。RT-Smart的调试串口为Uart3。
• K230 RT-Smart Only：大核运行RT-Smart，无Linux，与CanMV环境相比缺少MicroPython。
• SDK Linux：大核运行Linux，无RT-Smart，纯Linux开发环境。
• SDK Linux + RT-Smart SDK：大核运行RT-Smart，小核运行Linux。兼顾了RT-Smart的实时硬件操作和Linux的丰富资源，但镜像编译时间最长。

由于我们需要使用MicroPython，因此选择搭建CanMV开发环境。具体搭建步骤可参考官方文档。

这里建议使用WSL Ubuntu 20.04.06 LTS进行环境搭建，编译和调试都可以在Windows下完成，非常方便。

环境搭建完成后，在SDK根目录下执行 make list-def 命令，可以查看SDK支持的所有开发板配置。我们选择带有 lckfb 字样的配置（对应立创庐山派开发板）。

然后输入 make k230_canmv_lckfb_defconfig 选择我们的编译目标。紧接着执行 make 进行全局编译。如果是首次搭建环境，必须完整编译一次，否则后续的局部编译可能无法正常工作。如果遇到权限问题，可对相应文件夹执行 chmod 777。当终端输出 Build K230 done 时，表示编译成功。

这个开发环境提供了几个非常重要的源码和例程目录，是学习和开发的基础：

• RT-Smart用户态操作例程：/canmv_k230/src/rtsmart/mpp/userapps/sample
  
• HAL库驱动源码：/canmv_k230/src/rtsmart/libs/rtsmart_hal/drivers
  
• HAL库测试例程：/canmv_k230/src/rtsmart/libs/testcases/rtsmart_hal
  
• MicroPython移植与模块代码：/canmv_k230/src/canmv/port
  

如果要在RT-Smart上进行底层开发，建议仔细阅读官方文档并参考这些源码。本项目主要利用HAL库来实现功能。

注意：这些例程并非专为K230开发板编写，因此直接运行可能无法看到效果。例如，用户态例程中的 sample_pwm，如果我们要使用它输出PWM，必须参考 sample_gpio 的写法，先重新绑定FPIOA引脚到PWM功能，否则不会有输出。

6.2 编写AS5600编码器I2C驱动

首先初始化I2C，包括绑定引脚和创建I2C实例对象。

   #define i2c_clock 4000000
   drv_i2c_inst_t* i2c = NULL;
   if(drv_fpioa_set_pin_func(34, IIC1_SCL) == -1 || drv_fpioa_set_pin_func(35, IIC1_SDA) == -1)
   {
       printf("Failed to set fpioa pin function\n");
       return -1;
   }
   if(drv_i2c_inst_create(1, i2c_clock, 1000, 0xff, 0xff, &i2c) == -1)
   {
       printf("Failed to create i2c instance\n");
       return -1;
   }

接下来是读取AS5600编码器值的函数。通过配置 i2c_msg_t 结构体中的 .flags 成员，可以指示I2C总线进行读或写操作。关于I2C的详细操作，建议深入阅读HAL库的I2C驱动源码，官方文档和例程在此方面的说明不够全面（例如，例程只给出了写操作，没有读操作）。

   #define AS5600_I2C_ADDR 0x36
   #define AS5600_ANGLE_REG 0x0C
   uint16_t AS5600_Get_Angle(drv_i2c_inst_t* i2c)
{
       uint8_t write_buf[1] = {AS5600_ANGLE_REG};  // 要写入的寄存器地址
       uint8_t read_buf[2];  // 读取数据的缓冲区，最大 256 字节
       // 构造写消息，发送要读取的寄存器地址
       i2c_msg_t write_msg = {
           .addr = AS5600_I2C_ADDR,
           .flags = DRV_I2C_WR,
           .len = 1,
           .buf = write_buf
       };
       // 构造读消息，读取寄存器数据
       i2c_msg_t read_msg = {
           .addr = AS5600_I2C_ADDR,
           .flags = DRV_I2C_RD,
           .len = 2,
           .buf = read_buf
       };
       i2c_msg_t msgs[2] = {write_msg, read_msg};  // 消息数组
       drv_i2c_transfer(i2c, msgs, 2);  // 发送 I2C 消息
       uint16_t raw_angle = (read_buf[0] << 8) | read_buf[1];
       return raw_angle;
   }

完成以上代码，我们就可以读取到编码器的原始数据了。可以进一步编写一个函数，将AS5600的原始读数转换为弧度值。

   float getAngle_Without_track(drv_i2c_inst_t* i2c)
{
       float Angle = AS5600_Get_Angle(i2c)*0.08789* PI / 180;
       return Angle;
   }

6.3 编写电机三路PWM输出驱动

与I2C操作类似，先通过FPIOA绑定引脚功能。但PWM的操作用法略有不同，是直接通过通道号调用函数，而非通过一个对象句柄。

       int ret = 0;
       ret |= drv_fpioa_set_pin_func(42, PWM0);
       ret |= drv_fpioa_set_pin_func(46, PWM0);
       ret |= drv_fpioa_set_pin_func(52, PWM0);
       if(ret != 0)
       {
           printf("Failed to set pwm fpioa pin function\n");
           return -1;
       }
       drv_pwm_init();
       drv_pwm_set_freq(0, 100000);
       drv_pwm_set_freq(0, 100000);
       drv_pwm_set_freq(0, 100000);
       drv_pwm_set_duty(0, 0);//第一个形参是要操作的PWM通道，第二个是占空比
       drv_pwm_set_duty(0, 0);
       drv_pwm_set_duty(0, 0);
       drv_pwm_enable(0);
       drv_pwm_enable(0);
       drv_pwm_enable(0);
       return 0;

我们可以将设置一个电机三路PWM占空比的操作封装成一个函数。

   void setPwm(float Ua, float Ub, float Uc, int PWM0_CHANNEL, int PWM1_CHANNEL, int PWM2_CHANNEL)
{
       // 限制占空比从0到1
       float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
       float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
       float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
       //写入PWM到PWM 0 1 2 通道
       drv_pwm_set_duty(PWM0_CHANNEL - PWM0, (int)(dc_a*100));
       drv_pwm_set_duty(PWM1_CHANNEL - PWM0, (int)(dc_b*100));
       drv_pwm_set_duty(PWM2_CHANNEL - PWM0, (int)(dc_c*100));
   }

6.4 实现FOC算法

有了角度输入（编码器）和力矩输出（PWM），我们就可以实现FOC算法了。这部分我主要参考了Deng_FOC开源项目的设计思路。

作为FOC的初学者，我目前实现了基于电压和位置的力矩闭环控制，后续会逐步完善其他闭环模式。相关参考项目链接：https://github.com/ToanTech/DengFOC_Lib/tree/main

首先定义两个角度归一化函数，用于将电角度限制在合理的范围内。

   // 归一化角度到 [0,2PI]
   float _normalizeAngle(float angle)
   {
     float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
     return a >= 0 ? a : (a + 2*PI); 
   }
   // 将角度限制到 -180 到 +180 度的函数
   float _normalizeAngle_180(float angle) 
   {
       while (angle > 180.0) 
       {
           angle -= 360.0;
       }
       while (angle < -180.0) 
       {
           angle += 360.0;
       }
       return angle;
   }

下面是执行克拉克逆变换和帕克逆变换，并最终设置电机力矩的核心函数。

   #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))//将amt限制在[low, high]
   void setTorque(float Uq,float angle_el, float *Ualpha, float *Ubeta, float *Ua, float *Ub, float *Uc, int PWM0_CHANNEL, int PWM1_CHANNEL, int PWM2_CHANNEL)
{
     Uq=_constrain(Uq,-voltage_power_supply/2,voltage_power_supply/2);
   //   float Ud=0;
     angle_el = _normalizeAngle(angle_el);
     // 帕克逆变换
     *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, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL);
   }

为了进行准确的磁场定向，需要获取编码器读数为零时对应的电角度（对齐）。

   void DFOC_alignSensor(drv_i2c_inst_t* i2c, float *zero_electric_angle, float *Ualpha, float *Ubeta, float *Ua, float *Ub, float *Uc, int PWM0_CHANNEL, int PWM1_CHANNEL, int PWM2_CHANNEL)
{ 
     setTorque(3, _3PI_2, Ualpha, Ubeta, Ua, Ub, Uc, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL);
     sleep(1);
     *zero_electric_angle=_electricalAngle(0.0f, i2c);
     setTorque(0, _3PI_2, Ualpha, Ubeta, Ua, Ub, Uc, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL);
     printf("0电角度：%f\n", *zero_electric_angle);
   }

根据对齐得到的零电角度和当前编码器读数，计算电机实时的电角度。

   float _electricalAngle(float zero_electric_angle, drv_i2c_inst_t* i2c)
   {
     return  _normalizeAngle((float)(DIR *  PP) * getAngle_Without_track(i2c)-zero_electric_angle);
   }

在实际使用中，首先需要初始化FOC控制器。以下是一些必要的宏定义和全局变量声明。

   #define PWM0_PIN_1     42
   #define PWM1_PIN_1     46
   #define PWM2_PIN_1     52
   #define PWM0_CHANNEL_1 PWM0
   #define PWM1_CHANNEL_1 PWM2
   #define PWM2_CHANNEL_1 PWM4
   #define PWM0_PIN_2     61
   #define PWM1_PIN_2     47
   #define PWM2_PIN_2     53
   #define PWM0_CHANNEL_2 PWM1
   #define PWM1_CHANNEL_2 PWM3
   #define PWM2_CHANNEL_2 PWM5
   drv_i2c_inst_t* AS5600_i2c_1 = NULL;
   drv_i2c_inst_t* AS5600_i2c_2 = NULL;
   float voltage_power_supply;//电源电压
   float Ualpha_1,Ubeta_1=0,Ua_1=0,Ub_1=0,Uc_1=0;
   float Ualpha_2,Ubeta_2=0,Ua_2=0,Ub_2=0,Uc_2=0;

初始化流程示例：

   AS5600_Init(&AS5600_i2c_1, IIC1_SCL, 34, IIC1_SDA, 35, 4000000);
   AS5600_Init(&AS5600_i2c_2, IIC0_SCL, 48, IIC0_SDA, 49, 4000000);
   FOC_PWM_Init(PWM0_CHANNEL_1, PWM0_PIN_1, PWM1_CHANNEL_1, PWM1_PIN_1, PWM2_CHANNEL_1, PWM2_PIN_1);
   FOC_PWM_Init(PWM0_CHANNEL_2, PWM0_PIN_2, PWM1_CHANNEL_2, PWM1_PIN_2, PWM2_CHANNEL_2, PWM2_PIN_2);
   DFOC_Vbus(12.6);   //设定驱动器供电电压
   DFOC_alignSensor(AS5600_i2c_1, 7, -1, &zero_electric_angle_1, 
                   &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1);
   DFOC_alignSensor(AS5600_i2c_2, 7, -1, &zero_electric_angle_2, 
                   &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2);
   PID_Init(&pid1, 0.15, 0, 1.8, motor_target_1);
   PID_Init(&pid2, 0.15, 0, 1.8, motor_target_2);
   printf("FOC_Init\n");

在控制循环中，需要不断读取传感器角度，通过PID计算输出力矩，并应用FOC变换。

   float Sensor_Angle_1=getAngle_Without_track(AS5600_i2c_1);
   float Sensor_Angle_2=getAngle_Without_track(AS5600_i2c_2);
   Motor_Output_1 = PID_Compute(&pid1, Sensor_Angle_1);//笔者实测在位置闭环中加个D项会快很多
   Motor_Output_2 = PID_Compute(&pid2, Sensor_Angle_2);
   setTorque(Motor_Output_1,_electricalAngle(zero_electric_angle_1, AS5600_i2c_1), 
             &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1);
   setTorque(Motor_Output_2,_electricalAngle(zero_electric_angle_2, AS5600_i2c_2), 
             &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2);

由于我们需要将FOC库封装给MicroPython调用，力矩更新不能简单放在一个 while(1) 循环里。为了保证严格的实时性，必须使用硬件定时器中断。下面配置一个定时器，在中断回调函数中执行上述控制逻辑。

定时器通过一个实例对象来操作，中断逻辑在回调函数中执行。初始化定时器时，需要先停止定时器，再进行配置。hard_timer_callback 是我们自定义的中断服务函数。

   void hard_timer_callback(void* args)
{
     float Sensor_Angle_1=getAngle_Without_track(AS5600_i2c_1);
     float Sensor_Angle_2=getAngle_Without_track(AS5600_i2c_2);
     Motor_Output_1 = PID_Compute(&pid1, Sensor_Angle_1);//笔者实测在位置闭环中加个D项会快很多
     Motor_Output_2 = PID_Compute(&pid2, Sensor_Angle_2);
     setTorque(Motor_Output_1,_electricalAngle(zero_electric_angle_1, AS5600_i2c_1), 
               &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1);
     setTorque(Motor_Output_2,_electricalAngle(zero_electric_angle_2, AS5600_i2c_2), 
               &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2);
   }
   int Timer_Init(int timer_num, drv_hard_timer_inst_t** timer)
{
       rt_hwtimer_info_t info;
       uint32_t freq;
       if(drv_hard_timer_inst_create(timer_num, timer) == -1)
       {
           printf("Failed to create timer instance\n");
           return -1;
       }
       drv_hard_timer_stop(*timer);
       drv_hard_timer_set_mode(*timer, HWTIMER_MODE_PERIOD);
       drv_hard_timer_get_info(*timer, &info);
       uint32_t valid_freq = (info.minfreq + info.maxfreq) / 2;
       drv_hard_timer_set_freq(*timer, valid_freq);
       drv_hard_timer_get_freq(*timer, &freq);
       printf("Timer frequency: %d Hz\n", freq);
       drv_hard_timer_set_period(*timer, 1);
       drv_hard_timer_register_irq(*timer, hard_timer_callback, NULL);
       drv_hard_timer_start(*timer);
       return 0;
   }

6.5 测试FOC底层代码

至此，FOC的底层C代码已编写完成。在封装到MicroPython之前，可以先编译成独立的可执行文件进行测试。

我们需要在代码中声明主函数 main，并依次调用初始化函数。可以通过命令行参数来灵活设置电机的目标位置，方便测试。

   int main(int argc, char *argv[])
{
       if (argc < 2) 
       {
           // 检查是否有命令行参数传入，如果没有则提示用户并退出程序
           printf("请至少传入一个整数参数。\n");
           return 1;
       }
       for (int i = 1; i < argc; i++) 
       {
           // 使用 atoi 函数将字符串参数转换为整数
           num[i - 1] = atoi(argv[i]);
           printf("第 %d 个参数转换后的整数是: %d\n", i, num);
       }
       motor_target_1 = (float)num[0];
       motor_target_2 = (float)num[1];
       AS5600_Init(&AS5600_i2c_1, IIC1_SCL, 34, IIC1_SDA, 35, 4000000);
       AS5600_Init(&AS5600_i2c_2, IIC0_SCL, 48, IIC0_SDA, 49, 4000000);
       FOC_PWM_Init(PWM0_CHANNEL_1, PWM0_PIN_1, PWM1_CHANNEL_1, PWM1_PIN_1, PWM2_CHANNEL_1, PWM2_PIN_1);
       FOC_PWM_Init(PWM0_CHANNEL_2, PWM0_PIN_2, PWM1_CHANNEL_2, PWM1_PIN_2, PWM2_CHANNEL_2, PWM2_PIN_2);
       DFOC_Vbus(12.6);   //设定驱动器供电电压
       DFOC_alignSensor(AS5600_i2c_1, 7, -1, &zero_electric_angle_1, 
                       &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1);
       DFOC_alignSensor(AS5600_i2c_2, 7, -1, &zero_electric_angle_2, 
                       &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2);
       PID_Init(&pid1, 0.15, 0, 1.8, motor_target_1);
       PID_Init(&pid2, 0.15, 0, 1.8, motor_target_2);
       printf("FOC_Init\n");
       Timer_Init(0, &timer);
       while(1)
       {
       }
   }

将上述测试代码文件放置到 /canmv_k230/src/rtsmart/libs/testcases/rtsmart_hal 目录下，然后在该目录下直接执行 make 命令即可编译。

如果终端输出 [SUCCESS] Built all RtSmart HAL testcases，说明编译成功。编译生成的 .elf 可执行文件位于 canmv_k230/output/k230_canmv_lckfb_defconfig/rtsmart/libs/elf 目录中。

将对应的 .elf 文件拷贝到SD卡中。使用TTL转USB模块连接K230的UART3串口，通过串口终端调试工具即可运行测试。

如果使用的是官方的CanMV镜像，上电初始化后会默认运行MicroPython。在串口终端中，先按 Ctrl+C 退出MicroPython，然后按回车进入RT-Smart的msh shell。切换到存放 .elf 文件的目录（例如 /data），执行该文件即可。

6.6 将C库封装到MicroPython

底层测试通过后，就可以开始移植到MicroPython了。首先在 /canmv_k230/src/canmv/port 目录下为我们库新建一个文件夹（例如 dfoc），并在其中创建 .c 源文件，将我们的核心代码复制进去。

封装的第一步是引入MicroPython的必要头文件：

#include "py/obj.h"
#include "py/runtime.h"

然后编写模块的初始化函数。所有暴露给MicroPython的函数返回值类型必须是 mp_obj_t。如果函数没有返回值，则返回 mp_const_none。

   STATIC mp_obj_t DFOC_Init(void)
{
       AS5600_Init(&AS5600_i2c_1, IIC1_SCL, 34, IIC1_SDA, 35, 4000000);
       AS5600_Init(&AS5600_i2c_2, IIC0_SCL, 48, IIC0_SDA, 49, 4000000);
       ...
       ...
       Timer_Init(0, &timer);
       mp_printf(&mp_plat_print, "FOC Module Initialized\n");
       return mp_const_none;
   }
   STATIC MP_DEFINE_CONST_FUN_OBJ_0(mp_DFOC_Init_obj, DFOC_Init);

其他控制函数的编写方式类似，例如设置电机目标角度：

   STATIC mp_obj_t DFOC_Set_Motor_Angle(mp_obj_t angle_obj_1, mp_obj_t angle_obj_2)
{
       pid1.setpoint = mp_obj_get_float(angle_obj_1);
       pid2.setpoint = mp_obj_get_float(angle_obj_2);
       return mp_const_none;
   }
   STATIC MP_DEFINE_CONST_FUN_OBJ_2(mp_DFOC_Set_Motor_Angle_obj, DFOC_Set_Motor_Angle);

编写完每个函数后，都需要使用 MP_DEFINE_CONST_FUN_OBJ_* 系列宏来注册该函数对应的MicroPython函数对象。需要根据函数参数的数量选择对应的宏，例如：

• MP_DEFINE_CONST_FUN_OBJ_0 (无参数)
• MP_DEFINE_CONST_FUN_OBJ_1 (1个参数)
• MP_DEFINE_CONST_FUN_OBJ_2 (2个参数)
• MP_DEFINE_CONST_FUN_OBJ_VAR (可变参数)

对于参数大于2个的函数，写法如下：

   STATIC mp_obj_t DFOC_Set_PID(size_t n_args, const mp_obj_t *args)
{
       mp_obj_t Kp_obj = args[0];
       mp_obj_t Ki_obj = args[1];
       mp_obj_t Kd_obj = args[2];
       mp_obj_t num = args[3];
       if(mp_obj_get_int(num) == 1)
       {
           pid1.Kp = mp_obj_get_float(Kp_obj);
           pid1.Ki = mp_obj_get_float(Ki_obj);
           pid1.Kd = mp_obj_get_float(Kd_obj);
       }
       else if(mp_obj_get_int(num) == 2)
       {
           pid2.Kp = mp_obj_get_float(Kp_obj);
           pid2.Ki = mp_obj_get_float(Ki_obj);
           pid2.Kd = mp_obj_get_float(Kd_obj);
       }
       return mp_const_none;
   }
   STATIC MP_DEFINE_CONST_FUN_OBJ_VAR(mp_DFOC_Set_PID_obj, 4, DFOC_Set_PID);

所有函数对象注册完毕后，需要将它们放入一个 mp_rom_map_elem_t 类型的全局表中。这个表定义了MicroPython模块的属性和方法，也就是模块的“符号表”。

   STATIC const mp_rom_map_elem_t dfoc_module_globals_table[] = {
       { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_dfoc) },//这个dfoc就是我们未来在写micropython要调用的库名
       { MP_ROM_QSTR(MP_QSTR_DFOC_Init), MP_ROM_PTR(&mp_DFOC_Init_obj) },//DFOC_Init就是在micropython要使用的初始化函数名
       { MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle), MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_obj) },
       { MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle_1), MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_1_obj) },
       { MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle_2), MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_2_obj) },
       { MP_ROM_QSTR(MP_QSTR_DFOC_AS5600_GetAngle), MP_ROM_PTR(&mp_DFOC_AS5600_GetAngle_obj) },
       { MP_ROM_QSTR(MP_QSTR_DFOC_Set_PID), MP_ROM_PTR(&mp_DFOC_Set_PID_obj) },
   };

最后，将这个全局表注册到MicroPython系统中，声明模块类型并完成最终注册。

   STATIC MP_DEFINE_CONST_DICT(dfoc_globals_table, dfoc_module_globals_table);
   const mp_obj_module_t dfoc_module = {
       .base = { &mp_type_module },
       .globals = (mp_obj_dict_t *)&dfoc_globals_table,
   };
   MP_REGISTER_MODULE(MP_QSTR_dfoc, dfoc_module);

6.7 编译集成FOC模块的MicroPython

模块代码编写完成后，需要将其加入编译系统。在 /canmv_k230/src/canmv/port 目录下的 Makefile 文件中，添加一行，将我们新建的 dfoc 文件夹包含到编译列表中。

然后，返回上一级的 /canmv_k230/src/canmv 目录，执行 make 命令进行编译。如果代码没有问题但编译失败，可以尝试清理编译缓存（make clean），然后重新进行全局编译（在SDK根目录执行 make）。

编译成功后，会在 canmv_k230/output/k230_canmv_lckfb_defconfig/canmv 目录下生成一个新的 micropython 可执行文件。使用此文件替换掉SD卡中原有的 micropython 文件即可。可以使用DiskGenius等磁盘工具进行替换操作。

6.8 在MicroPython中测试FOC库

将更新了SD卡的玄铁K230连接到CanMV IDE，测试我们刚刚封装的 dfoc 库。下面是一个简单的测试脚本，用于设置PID参数和控制电机转到特定角度。

   import time
   import dfoc
   import math
   dfoc.DFOC_Init()
   def DFOC_Set_Motor_1(angle1):
       #40 ~ 130限位，防止把线扯断了
       if(angle1 > 130):
           angle1 = 130
       elif(angle1 < -40):
           angle1 = -40
       angle1 = -angle1#这一步变换和云台的安装位置对应，我这里取-90°云台Y轴正好对着中间，所以取个负
       radians1 = math.radians(angle1)#角度转弧度
       dfoc.DFOC_Set_Motor_Angle_1(radians1)
   def DFOC_Set_Motor_2(angle2):
       #50 ~ 150
       if(angle2 > 150):
           angle2 = 150
       elif(angle2 < 50):
           angle2 = 50
       angle2 = angle2 - 90
       radians2 = math.radians(angle2)
       dfoc.DFOC_Set_Motor_Angle_2(radians2)
   dfoc.DFOC_Set_PID(0.15, 0, 2.0, 1)
   dfoc.DFOC_Set_PID(0.7, 0, 3.0, 2)
   DFOC_Set_Motor_1(90)
   DFOC_Set_Motor_2(90)
   while True:
       pass

6.9 位置闭环控制效果

封装好的FOC库，其位置闭环响应快速且平稳，能够准确地将电机驱动到指定角度。

6.10 结合官方AI库实现物体自动跟踪

最后，我们将自定义的FOC库与官方的人脸检测AI例程相结合，实现一个完整的人脸自动跟踪云台。核心思想是：将人脸在图像中的坐标偏差作为PID控制器的输入，计算出云台需要调整的角度，然后通过FOC库驱动电机。

需要在Python层实现一个PID控制器，建议加入积分限幅以防止饱和。

from libs.PipeLine import PipeLine
from libs.AIBase import AIBase
from libs.AI2D import Ai2d
from libs.Utils import *
import os,sys,ujson,gc,math
from media.media import *
import nncase_runtime as nn
import ulab.numpy as np
import image
import aidemo
import dfoc

def DFOC_Set_Motor_1(angle1):
    #40 ~ 130
    if(angle1 > 130):
        angle1 = 130
    elif(angle1 < 40):
        angle1 = 40
    angle1 = -angle1
    radians1 = math.radians(angle1)
    dfoc.DFOC_Set_Motor_Angle_1(radians1)

def DFOC_Set_Motor_2(angle2):
    #50 ~ 150
    if(angle2 > 150):
        angle2 = 150
    elif(angle2 < 50):
        angle2 = 50
    angle2 = angle2 - 90
    radians2 = math.radians(angle2)
    dfoc.DFOC_Set_Motor_Angle_2(radians2)

class PID:
    def __init__(self, kp, ki, kd, setpoint, integral_limit):
        # 比例系数
        self.kp = kp
        # 积分系数
        self.ki = ki
        # 微分系数
        self.kd = kd
        # 设定值
        self.setpoint = setpoint
        # 积分项
        self.integral = 0
        # 上一次的误差
        self.last_error = 0
        # 积分限幅的上限和下限，格式为 (min, max)
        self.integral_limit = integral_limit
    def update(self, current_value):
        # 计算当前误差
        error = self.setpoint - current_value
        # 计算积分项
        self.integral += error
        # 积分限幅
        min_limit, max_limit = self.integral_limit
        if self.integral > max_limit:
            self.integral = max_limit
        elif self.integral < min_limit:
            self.integral = min_limit
        # 计算微分项
        derivative = error - self.last_error
        # 计算PID输出
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        # 更新上一次的误差
        self.last_error = error
        return output

pid1 = PID(kp=0.015, ki=0.001, kd=0.006, setpoint=0, integral_limit=(-10, 10))
pid2 = PID(kp=0.015, ki=0.001, kd=0.006, setpoint=0, integral_limit=(-10, 10))
motor_x = 90
motor_y = 90

def move(x, y):
    global motor_x
    global motor_y
    temp1 = pid1.update(x)
    temp2 = pid2.update(y)
    motor_x = motor_x - temp1
    motor_y = motor_y - temp2
    DFOC_Set_Motor_1(motor_y)
    DFOC_Set_Motor_2(motor_x)

# 自定义人脸检测类，继承自AIBase基类
class FaceDetectionApp(AIBase):
    def __init__(self, kmodel_path, model_input_size, anchors, confidence_threshold=0.5, nms_threshold=0.2, rgb888p_size=[224,224], display_size=[1920,1080], debug_mode=0):
        super().__init__(kmodel_path, model_input_size, rgb888p_size, debug_mode)  # 调用基类的构造函数
        self.kmodel_path = kmodel_path  # 模型文件路径
        self.model_input_size = model_input_size  # 模型输入分辨率
        self.confidence_threshold = confidence_threshold  # 置信度阈值
        self.nms_threshold = nms_threshold  # NMS（非极大值抑制）阈值
        self.anchors = anchors  # 锚点数据，用于目标检测
        self.rgb888p_size = [ALIGN_UP(rgb888p_size[0], 16), rgb888p_size[1]]  # sensor给到AI的图像分辨率，并对宽度进行16的对齐
        self.display_size = [ALIGN_UP(display_size[0], 16), display_size[1]]  # 显示分辨率，并对宽度进行16的对齐
        self.debug_mode = debug_mode  # 是否开启调试模式
        self.ai2d = Ai2d(debug_mode)  # 实例化Ai2d，用于实现模型预处理
        self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8)  # 设置Ai2d的输入输出格式和类型
    # 配置预处理操作，这里使用了pad和resize，Ai2d支持crop/shift/pad/resize/affine，具体代码请打开/sdcard/app/libs/AI2D.py查看
    def config_preprocess(self, input_image_size=None):
        with ScopedTiming("set preprocess config", self.debug_mode > 0):  # 计时器，如果debug_mode大于0则开启
            ai2d_input_size = input_image_size if input_image_size else self.rgb888p_size  # 初始化ai2d预处理配置，默认为sensor给到AI的尺寸，可以通过设置input_image_size自行修改输入尺寸
            top, bottom, left, right,_ =letterbox_pad_param(self.rgb888p_size,self.model_input_size)
            self.ai2d.pad([0, 0, 0, 0, top, bottom, left, right], 0, [104, 117, 123])  # 填充边缘
            self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel)  # 缩放图像
            self.ai2d.build([1,3,ai2d_input_size[1],ai2d_input_size[0]],[1,3,self.model_input_size[1],self.model_input_size[0]])  # 构建预处理流程
    # 自定义当前任务的后处理，results是模型输出array列表，这里使用了aidemo库的face_det_post_process接口
    def postprocess(self, results):
        with ScopedTiming("postprocess", self.debug_mode > 0):
            post_ret = aidemo.face_det_post_process(self.confidence_threshold, self.nms_threshold, self.model_input_size[1], self.anchors, self.rgb888p_size, results)
            if len(post_ret) == 0:
                return post_ret
            else:
                return post_ret[0]
    # 绘制检测结果到画面上
    def draw_result(self, pl, dets):
        with ScopedTiming("display_draw", self.debug_mode > 0):
            if dets:
                pl.osd_img.clear()  # 清除OSD图像
                for det in dets:
                    # 将检测框的坐标转换为显示分辨率下的坐标
                    x, y, w, h = map(lambda x: int(round(x, 0)), det[:4])
                    xm = x + w/2 - 1280/2
                    ym = y + h/2 - 720/2
                    x = x * self.display_size[0] // self.rgb888p_size[0]
                    y = y * self.display_size[1] // self.rgb888p_size[1]
                    w = w * self.display_size[0] // self.rgb888p_size[0]
                    h = h * self.display_size[1] // self.rgb888p_size[1]
                    pl.osd_img.draw_rectangle(x, y, w, h, color=(255, 255, 0, 255), thickness=2)  # 绘制矩形框
                    pl.osd_img.draw_cross(int(1280/2 * self.display_size[0] // self.rgb888p_size[0]), int(720/2 * self.display_size[1] // self.rgb888p_size[1]), color=(255, 255, 0, 255), size=10, thickness=3)
                    move(xm, ym)
            else:
                pl.osd_img.clear()

if __name__ == "__main__":
    dfoc.DFOC_Init()
    dfoc.DFOC_Set_PID(0.15, 0, 1.8, 1)
    dfoc.DFOC_Set_PID(0.15, 0, 1.8, 2)
    DFOC_Set_Motor_1(90)
    DFOC_Set_Motor_2(90)
    time.sleep(2)
    # 添加显示模式，默认hdmi，可选hdmi/lcd/lt9611/st7701/hx8399/nt35516,其中hdmi默认置为lt9611，分辨率1920*1080；lcd默认置为st7701，分辨率800*480
    display_mode="hdmi"
    # k230保持不变，k230d可调整为[640,360]
    rgb888p_size = [1280, 720]
    # 设置模型路径和其他参数
    kmodel_path = "/sdcard/examples/kmodel/face_detection_320.kmodel"
    # 其它参数
    confidence_threshold = 0.5
    nms_threshold = 0.2
    anchor_len = 4200
    det_dim = 4
    anchors_path = "/sdcard/examples/utils/prior_data_320.bin"
    anchors = np.fromfile(anchors_path, dtype=np.float)
    anchors = anchors.reshape((anchor_len, det_dim))
    # 初始化PipeLine，用于图像处理流程
    pl = PipeLine(rgb888p_size=rgb888p_size, display_mode=display_mode)
    pl.create()  # 创建PipeLine实例
    display_size=pl.get_display_size()
    # 初始化自定义人脸检测实例
    face_det = FaceDetectionApp(kmodel_path, model_input_size=[320, 320], anchors=anchors, confidence_threshold=confidence_threshold, nms_threshold=nms_threshold, rgb888p_size=rgb888p_size, display_size=display_size, debug_mode=0)
    face_det.config_preprocess()  # 配置预处理
    while True:
        with ScopedTiming("total",1):
            img = pl.get_frame()            # 获取当前帧数据
            res = face_det.run(img)         # 推理当前帧
            face_det.draw_result(pl, res)   # 绘制结果
            pl.show_image()                 # 显示结果
            gc.collect()                    # 垃圾回收
    face_det.deinit()                       # 反初始化
    pl.destroy()                            # 销毁PipeLine实例

运行上述代码，云台能够快速定位并跟踪画面中的人脸。

开源代码

本项目的所有代码，包括C语言FOC库、MicroPython封装模块、硬件设计文件以及测试脚本，均已开源。欢迎有兴趣的开发者一起交流和完善。

项目地址：https://github.com/Death6sentence/FOC_Lib_for_K230micropython

结语

本次基于玄铁K230、RT-Smart和MicroPython的FOC云台控制系统开发，是一次有益的尝试。它展示了如何在资源丰富的RISC-V平台上，通过合理的软硬件架构设计，同时满足高性能实时控制与高效应用开发的需求。

在开发过程中，玄铁K230双核架构的潜力令人印象深刻。结合RT-Smart的实时性保障和MicroPython的开发便捷性，为嵌入式AI与实时控制融合应用提供了新的思路。目前的CanMV官方镜像仅在大核运行RT-Smart，笔者期待未来官方能提供小核同时运行Linux的CanMV镜像，这样就能更方便地在MicroPython层利用丰富的网络协议栈和网络服务，解锁更多物联网应用场景。

本项目实现的FOC库和硬件设计仍在不断完善中。希望借此抛砖引玉，吸引更多开发者参与到玄铁K230的生态建设中，共同创造出更多有趣、实用的开源项目。