• The-Little-Embedded-System
• 作者：Xian.Chen

前言

本书适合有一些嵌入式入门基础的学习者阅读。希望阅读完本小书，能够解答大家这些问题

• 硬件布线最基本的准则
• 不带RTOS的嵌入式系统如何更好地开发
• 嵌入式软件的分层设计怎么做
• 在设计控制器时，被控系统的简化动态模型怎么获得
• 传统的PID控制器设计在工程上到底怎么实施，如何仿真测试和写代码
• Matlab/Simulink怎么可以帮助提高嵌入式控制器的开发和测试效率
• 如何简单快速地做嵌入式控制器故障分析和错误排查，等等

智能车系统介绍

1.从智能车说起

本小书是以全国大学生智能车比赛作为案例来展开，这是我大学时候的最爱，也是目前自己带的学生创新实验室主要的比赛项目，所以站在这个角度给大家简单说一下如何构建一个基本的嵌入式系统，不求实现的多么完美，但求简洁而明白。

智能车比赛主要任务是做赛道寻线跟踪处理，用摄像头采集前方赛道图像，实现车速和转向的控制，达到跟踪赛道行驶的效果，如果类比人开车的话，摄像头就相当于司机的眼睛，图像处理和控制相当于老司机，转速控制相当于加减油门和刹车，转向控制相当于方向盘，这样对应理解了吧。

智能车整体结构的俯视图和侧视图如图1所示，后轮电机驱动，电池后放，摄像头中间放置，舵机改造为立式。

整个智能车系统里面涉及几部分：机械设计，硬件设计，嵌入式平台软件，控制算法软件与图像处理软件，本小书将会重点对后三部分详细展开介绍。这里要说一点，虽然本小书不探讨机械，但是整个智能车机械部分是非常重要的部分，或者可以这样说，机械调教决定了车的理论最高车速，而控制软件部分只决定实际车速，由于受限于自己自动化的专业背景，机械这方面不能过多展开。如果想深入学习机械，请大家阅读相关的专业书籍。

2.整体系统结构

智能车的整体结构如图2所示，上面是软件部分，主要包括图像处理，转速控制和转向控制，下面是硬件机械部分，主要包括电机，驱动，编码器，舵机，摄像头。整个系统的信号流和控制结构都可以从图中看出来，获取图像进行处理得到加减速指令和转向指令，下发给转速控制器和转向控制器，转向控制器负责控制舵机实现路径跟踪，转速控制器负责两个后轮的转速闭环控制。

• 机械与硬件设计（电源，编码器，电机驱动，摄像头）

• 嵌入式平台软件（系统结构，分层设计，模块设计）

• 控制算法软件（转速PI控制器，Matlab仿真，转向控制器设计）

• 图像处理软件（Matlab与C语言混合编程，图像处理与验证）

• 系统测试与分析（串口辅助调试，IO辅助测试，Matlab可视化数据分析）

3.经验之谈

一个嵌入式产品都会涉及到机械，硬件和软件，需要三部分的协作才能够完成，如果把做这个比赛看成一个项目的话，那就要考虑时间上的安排，这里要注意三者时间上的关系，机械如果有定制件的话，迭代周期会是1-2个月，硬件的迭代周期一般是1-2周，软件的迭代周期是1-2天，务必要清楚这一点，用到的机械上大部件（电机，舵机，摄像头等），必须提前采购好，做好充分的备料，这玩意如果出了问题，妥妥地托一个星期没商量，所以在满足性能和可用性的前提下，尽量简化机械与硬件的设计，做好保护措施。同时也要提前安排好时间，比如PCB发出去之前，最好所有用到的元器件要买齐，不然PCB板回来了，根本焊不出一个完整板子。

---

系统分层设计

1.分层与模块化

说起分层，让我想起了大学刚毕业去的那家小公司，当时自己维护空调控制板，代码是前辈做的，功能相当棒，但是当我看到代码的那一刻，差点吐血，因为整个代码就两个文件shuikongtiao.c和shuikongtiao.h，里面的变量和函数定义全部用汉语拼音，哈哈哈，一万只草泥马从心中飞过。我擦，终于见识到了国产一线小厂的实力水平。看到代码的那一刻，就在想，这玩意居然好使，写代码的人是爽了，维护者怎么搞呀，一个.c文件上万行。当然这些都是内心戏，说出来，前辈肯定把我废了。

我大学时学C语言，刚开始码代码就是一个main.c然后用一个main函数搞定所有的事，比如下面：

#include<stdio.h>
#include<stdlib.h>

int main(int argc,char**argv)
{
    //定义变量，balabala
    int a,b;
    int sum;
    //输入数据或者读取文件
    scanf("%d %d",&a,&b);
    //处理逻辑
    sum=a+b;
    //打印输出
    printf("%d+%d=%d",a,b,sum);
    return 0;
}

这样的单文件单函数处理一些比如简单计算，文件读写，代码行不超过两个屏还不错，当代码超过2屏（大概150行），就要开始切分模块分割函数了，于是main.c变成了下面的样子。

#include<stdio.h>
#include<stdlib.h>
struct data{
    int a;
    int b;
}
int read_data(char*filename,struct data*d)
{
    //读取数据
}
int process_data(struct data*d)
{
    //处理数据和逻辑
}
int print_data(struct data*d)
{
    //输出数据
}

int main(int argc,char**argv)
{
    //定义变量，balabala
    struct data d;

    read_data("input.dat",&d)
    process_data(&d);
    print_data(&d);

    return 0;
}

通过将部分功能模块化抽离出函数，原本上千行的代码被切割为几个50-200行的代码，既方便阅读，又方便处理，随着功能继续完善，我们会产生不同的数据处理方式，比如添加，删除，修改，查看，查找，排序等，这时候我们就需要把处理数据的部分单独拿出来成为一个独立的模块，于是我们产生了新的模块process_data.c和process_data.h，其中.c文件负责模块的代码实现，.h负责模块的对外接口声明，其他的模块也类似，于是我们的代码变成了下面几个文件main.h，process_data.c和process_data.h，read_data.c和read_data.h，print_data.c和print_data.h。

//process_data.h
#ifndef __PROCESS_DATA_DEF__
#def __PROCESS_DATA_DEF__

//数据元素
struct data{
    int a;
    int b;
}
typedef struct data* dat;

//数据链表
struct data_list{
    struct data d;
    dat    next;
}
typedef struct data_list* dat_list;
extern int new_list   (dat_list dl);
extern int add_data   (dat_list dl,dat d);
extern int update_data(dat_list dl,int index,dat d);
extern int delete_data(dat_list dl,int index);
extern dat select_data(dat_list dl,char* cmd);
extern int sort_data  (dat_list dl);
extern int search_data(dat_list dl,dat d);

#endif

//process_data.c
#include "process_data.h"


int new_list   (dat_list dl)
{
}

int add_data   (dat_list dl,dat d)
{
    //添加数据
}
int update_data(dat_list dl,int index,dat d)
{
    //更新数据
}
int delete_data(dat_list dl,int index)
{
    //删除数据
}
dat select_data(dat_list dl,char* cmd)
{
    //查找数据
}
int sort_data  (dat_list dl)
{
    //排序数据
}
int search_data(dat_list dl,dat d)
{
    //查找数据
}

#endif

这时候的main.c就把process_data，read_data，print_data包含进来，即可以使用该模块，main.c的代码进一步缩减，框架和结构更清晰明了。

#include <stdio.h>
#include <stdlib.h>
#include "process_data.h"
#include "read_data.h"
#include "print_data.h"

int main(int argc,char**argv)
{
    //定义变量，balabala
    struct data d;
    dat_list dl;
    //输入数据
    new_list(dl);
    while(read_data("input.dat",&d) != 0)
         add_data(&d);

    //一系列的数据处理过程
    select_data(dl,d);

    //个性化显示数据
    print_data(&d);

    return 0;
}

随着系统功能进一步复杂，输入设备会有各种各样，输出设备与模式也会有各种各样的适配，为了控制系统的复杂度，会进一步进行分层，整体的进化流程就如图1所示。

其实说到底，最初其分成几个函数，到后面的模块化，再到最后的分层设计，都是在简化系统的复杂度，做到局部可控，这样才能hold住全场，让我们同一时刻只关注有限的信息量，毕竟都是人类，谁能一下子接受那么多code，更何况是凌乱的呢。善待code，善待自己，请从模块和分层开始。其实分层不是绝对的完美，所有的分层都会带来效率的降低，比如额外增加的函数调用时间损耗，但是为了可读性和可维护性，牺牲一点效率又能怎么样呢。不过千万不要过度分层，那是在装逼，不是在设计。

2.整个系统的总体设计

智能车系统的模块与分层划分，总体上分为三层，如图2所示。

• 控制与图像层（转速与转向控制器，图像处理）
• 嵌入式平台层（信号采集，器件驱动，任务调度）
• 硬件与机械层（舵机，电机，硬件驱动，编码器，电源，摄像头，巴拉巴拉）

这三层划分，在一般的项目中正好对应四类工程师，控制与图像层对应控制与算法工程师，嵌入式平台层对应嵌入式软件工程师，硬件与机械层，对应嵌入式硬件工程师和机械设计工程师。如果要想实现一个完备的嵌入式系统产品，需要凑齐这四类人才才能够有备无患。

控制与图像层

系统中图像处理模块图如图3所示，主要实现图像的处理，寻找中线，以及与Matlab2011a和VS2010配合实现对算法的快速仿真验证，大大提高开发效率，后文会重点介绍这里。

• imCom：图像处理的公用模块
• imProc：图像处理找中线和计算方向偏差的算法实现
• imType：自定义数据类型
• imCar：与Matlab的接口模块，用来快速批量验证算法

系统中控制算法部分的模块图如图4所示，主要负责实现转速和转向控制，其中转速控制会结合Matlab/Simulink 进行仿真，寻找合理的PI控制参数，后面会详细展开如何设计PI控制器。

• ControlVar：所有的共享全局变量
• ControlParam：所有的全局配置参数
• ControlGraphTask：图像和方向控制任务模块
• ControlSpeedTask：速度控制任务模块

Control子模块介绍：

• EIT_PID：PID控制器模块
• EIT_SpeedL：左轮速度控制器
• EIT_SpeedR：右轮速度控制器

嵌入式平台层

嵌入式平台层，负责整个嵌入式软件系统的初始化，信号采集以及驱动执行，模块结构如图5所示，其中本小书会详细介绍EITLIb库中的电机驱动与编码器和摄像头采集部分。

• CarDisplay：显示模块
• CarSystem：系统初始化模块
• CarTest：主循环模块
• IntHandler：中断处理模块
• Board：Vcan山外的K60核心板库
• EITLib：自定义的硬件驱动库
• Chip：Vcan山外实现的 K60的部件库
• CMSIS：CMSIS支持库

硬件与机械层

硬件采用山外的K60核心板，其他部分，控制核心板和驱动电源板都是自制。模块结构如图6所示，其中主要的是Power，Camera，Motor，Sensor，LCD和Key模块图。

机械部分在ch2再做介绍。

到现在为止，对整个智能车系统有了一个总体的了解，下面我们会分模块进行详细的介绍。

---

机械与硬件设计部分

1.硬件布线

当初大学画电路板的时候，啥玩意都不懂，记得当初直接用面包板焊接，虽然好使，但其丑无比。后来工作也画过一些PCB，但是始终不得要领，以为能把线连上连对就万事大吉了。后来阅读了一些电子电路和硬件的书籍，有了自己的一点点体会，虽然不多，但是应付一般场合足够了。很简单的物理知识就可以理解，至于高手搞通信高频布线，要考虑群延时，分布电感电容，信号完整性分析，我觉得大家以后真的玩高级硬件的话，可以再深入，这里就算是一个简单入门吧。

这里我就先画一个非常简单的电路图，做一个简单的计算，大家就会明白。

比如一个电源给两个器件供电，一个工作电流10mA，一个工作电流10A，我们看下面图1中两种接线方式有啥区别？？？？？

很多同学会说，不都是两个器件并联吗，不都是电源电压吗？？？电路原理的课上说啦，并联电压相等，所以上面的两张图，完全一个样子嘛，能有啥区别？？？！！！！

这里我要提醒大家啦，注意连线，那都是铜线，不是超导体，拿起小本子要记住啦，铜线也有阻抗，PCB布线也有阻抗。考虑进来，之后的图就是下面这样子图2所示。

有的同学看到图2，会说，才20m欧，能有啥大事嘛。。。。大家要记住，我们这里还没说电源电压多少V呢，如果是1000V的话，那当然没事，但是如果电源是7V电压呢？？？你想想两种布局方式，1A器件与10A器件的静态工作电压有什么差别？？

左侧布局	右侧布局
1A器件电压：7-(1+10)*(0.02*2)=6.56V	1A器件电压：7-10*0.02*2-1*0.02*4=6.52V
10A器件电压：7-10*0.02*4-1*0.02*2=6.16V	10A器件电压：7-10*0.02*2-1*0.02*2=6.56V

只通过一个简单的计算，我们就发现，右侧布局明显好于左侧布局，就因为10A电流在线路上产生了更大的线损，所以越靠近电源越好。这时候我们就得到这个结论，功率越大的器件，越靠近电源供电的话，那对整个系统工作的影响就越小。

于是我们的电路布局图就成为这个样子图3所示，大功率和小功率分布布局，大功率的地和电源尽可能从靠近电源的正负极直接引线，这样大功率器件对其他小功率器件的影响能够降到最小。

下面我的电路继续升级，加入了PWM数字开关器件，还有一些小的器件，大家有没有想过，这样玩，会不会有啥问题。。。

如果说第一条只用到了电路的欧姆定律，那这一条就要用到电磁感应原理，大家跟我的思路想哈，PWM要不停地开关，那0.5A电流就是交变的电流，交变的电流线圈会在整个线路环里产生交变的磁场，那是不是说所有包含在电源到0.5APWM器件的环路里的所有器件信号都会被这个交变磁场影响，因为交变的磁场会在其内部线路里又产生感性电动势和感应电流。这里只是0.5A，那如果10A电流也是开关器件呢，那简直瞬间干扰死那些线路里的小信号器件。于是我们就有了第二条定律，大功率器件的电源和地最好贴着走，不要包含小功率的器件。

于是图4就会进化到图5，虽然丑了点，是效果好呀！！！

所以最终结论就是如下：

• 尽可能大功率的器件靠近电源接线
• 尽可能开关功率器件尤其是大功率的开关器件，电源和地包络器件越少越好。

铭记上面两条法则，应该能应付一般的PCB布线了。

智能车电路为例，整体电路分为三部分，电机驱动，舵机，控制电路，总体布线如图6所示，三大部分彻底分开，尤其是电机驱动这块，流大电流。

电机驱动阻抗分析：

B车电机内阻150m欧，电池内阻80m欧，电源线，保险和开关20-30m欧，PCB布线电阻20m欧，整体线路内阻大概是250-300m欧，如果按照电池工作电压7-8V计算的话，那短路电流有20-30A

C车模相对好一些，电机内阻900m欧，加上电池内阻，电源线，开关，保险丝，PCB布线100-150m欧，总阻抗1欧，即使考虑到双电机并联，最大电流不超过15A左右。

2.电机H桥

聊完硬件布线，我们再聊聊直流电机，说简单点就是给它加直流电压，那它就转，比如图1的电路，一个开关K就可以控制电机转动和停止。

如果我们想让既能让电机正转，又能反转，那该怎么设计呢，如图8所示，为了简化，图中未画出续流二极管。K1和K2通的时候，电机正转，K3和K4通的时候，电机反转，这个电路结构称为H桥电路。

H桥可以让电机工作于四个状态，如下表所示。

开关状态	电机状态
K1和K2闭合	正转
K3和K4闭合	反转
K2和K4闭合	刹车状态
四个开关都断开	滑行状态

截至目前，电机的工作电压，要么是电池电压，要么电压为0，要么是反向电池电压，如果想调压调速的时候，那怎么办呢？？？
搞电力电子的兄弟们发明了一种简单方法，比如现在电池只有7V电压，那可以这样玩，加个电子开关，给电机通电5ms7V，再断开5ms，然后依次循环，只要频率足够快就没啥事，这样等效下来是不是就相当于3.5V电压呀，然后通过调节开通关断时间比例，来连续调节电压，这就叫做PWM控制。简单点说，有了PWM调节，我们的电机就可以调压调速了。

H桥调压调速具体玩法如图9所示。比如正向，下桥K2长通，然后给K1加PWM控制，K1导通的时候，电池电压加到电机上正向电流，如果图9的左边，K1关闭的时候，电池电机的电流通过K4的续流二极管进行续流，图9的右边。反转的话，依次类推即可。

电机控制的示例代码：

//初始化代码
void MotorR_Init(void)
{
   /*Motor Drive*/
   gpio_init (MOTORR_EN, GPO,0);
   FTM_PWM_init(MOTORR_FTM, MOTORR_PWMA, MOTORR_PWM_FREQ,0);
   FTM_PWM_init(MOTORR_FTM, MOTORR_PWMB, MOTORR_PWM_FREQ,0);

   /*Speed Measure*/
   FTM_QUAD_Init(MOTORR_ENCODE_FTM);
}
//PWM控制正反转
void MotorR_Run(int32 pwm)
{
   uint32 PWM_A =0;
   uint32 PWM_B =0;

   if(pwm>MOTORR_PWM_MAX)
       pwm=MOTORR_PWM_MAX;
   else if(pwm<MOTORR_PWM_MIN)
       pwm=MOTORR_PWM_MIN;

   if ( pwm >0 )
   {
      PWM_A = pwm;
   }
   else if ( pwm <0 )
   {
      PWM_B = -pwm;
   }
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMA,PWM_A);
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMB,PWM_B);
   gpio_set ( MOTORR_EN,1);
}
//刹车
void MotorR_Brake(void)
{   
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMA,0);
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMB,0);
   gpio_set  ( MOTORR_EN,1);
}
//滑行
void MotorR_Slip(void)
{
   gpio_set  (MOTORR_EN,0);
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMA,0);
   FTM_PWM_Duty(MOTORR_FTM, MOTORR_PWMB,0);
}

至于其他的硬件问题，我这里就不过多赘述，比如电源线正反要接对，稳压芯片前要加TVS管，抑制直流电机换向的尖峰脉冲电压，稳压电源的散热面积要大一些，尽量加保险丝保护，摄像头这块，我觉得山外的文档《ov7725数字摄像头编程基本知识笔记》已经解释的很清楚了，就不多赘述了。

3.机械这块简单说几句吧

首先有几点我一定要提醒大家：

• 车前的防档杆必须加，否则你的舵机一撞墙齿轮就会被挤坏
• 车底盘变形问题一定要注意
• 轮胎务必加胎水，否则轮胎一上赛道滑的要命

其实，机械这块的改造，经过这十来年基本成型了，每年智能车比赛这块，大家差不多都一个模子。但是有个点，一定要强调，就是前轮定位的机械调教。

前轮定位的作用是保障汽车直线行驶的稳定性，转向轻便和减少轮胎的磨 损。前轮是转向轮，它的安装位置由主销内倾、主销后倾、前轮外倾和前轮前 束等 4 个项目决定，反映了转向轮、主销和前轴等三者在车架上的位置关系。 下面这几个角度的介绍摘自《第十届“飞思卡尔”杯全国大学生 智能汽车竞赛-北京科技大学电磁组一队》的技术报告，感谢他们做的这么好的总结。后续我仔细研究过汽车理论之后，再为大家详细画图详细介绍这几个角度对车辆的影响。

**主销后倾角
**

所谓主销后倾，是将主销（即转向轴线）的上端略向后倾斜。从汽车的侧面看去，主销轴线与通过前轮中心的垂线之间形成一个夹角，即主销后倾角。主销后倾的作用是增加汽车直线行驶时的稳定性和在转向后使前轮自动回正。由于主销后倾，主销（即转向轴线）与地面的交点位于车轮接地点的前面。这时，车轮所受到的阻力的作用点总是在主销轴线之后，相当于主销拖着车轮前进。这样，就能保持行驶方向的稳定性。当车转弯时，由于车轮所受阻力作用线，不通过主销轴线，这样，车轮所受阻力在主销方向有力矩
作用产生，迫使车轮自动偏转直到到车轮所受阻力作用线通过主销轴线，此时，车轮已回正，这就是转向车轮的自动回正功能。

主销后倾角越大，方向稳定性越好，自动回正作用也越强，但转向越沉重。汽车主销后倾角一般不超过3°，由前悬架在车架上的安装位置来保证。现代轿车由于采用低压宽幅子午线轮胎，高速行驶时轮胎的变形加大，接地点后移，因此主销后倾角可以减小，甚至为负值（变成主销前倾），以避免由于回正力矩过大而造成前轮摆振。

模型车通过增减黄色垫片的数量来改变主销后倾角的，由于竞赛所用的转向舵机力矩不大，过大的主销后倾角会使转向变得沉重，转弯反应迟滞，所以设置为0°，以便增加其转向的灵活性。

主销内倾角

所谓主销内倾，是将主销（即转向轴线）的上端向内倾斜。从汽车的前面看去，主销轴线与通过前轮中心的垂线之间形成一个夹角，即主销内倾角。主销内倾的作用是使车轮转向后能及时自动回正和转向轻便。对于模型车，通过调整前桥的螺杆的长度可以改变主销内倾角的大小，由于过大的内倾角也会增大转向阻力，增加轮胎磨损，所以在调整时可以近似调整为0°~3°左右，不宜太大。

主销内倾和主销后倾都有使汽车转向自动回正，保持直线行驶的功能。不同之处是主销内倾的回正与车速无关，主销后倾的回正与车速有关，因此高速时主销后倾的回正作用大，低速时主销内倾的回正作用大。

**车轮外倾角
**

前轮外倾角是指通过车轮中心的汽车横向平面与车轮平面的交线与地面垂 线之间的夹角，对汽车的转向性能有直接影响，它的作用是提高前轮的转向安 全性和转向操纵的轻便性。在汽车的横向平面内，轮胎呈“八”字型时称为“负 外倾”，而呈现“V”字形张开时称为正外倾。如果车轮垂直地面一旦满载就易 产生变形，可能引起车轮上部向内倾侧，导致车轮联接件损坏。所以事先将车 轮校偏一个正外倾角度，一般这个角度约在 1°左右，以减少承载轴承负荷，增 加零件使用寿命，提高汽车的安全性能。

模型车提供了专门的外倾角调整配件，近似调节其外倾角。由于竞赛中模 型主要用于竞速，所以要求尽量减轻重量，其底盘和前桥上承受的载荷不大， 所以外倾角调整为 0°即可，并且要与前轮前束匹配。

**前轮前束
**

所谓前束是指两轮之间的后距离数值与前距离数值之差，也指前轮中心线 与纵向中心线的夹角。前轮前束的作用是保证汽车的行驶性能，减少轮胎的磨 损。前轮在滚动时，其惯性力自然将轮胎向内偏斜，如果前束适当，轮胎滚动 时的偏斜方向就会抵消，轮胎内外侧磨损的现象会减少。像内八字那样前端小 后端大的称为“前束”，反之则称为“后束”或“负前束”。在实际的汽车中， 一般前束为 012mm。

在模型车中，前轮前束是通过调整伺服电机带动的左右横拉杆实现的。主 销在垂直方向的位置确定后，改变左右横拉杆的长度即可以改变前轮前束的大 小。在实际的调整过程中，我们发现较小的前束，约束 02mm 可以减小转向阻力， 使模型车转向更为轻便，但实际效果不是十分明显。

虽然模型车的主销后倾角、主销内倾角、车轮外倾角和前束等均可以调整， 但是由于车模加工和制造精度的问题，在通用的规律中还存在着不少的偶然性， 一切是实际调整的效果为准。

在实际调试中，我们发现适当增大内倾角的确可以增大转弯时车轮和地面的接触面积，从而增大车了地面的摩擦程度，使车转向更灵活，减小因摩擦不够而引起的转向不足的情况。前轮前束为0-1度左右，直线行驶更稳定。

硬件与机械就到此了，这块自己也还需要继续学习。

---

嵌入式平台软件搭建

1.从任务调度说起

最开始我们在单片机写代码的样子是怎样的呢？在ch1那一章我们对模块和分层进行了讨论，模块是对功能代码的封装，分层是在平台层面封装，都是在解决项目复杂度控制的问题，但是我们拿单片机最主要的目的是来执行任务Task帮我们做事的，比如读取ADC采样数据，读取键盘按键，输出PWM，I2C通讯，运行PID控制，等等。

那在单片机里如何组织任务调度的设计？

大循环调度

最初的最初，我们的任务调度简单直接——也就是大循环方式，示例代码如下：

int main()
{
    Dis_Interrupt();
    System_Init();
    En_Interrupt();

    while(1)
    {
        Task0_Run();
        Task1_Run();
        Task2_Run();
        Task3_Run();
        Task4_Run();
    }
}

void Task0_Run(void)
{
    Pot1Calc(); //加速器信号计算
    Pot2Calc(); //制动器信号计算（保留）
    TempCalc(); //电机及控制器温度计算
}
.....

大循环方式的任务调度如图1所示，优点就是简单直接，适合比较简单的系统，带来的不好的地方：

• 每个任务的调度周期和时间是不固定的(if else的存在)，无法保证确定的周期性执行任务
• 随着任务数量的增加，系统会越来越慢
• 如果遇上长时间任务，会拖累整个系统变慢

定时任务调度

为了克服大循环方式的缺点（任务调度周期性无法保证，任务数量增加系统会变慢），提出了定时的任务调度的方式，不过需要使用单片机一个定时器，来实现一个简单的任务调度器，利用定时器将CPU切割为一个等周期的时间片调度单元，然后利用标志位控制在每个时间片只调用一个任务。整个系统代码结构如下所示：

#define TASK_MAX_LENGTH 10
typedef struct
{
    Int16 Flag[TASK_MAX_LENGTH];
    Int16 Timer;
    Int32 Number;
} USERTASK;
USERTASK UserTask0={0,0,0,0,0,0,0,0,0,0,0,TASK_MAX_LENGTH};//任务初始化

//任务调度函数
void TaskScheduler(USERTASK* v)
{
    v->Flag[v->Timer++] = 1;
    if(v->Timer >= v->Number)
    {
        v->Timer = 0;
    }
}
//主函数
int main()
{
    Dis_Interrupt();
    System_Init();
    En_Interrupt();

    while(1)
    {
        Task0_Run();
        Task1_Run();
        Task2_Run();
        Task3_Run();
        Task4_Run();
    }
}

//1ms定时中断
__interrupt void Timer0_INT_MapedISR(void)
{
    TaskScheduler(&UserTask0);
}

//单个任务示例函数
void Task0_Run(void)
{
    if(UserTask0.Flag[0])
    {
        Pot1Calc();                    //加速器信号计算
        Pot2Calc();                    //制动器信号计算（保留）
        TempCalc();                    //电机及控制器温度计算

        UserTask0.Flag[0] = 0;
    }
}
......

定时任务调度的流程图如图2所示。与大循环调度方式对比，这种方式能够实现周期性的任务调度，同时随着任务的增加，依然能够保证调度的周期性，这种调度能够应对大多数的控制系统，比如TI的PMSM电机控制器，一般小的家电控制器，都可以搞定。但是使用时有几点要注意：

1. 单个任务的最长时间长度务必保证不超过单个时间片，否则会导致周期性延迟
2. 对于严格实时的控制周期任务，定时调度器不能够保证
3. 对于长周期任务（比如通讯等待等），定时任务调度器要么把任务切割为小任务，要么安排几个连续的空闲周期来执行

针对第1点，需要测试或者预估任务的最长执行时间，这个可以采用IO测试的方式解决（具体参见ch6）。

针对第2点，对于实时性要求高，并且周期控制快的任务（比如PID控制），只能将这个任务放到定时中断里做，示例代码如下：

//1ms定时中断
__interrupt void Timer0_INT_MapedISR(void)
{
    TaskScheduler(&UserTask0);
    Task_SpeedPID_Control();
}

//实时性要求高的任务，示例函数，如果控周期慢的话，也可以选择加入if(UserTask0.Flag[0])做判断
void Task_SpeedPID_Control(void)
{
   SpeedPID_Input();                    //读取输入指令和反馈信号
   SpeedPID_Run();                      //运行PID
   SpeedPID_Output();                   //输出PWM控制
}
......

针对第3点，我们可以将长周期任务放在最后面，如图3所示，可以把最后几个空闲周期都留给Task4执行。但是要注意，如果有多个长周期任务，依然会拖慢整个调度周期，于是就出现了基于优先级的任务调度方式，高优先级的任务可以中断低优先级的任务，在保证长周期任务调度的同时，短周期任务的调度依然能够保证，这就是RTOS。

实时操作系统RTOS调度

实时操作系统，常用的小型RTOS有uCosII，FreeRTOS，Rt-thread，主要是任务优先级的调度方式不一样，这里感兴趣的同学，可以参见相关的专业书籍，对RTOS内核代码不做详细介绍。RTOS的对任务的调度方式如图4所示。Task0的优先级高，可以中断优先级低的Task1，等Task0执行完，然后RTOS会切换到Task1继续执行。

2.智能车总体任务调度

智能车调度平台总体上只有两个任务SpeedControlTask和ControlGraphTask，考虑到系统简单，没有用RTOS和任务调度器，直接中断配合While实现，代码示例如下，运行时序如图5所示。

//main主循环
void main(void)
{                                                               
   DisableInterrupts;  
   CarSystem_Init();
   EnableInterrupts;
   Car_Test();//主循环在这里
   while(1);                                                  
}

//主循环
void  Car_Test(void)
{ 
    while(1)
    {  
       if(ImageOver)//图像DMA传输结束
       {
           ImageOver=0;
           img_extract((uint8 *)Image_Data, (uint8 *)imgbuff0, CAMERA_SIZE);//解压图像
           ControlGraphTask();//图像处理任务

           DataLog_Add();//数据记录
           if(DataLog_CheckEN())
              DataLog_Print();
      }
    }
}

//中断
#define CAM_VSYNC 29

void PORTA_handler(void)
{
    uint32 flag = PORTA_ISFR;
    PORTA_ISFR  = ~0; 
   if(flag & (1 << CAM_VSYNC))                                 //PTA29触发摄像头帧中断
   {
       ImageOver=0;                                            //清除图像采集标志                                  
       camera_vsync();
       gVar.time++;
   }
}
//DMA传输图像数据
void DMA0_IRQHandler()
{
    camera_dma();
    ImageOver=1;
}

//定时10ms中断，执行速度PID控制任务
void PIT0_IRQHandler(void)
{
   SpeedControlTask();
   PIT_Flag_Clear(PIT0);
}

总体思路就是，每一幅图像的帧中断VSYNC触发PORTA_handler(PA29)中断函数，此时ImageOver清零，同时DMA开始传输图像，当DMA传输结束触发DMA0_IRQHandler中断，此时ImageOver=1，如果ControlGraphTask检测到的话，那就开始执行，如果ControlGraphTask在VSYNC到来清零ImageOver之前没有开始执行的话，那只能等待下一次DMA中断。最终测试结果，每两帧触发一次ControlGraphTask执行，控制周期为13.33ms。

3.嵌入式驱动层设计

嵌入式驱动层大部分复用了Vcan山外的板级库，新加入比较重要的库有EITMotorL，EITMotor_R，EIT_Steer和EIT_Log，封装在EITLib文件夹，总体的思路就是，.h负责接口，.c负责功能实现。

这里以Motor库为例，介绍一下嵌入式驱动库的封装。

考虑到速度控制用到Motor和Encode，所以把二者集成放到了一起，整体MotorR代码解析如下所示。

#ifndef __EIT_MOTORR_DEF__
#define __EIT_MOTORR_DEF__
#include "include.h"
/*Motor Driver*/
#define    MOTORR_PWM_MAX   1000                   //PWM范围：-1000到1000
#define    MOTORR_PWM_MIN   (-1000)
#define    MOTORR_PWM_FREQ  15000                  //PWM工作频率
#define    MOTORR_FTM    FTM0
#define    MOTORR_EN     PTA24
#define    MOTORR_PWMA   FTM_CH3
#define    MOTORR_PWMB   FTM_CH4
#define    MOTORR_PWMAIO PTA6
#define    MOTORR_PWMBIO PTA7


/*Encode */
#define    MOTORR_ENCODE_FTM             FTM2         //左编码器用FTM1
#define    MOTORR_GEAR_N                 36           //B车电机自带齿轮齿轮数
#define    ENCODR_GEAR_N                 40           //B车主动轴齿轮齿轮数
#define    WHEELR_GEAR_N                 105          //编码器比例系数
#define    ENCODR_CYCLE                  2000         //编码器一圈触发2000个脉冲
#define    WHEELR_LENGTH                 18           //17.8cm车轮周长

#define    SPEEDR_FS                     100          //速度采样频率Hz，周期10ms

extern void MotorR_Init(void);                        //电机初始化
extern void MotorR_Run(int32 pwm);                    //电机PWM控制
extern void MotorR_Brake(void);                       //电机刹车
extern void MotorR_Slip(void);                        //电机滑行

extern int32 MotorR_GetWheelSpeed(int32 CntInTs);     //speed单位为cm/s
extern int32 MotorR_GetTsCount(void);                 //10ms周期内，编码器脉冲计数值

#endif

---

Matlab处理和验证图像算法

1.先聊聊基于模型的设计

可以先看看知乎这篇文章《基于模型设计——电力电子的利器》

最开始我们做小的系统，自己想怎么码就怎么码，想怎么命名就怎么命名，因为系统小，不管怎么折腾复杂度都可控，但是随着代码量的增加，我们开始切分函数，然后切分模块，（参见ch1），这时候我们开始考虑模块化设计，考虑模块的封装与耦合，尽量高内聚低耦合，随着系统复杂度进一步增加，我们开始几个人协同开发，考虑分层，考虑应用层，系统层，驱动层，数据服务是不是要独立出来，通讯是不是要单独出来，是裸跑，RTOS还是Linux，接口如何划分和设计，这时候就开始系统级地去进行软件设计，实现尽可能正交的系统，减少冗余代码。随着系统进一步复杂，代码量和复杂度都在不断攀升，这时候又有什么应对措施呢？？

还有一个问题，我们始终避免不了，就是嵌入式系统的测试与验证，如果按照瀑布模型走的话，等所有代码都完成了，再进行验证测试的话，如果一旦出现问题，再返工重新设计，那不拖延项目才怪呢，所以我们尽可能的将验证测试提前，越早期发现错误，那风险也就越低。于是尝试喷泉模型，在每一个阶段都能进行验证测试，但是嵌入式系统软件和一般软件系统不太一样，因为要与实物配合才能进行测试验证。往往硬件还没出来，我们都无法进行软件的调试测试，然后整个项目就卡在这里，有没有什么好的解决方案呢？

其实，嵌入式系统里，有一部分是纯逻辑与控制算法，这是系统的核心，大量的精力应该放在这里，但是实际中我们更多的时间被硬件接口，驱动，RTOS所累，最后对控制模型反而有点心有余而力不足。那是不是有让我们更专注于模型设计与控制算法的设计方法呢，以此提高我们的工作效率，更专注于核心。

正是基于以上几点，MBD（Model Based Design-基于模型的设计）闪亮登场，于是Matlab/Simulink进入了我们的工具箱，不过这里面分三种玩法：

• 土鳖的玩法：Simulink搭建模型与算法，验证测试通过后，然后再徒手实现C代码
• 折中的玩法：利用mexfunction写C的控制算法，Simulink模型配合mex构建的控制算法，仿真验证
• 高级的玩法：Simulink搭建模型与算法，验证测试通过后，自动生成C代码（高级是高级，但是生成的代码没法看）

考虑到代码的移植以及可读性，我们采用的是折中玩法，下面我以智能车的图像处理来简单介绍一下。

最低效率的方式（比土鳖还土鳖）

徒手写图像处理的C代码下载到车里，放到赛道上跑两圈，如果好了一把成，但如果出了问题，只能抱着笔记本，连着J-Link调试，趴在那里，一点点查找程序错误，这中间哪怕一个很小的bug问题，都会消耗极大的体力和脑力，因为不受控的因素太多了。

改进版的方式（土鳖玩法）

先大量采集赛道图片，覆盖90%以上路况的情况，然后在Matlab里对这些图片进行处理，实现图像处理算法，等验证通过后，再把Matlab徒手翻译为C代码，然后上赛道测试，依然会出现部分bug，但是80%的低级的原则性的bug在Matlab验证阶段已经被消灭了。

目前要做的方式（折中玩法）

在改进版中，matlab中验证的是matlab代码，车上跑的是C代码，由于这中间存在人工转化的过程，意味着依然可能会引入未知的错误。为何不直接在matlab里直接验证我的C代码呢？对了，这就是交叉编译，在matlab里直接调用C语言，OK，这样就能保证最终车里跑的代码，是在matlab里最终验证通过的了，Yeah。下面我们就跟着老司机一起开车喽。

2.交叉编译环境搭建

Matlab 的软件版本推荐2011a（我比较钟爱老版本，哈哈，因为占地小）

C&C++编译器，推荐VS2010

第一步，先装好Matlab 2011a和VS2010（大家度娘解决吧）。

第二步，在Matlab下配置C&C++交叉编译器。

1.在matlab的工作台中输入mex–setup命令（mex就是matlab支持交叉编译的工具）

2.回车后，会输出一段话，最后是“Would you like mex to locate installed compilers[y]/n”是问你确定要为mex配置编译器？请输入y表示确定。

3.此时，会显示matlab搜索到已经安装好的VS2010，你要在Compiler：后面输入相应的选择序号1，回车.

4.在上图里，显示了你配置的编译器，最后需要输入y确认此配置，这样就OK了。

比如我们实现最简单的y=a+b这样一个加法操作。

用matlab的函数function如何做呢？看下面：

function y=add(a,b)
    y=a+b;

将上面的代码保存为add.m，这样在matlab的工作台就可以调用这个函数了，比如：

那这样一个简单的加法，我用C语言怎么实现呢？也很简单

double  add(double  a, double  b)
{
    return a+b;
}

既然我实现了C语言函数，那在matlab里怎么样调用这个C函数呢？这时候，我们前面配置好的mex交叉编译工具就上场了。

看下面这段代码，看不懂没关系，后面会一一解释的。

void mexFunction ( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    double *Y;
    double A, B;

    //输入接口绑定
    A = *(mxGetPr(prhs[0]));
    B = *(mxGetPr(prhs[1]));

    //输出接口绑定
    plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 
    Y = mxGetPr(plhs[0]);

    //做你该做的事
    *Y = add(A, B);
}

将上面的这段代码，创建为了new_add.c，然后在工作台上输入mex new_add.c命令，编译无错，就可以使用了。

我们到底做了什么？其实，你只是把这个函数需要的两个参数a和b从matlab倒腾到C语言里面，进行了相应运算之后，再把输出结果从C语言里面倒腾到Matlab里而已。

不要怕麻烦，为了不熬夜调车，为了不做码农，刚开始势必会麻烦一点，等熟练了就好了，等你的代码到2000行或者更多的时候，你依然可以有时间优哉游哉地玩耍。

其实Matlab的大侠们，已经给这个倒腾过程，建立了一个专门的接口函数叫mexFunction，这个函数有四个参数分别为：

int nlhs                  输出变量个数
mxArray *plhs[]           输出变量指针数组
int nrhs                  输入变量个数
const mxArray *prhs[]     输入变量的指针数组

比如看我们上面的例子，当我调用new_add(3,4)时，

int     nlhs              输出变量个数为1
mxArray *plhs[]           输出变量指针数组，plhs[0]对应求和结果y的变量地址
int     nrhs              输入变量个数为2
const  mxArray *prhs[]    输入变量的指针数组有两个，prhs[0]为参数a=3对应的地址，prhs[1]为参数b=4对应的地址。

//接下来就是把输入的两个参数读取到C变量里暂存，mxGetPr是获取数组地址，*就是获取地址里的内容。
//输入接口绑定
A = *(mxGetPr(prhs[0]));
B = *(mxGetPr(prhs[1]));

//再下面是输出接口的绑定，输出未分配存储空间，所以必须先申请存储空间，用mxCreateDoubleMatrix，然后用C指针变量指向这个地址。
//输出接口绑定
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 
Y = mxGetPr(plhs[0]);

//然后就是你要做的操作，调用add，实现加操作
*Y = add(A, B);

有几点注意事项：

• matlab默认数据类型为double，如果你直接调用的接口数据的话，你的C语言声明的类型必须与之对应，否则Matlab会挂掉。

• 像我们图像一般都是0-255单字节的灰度或者二值化图，那应该怎么办呢？你要在matlab下将二维数组转化为uint8类型，然后再传给mexFunction接口，同时C语言中，一定要用uint8*类型的指针去操作。

• Matlab中矩阵的排列与C语言中的排列不太一样，这一点也要注意，matlab的存储是按照列来顺序存储，不是C语言中的按照行顺序存储，如图6所示。

• C的数组起始是0，matlab的话是1，这点一定要注意。

• 交叉编译不好的地方，就是不能进行单步调试，那出了问题，怎么办呢？用mexPrintf函数，与你用C里面的printf一样。

这里附上源代码

#include "mex.h"

double add(double a,double b)
{
    return (a+b);
}

void mexFunction ( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    double *Y;
    double A, B;

    A = *(mxGetPr(prhs[0]));
    B = *(mxGetPr(prhs[1]));


    plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 
    Y = mxGetPr(plhs[0]);

    *Y = add(A, B);
}

3.智能车比赛图像处理

图像处理模块的结构图如图7所示，其中图像处理部分主要是imProc完成，与Matlab的mex接口由imCar完成。

整个图像处理模块imProc的内部结构如图8所示，输入信号为图像数据Image_Data（用于寻找中线），Speed和寻找到的中线接合起来用于计算方向偏差，之所以要跟速度相关，因为车速快了的话，需要用更远的图像信息去计算方向偏差，加大提前量，近了则用更近的图像数据去计算，最终计算的中线偏差有三个值：

• gDir_Near：近距离方向偏差，暂时未使用
• gDir_Mid：中距离方向偏差，主要用于转向PD控制
• gDir_Far：远距离方向偏差，主要用于识别入弯和出弯，提前进行加减速控制

整个代码实现了一个基本的寻线处理和计算中线偏差的思路，具体过程如下：

1. 判断有没有出界，如果出界，则不做处理，保持原来的方向偏差不变，否则开始寻找新的中线
2. 逐行扫，先寻找中间位置，然后向左右寻找左右边界
3. 根据左右边界计算左右边界的斜率变化，然后递推得到最终的左右边界值
4. 根据左右边界，计算中线
5. 将中线做均值滤波
6. 滤波后的中线，映射到实际的物理坐标上（单位为cm）
7. 根据速度，计算三个中线偏差值

imProc的几个函数的功能：

• int Graph_JudgeOut(void)：判断是否出界

• void Graph_FindMidLine(void)：寻找中线

• void Graph_AverageMBound(void)：均值滤波函数

• void Graph_Cam2Real_BoundM(void)：将中线映射到真是物理坐标

• int Graph_Real2Cam(int D)：将真实距离映射到图像位置

• int Graph_Cam2Real(int H)：将图像位置映射到真实距离

• void Graph_Calculate_Dir(int Speed)：计算方向偏差

图像处理好之后，下一步就是实现Matlab结合C编程的接口imCar，代码如下，

#include "mex.h"
#include "imProc.h"
#include "imType.h"

imUINT8  Image_Data[CAMERA_H][CAMERA_W];
extern imUINT8  Image_DataF[CAMERA_H][CAMERA_W];
extern imINT32  gDir_Near;
extern imINT32  gDir_Mid;
extern imINT32  gDir_Far;
extern imINT16  HBoundL[CAMERA_H];
extern imINT16  HBoundR[CAMERA_H];
extern imINT16  HBoundM[CAMERA_H];
extern imINT16  HBoundM_F[CAMERA_H];
extern imINT16  HBoundM_REAL[CAM_MAX_LENGTH_CM+1];


void mexFunction ( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    imUINT8 *imIn;
    imUINT8 *imOut;
    int H,W;
    imINT16 *bound;
    imINT32 *dir;
    imINT32 CarSpeed;

    imIn=mxGetPr(prhs[0]);
    for(H=0;H<CAMERA_H;H++)
    {    
        for(W=0;W<CAMERA_W;W++)
        {
        Image_Data[H][W]=imIn[H*CAMERA_W+W];
            //mexPrintf("%d ",Image_Data[H][W]);
        }
    }
    CarSpeed = *mxGetPr(prhs[1]);
    ControlParam_Init();
    Graph_FindMidLine();
    Graph_Calculate_Dir(CarSpeed);
    plhs[0]=mxCreateNumericMatrix(CAMERA_H,1,mxINT16_CLASS,mxREAL);
    plhs[1]=mxCreateNumericMatrix(CAMERA_H,1,mxINT16_CLASS,mxREAL);
    plhs[2]=mxCreateNumericMatrix(CAMERA_H,1,mxINT16_CLASS,mxREAL);
    plhs[3]=mxCreateNumericMatrix(3,1,mxINT32_CLASS,mxREAL);
    plhs[4]=mxCreateNumericMatrix(CAMERA_W,CAMERA_H,mxUINT8_CLASS,mxREAL);
    plhs[5]=mxCreateNumericMatrix(CAMERA_H,1,mxINT16_CLASS,mxREAL);
    plhs[6]=mxCreateNumericMatrix(CAM_MAX_LENGTH_CM+1,1,mxINT16_CLASS,mxREAL);

    bound=mxGetPr(plhs[0]);
    for(H=0;H<CAMERA_H;H++)
    {    
        bound[H]=HBoundL[H];
    }
    bound=mxGetPr(plhs[1]);
    for(H=0;H<CAMERA_H;H++)
    {    
        bound[H]=HBoundR[H];
    }
    bound=mxGetPr(plhs[2]);
    for(H=0;H<CAMERA_H;H++)
    {    
        bound[H]=HBoundM_F[H];
    }
    dir=mxGetPr(plhs[3]);
    dir[0]=gDir_Near;
    dir[1]=gDir_Mid;
    dir[2]=gDir_Far;
    imOut=mxGetPr(plhs[4]);
    for(H=0;H<CAMERA_H;H++)
    {
       for(W=0;W<CAMERA_W;W++)
       {
           imOut[H*CAMERA_W+W]=Image_Data[H][W];
       }
    }

    bound=mxGetPr(plhs[5]);
    for(H=0;H<CAMERA_H;H++)
    {    
        bound[H]=HBoundM_F[H];
    }
    bound=mxGetPr(plhs[6]);
    for(H=0;H<CAM_MAX_LENGTH_CM;H++)
    {    
        bound[H]=HBoundM_REAL[H];
    }
}

matlab代码调用接口函数的代码如下：

clc;
clear mex
mex -I"../ControlLib/Inc" ...,
    imCar.c ...,
    imProc.c ...,
    imCom.c ...,
    ../ControlLib/ControlParam.c
CarSpeed=200;%单位为cm/s
CAMERA_W=160;
CAMERA_H=120;
for i=1:127
    try
        imfilename=strcat('.\Image_txt\Imag',int2str(i),'.txt');%输入图片
        svfilename=strcat('.\Image_txt\solve\Imag',int2str(i),'.bmp');%输出图片
        %img=uint8(not(imread(imfilename))*255)';%加载BMP格式图片
        img=uint8(load(imfilename))';      %加载txt文本格式图片
        [W H]=size(img);
        if W ~=CAMERA_W && H~= CAMERA_H
            continue
        end
        [L R M  dir imOut M_F M_Real]=imCar(img,CarSpeed);
        imshow(imOut) 
        hold on
        plot(1:1:CAMERA_H,[L R M],'-r')
        saveas(gcf,svfilename)
        close all
        clear mex
    catch e
        e
        continue
    end
end

imCar一共返回7个变量，分别代表的含义是：

• L：左边界
• M：中线
• R：右边界
• dir：[gDir_Near gDir_Mid gDir_Far]
• imOut:输入图像重新返回
• M_F：中线滤波后的结果
• M_Real：中线滤波后，映射到实际距离

这里要说一下，由于imProc用到了ControParam模块的配置参数，来实现方向偏差的计算，所以mex的命令如下：

mex -I"../ControlLib/Inc" ...,%包含ControlParam.h
    imCar.c ...,
    imProc.c ...,
    imCom.c ...,
    ../ControlLib/ControlParam.c%编译ControlParam.c文件

将matlab的工作目录设置为Graphic，然后运行Compile.m，默认选择的是txt文本图像（Image_txt文件夹），1分钟之后，127张图像就全部处理结束啦（在Image_txt下的solve文件下），速度是不是很快呀，哈哈哈，如图10所示。然后你就可以针对不同的路况，去优化算法，立刻就可以在Matlab上验证，知道所有的路况全部验证通过之后，再把代码烧到单片机里，进行真实赛道测试。

这里要提一下，Graphic下目前有两个保存图像的文件夹，分别为Image__txt_和Image_bmp_，_Image_Txt是我们用串口再赛道上每隔10cm采集的部分图像，Image_bmp是山外自带的bmp格式图像。大家可以根据自己情况自由选择图片格式。

如果要处理Image_bmp文件夹下的图像请将compile文件修改为如下：

clc;
clear mex
mex -I"../ControlLib/Inc" ...,
    imCar.c ...,
    imProc.c ...,
    imCom.c ...,
    ../ControlLib/ControlParam.c
CarSpeed=200;%单位为cm/s
CAMERA_W=160;
CAMERA_H=120;
for i=1:1000
    try
        imfilename=strcat('.\Image_bmp\fire',int2str(i),'.bmp');%输入图片
        svfilename=strcat('.\Image_bmp\solve\fire',int2str(i),'.bmp');%输出图片
        img=uint8(not(imread(imfilename))*255)';%加载BMP格式图片
        %img=uint8(load(imfilename))';      %加载txt文本格式图片
        [W H]=size(img);
        if W ~=CAMERA_W && H~= CAMERA_H
            continue
        end
        [L R M  dir imOut M_F M_Real]=imCar(img,CarSpeed);
        imshow(imOut) 
        hold on
        plot(1:1:CAMERA_H,[L R M],'-r')
        saveas(gcf,svfilename)
        close all
        clear mex
    catch e
        e
        continue
    end
end

这一小节，中间略掉了很多细节但是行文依然比较长，希望能够帮助到大家。

相关代码已经上传到Github，如果对你有帮助的话，可以点个赞支持一下在下。

---

转速和转向控制器设计

1.转速PID控制

关于PID控制器的解释可以看看知乎的这篇回答https://www.zhihu.com/question/23088613

PID控制器应该怎么设计，各种玩家各种玩法，

• 土鳖玩法：不停地试凑PID参数，改一次，烧一次程序，然后实际测试，跟着感觉走，老铁
• 折中玩法：先搞到系统模型，然后Simulink搭建仿真环境，在仿真里试凑，试得差不多了，再放到实际环境进行真实测试
• 高级玩法：硬件在环或者直接MBD设计（基于模型的设计，频域和时域都有不错的玩法）

下面就用智能车的转速PID控制器举例，来跟大家说一下PID到底怎么玩，这里采用的是折中玩法，首先是测得被控对象的模型，被控对象输入控制量是PWM，输出是车速，那系统模型就是一个PWM占空比与到车速之间的关系，如果要推公式的话，那电压电流，转矩，摩擦系数，叽里呱啦一大堆，有没有什么简单易行的方法呢？？废话，当然有呀，我们要得到系统的模型，无非是想知道给这个系统输入什么，它会输出什么反应。我们可以给系统加不同的激励输入，然后测输出反应，根据输入输出反应，就能反推出系统模型呀。

这里我们车的加速和减速性能，所以我们选择加阶跃输入，就是突然给车加一个电压，看车速怎么变化。具体玩法：

1. 做一条长约10-20m的长直赛道，土豪可以再长点
2. 智能车方向控制要有，保证车沿长直赛道行驶
3. 代码设定PWM占空比25%，也就是250（不要太大或者太小），固定不变开环控制，不加入任何速度控制
4. 系统上电，车开始加速行驶，直到速度稳定
5. 从系统上电开始，每隔一个时间在Log里记录一下当前速度（可以选定10ms间隔）
6. 全部跑完之后，将Log记录的数据导出到电脑里，matlab开始画图建模

这里就用到了测试Log模块，会在ch6会详细解释。由于轮胎表面处理对摩擦系数影响比较大，建议测试前适当处理，尽量模拟真实赛况下的轮胎。

测试结束后，我们会在Matlab中画出这样一张车速随时间变化，如图1所示，最后凹下去一大坑又飚起来，是因为车走到终点被抓住速度降了，拿起来空转速度又飚起来了。

根据这张阶跃响应测试图，我们就可以用1阶或者2阶模型去做建模，传递函数形式：

在这里选的二阶模型建模，Wn=1.5rad/s，zeta=1.6，最终拟合出来的系统模型是：

在Simulink搭建模型，同样加阶跃响应，可以测试得到实测图与仿真模型的对比结果，如图2所示。

下一步就是搭建PID控制模块，我们直接来上Simulink仿真模型图，如图3所示，PI控制器的控制效果图如图4所示。

• Test Motor B Car Data：实地测试的B车车速数据
• Model Motor B Car Data：仿真建模的模型阶跃输出
• PI Control Data：PI控制器的输出
• Set PWM：测试设定PWM值（量程-1000至1000）
• Set Speed：设定速度数据（单位为cm/s）

这里要简单说一下，在反馈回路加了三个部件，一个是Delay环节，因为我们10ms测一次速度，延时一半5ms，RateTransition ZOH是采样率转换，因为前后两级采样率不一致，必须加一个零阶保持器，FIR Filter是均值滤波器，4阶，把车速的高频抖动滤除掉再进控制器。

下面重点介绍一下PI Controller，之所以没有加D微分，因为实测速度抖动太厉害，再加微分不抖死呀，目前PI用着就不错。PI的控制模型用的是：

离散化后的差分方程（采用欧拉前向差分）是：

对应到Simulink的PI Controller模块设置，下面图5中的几处设置，务必要注意：

• Controller：选择PI
• Form：选择Parallel，并型模式
• Time domain:Discrete-time离散时间域，因为我们是要仿真10ms控制一次
• Integrator method：积分的差分方法，前向欧拉
• Sample time：采样时间Ts=10ms
• Proportional(P)：比例系数=4
• Integral(I)：积分系数=2.5
• Compensator formula：模块公式与我们上面的差分公式一模一样，Kp=P，Ki=I

下面我们就看看代码吧：

//EIT_PID.h接口文件
typedef struct _PID
{
    /*In*/
    int32   spVal;
    int32   spValRamp;
    int32   spUpRate;
    int32   spDnRate;
    int32   fbValFilterLast;
    int32   fbValFilter;
    int32   fbValFilterDiff;
    int32   fbVal_k0;
    int32   fbVal_k1;
    int32   fbVal_k2;
    int32   fbVal_k3;
    /*Out*/
    int32   outVal;
    /*Var*/
    int32   err;
    int32   P;
    float   I;
    int32   D;

    /*Param*/
    int32   MAX_Val;
    int32   MIN_Val;

    float   Kp;
    float   Ki;
    float   Kd;
}PID;
typedef  PID*   PID_t;
extern void   PID_InitFbVal(PID_t tPID,int32 fbVal);
extern void   PID_SetFbVal(PID_t tPID,int32 fbVal);
extern void   PID_Run_STD(PID_t tPID);
extern void   PID_Run_PID(PID_t tPID);


//EIT_PID.c模块代码文件
void   PID_SetFbVal(PID_t tPID,int32 fbVal)
{
        tPID->fbVal_k3 =tPID->fbVal_k2;
        tPID->fbVal_k2 =tPID->fbVal_k1;
        tPID->fbVal_k1 =tPID->fbVal_k0;
        tPID->fbVal_k0 =fbVal;
        tPID->fbValFilterLast=tPID->fbValFilter;
        tPID->fbValFilter    =(fbVal+tPID->fbVal_k1+tPID->fbVal_k2+tPID->fbVal_k3)/4;//FIR滤波器
        tPID->fbValFilterDiff=tPID->fbValFilter-tPID->fbValFilterLast;
}

//采用只对反馈值进行微分的PID控制器，本文采用的这种方法，将Kd设置为0，去掉微分
void  PID_Run_PID(PID_t tPID)
{
    int32 err;
    //指令加了Ramp平滑处理
    if(tPID->spVal-tPID->spValRamp > tPID->spUpRate)
        tPID->spValRamp+= tPID->spUpRate;
    if(tPID->spVal-tPID->spValRamp < tPID->spDnRate)
        tPID->spValRamp+= tPID->spDnRate;

    //计算error偏差
    err=tPID->spValRamp-tPID->fbValFilter;
    tPID->err = err; 

    tPID->P =   (int32)(tPID->Kp*err);//比例计算

    tPID->D =  (int32)(tPID->Kd*tPID->fbValFilterDiff);//微分计算

    tPID->outVal = tPID->P + (int32)(tPID->I)+tPID->D;//控制量计算
    tPID->outVal = PID_MaxMin(tPID,tPID->outVal);

    tPID->I =   (int32)(tPID->I  +  tPID->Ki*err);    //前向差分计算积分
    tPID->I =  PID_MaxMinFloat(tPID,tPID->I);  
}
//标准PID控制器
void  PID_Run_STD(PID_t tPID)
{
    int32 err;
    if(tPID->spVal-tPID->spValRamp > tPID->spUpRate)
        tPID->spValRamp+= tPID->spUpRate;
    if(tPID->spVal-tPID->spValRamp < tPID->spDnRate)
        tPID->spValRamp+= tPID->spDnRate;

    err=tPID->spValRamp-tPID->fbValFilter;
    tPID->err = err;

    tPID->P =  (int32)(tPID->Kp*err);

    tPID->D =  (int32)(tPID->Kd*(tPID->fbVal_k0-tPID->fbVal_k1));

    tPID->outVal = (int32)(tPID->P + tPID->I+tPID->D);
    tPID->outVal = PID_MaxMin(tPID,tPID->outVal);

    tPID->I =  (int32)(tPID->I  +  tPID->Ki*err);
    tPID->I =  PID_MaxMinFloat(tPID,tPID->I);
}


Controlparam设置
/*B car just one Motor-Right Motor*/    
gParam.MotorR_PID_KP=4.0;    
gParam.MotorR_PID_KI=2.5;        
gParam.MotorR_PID_KD=0.0;          
gParam.MotorR_PID_Ts=MOTOR_PID_TS; /*Unit: s   */
gParam.MOtroR_PID_UpRate = 1000;/*指令最大m/s^2*/
gParam.MOtroR_PID_DnRate = -2000;/*指令最大m/s^2*/

整个速度控制的Simulink模型和C代码已经上传到github。

2.转向PD控制器

没有用什么高大上的算法，就是用最基本的，好使够用。之前在ch4节中，我们通过对赛道图像处理得到了3个gDir的偏差值，分别为gDir_Near，_gDir_Mid和gDir_Far，大概含义如图6所示。分别选择不同远近区域的中线偏差做平均得到。

• gDir_Far：用于识别入弯和出弯，因为Far距离远，可以入弯提前减速
• gDir_Mid：用于方向PD跟踪控制
• gDir_Near：暂时未使用

整体控制，就将所有赛道路况就分为2种，一种就是直道，另一种就是弯道，根据gDir_Far以及它的变化率进行识别，具体代码如下：

//gDir的滤波处理，这个必须做，因为图像识别算法没处理好的话，很容易出现突变，再一微分，那分分钟搞死
void gDir_Filter(void)
{
   static int MidDir[5];
   static int FarDir[15];
   //gDir_Mid滤波
   MidDir[4]=MidDir[3];
   MidDir[3]=MidDir[2];
   MidDir[2]=MidDir[1];
   MidDir[1]=MidDir[0];
   MidDir[0]=gDir_Mid;

   gDir_MidFilterLast=gDir_MidFilter;
   gDir_MidFilter=(MidDir[0]+MidDir[1]+MidDir[2]+MidDir[3]+MidDir[4])/5;
   gDir_MidFilterDiff=gDir_MidFilter-gDir_MidFilterLast;

   //gDir_Far滤波
   FarDir[9]=FarDir[8];
   FarDir[8]=FarDir[7];
   FarDir[7]=FarDir[6];
   FarDir[6]=FarDir[5];
   FarDir[5]=FarDir[4];
   FarDir[4]=FarDir[3];
   FarDir[3]=FarDir[2];
   FarDir[2]=FarDir[1];
   FarDir[1]=FarDir[0];
   FarDir[0]=gDir_Far;

   //普通滤波5次加权
   gDir_FarFilterLast=gDir_FarFilter;
   gDir_FarFilter=(FarDir[0]+FarDir[1]+FarDir[2]+FarDir[3]+FarDir[4])/5;
   gDir_FarFilterDiff=gDir_FarFilter-gDir_FarFilterLast;

   //慢速滤波5次加权，更慢也更平滑
   gDir_FarFilterSlowLast=gDir_FarFilterSlow;
   gDir_FarFilterSlow=gDir_FarFilter/2+(FarDir[9]+FarDir[8]+FarDir[7]+FarDir[6]+FarDir[5])/10;
   gDir_FarFilterSlowDiff=gDir_FarFilterSlow-gDir_FarFilterSlowLast;

   //入弯和出弯识别
   switch(gVar.InAngle)
   {
       case 0:
          //长直道，如果gDir_far大于某正阀值，并且还在增加，那就是右入弯
          if(gDir_FarFilterDiff>0 && gDir_FarFilter>gParam.InAngle_FarDir )
             gVar.InAngle=1;

          //长直道，如果gDir_far小于某负阀值，并且还在减小，那就是左入弯
          if(gDir_FarFilterDiff<0 && gDir_FarFilter<-gParam.InAngle_FarDir)
             gVar.InAngle=1;
          if(gVar.InAngle == 1)
             MotorR_PID.I = MotorR_PID.I/3;
       break;
       case 1:
           //出弯，可以根据入弯类推
          if(gDir_FarFilterDiff<0 && gDir_FarFilter<gParam.OutAngle_FarDir && gDir_FarFilter>0)
             gVar.InAngle=0;
          if(gDir_FarFilterDiff>0 && gDir_FarFilter>-gParam.OutAngle_FarDir && gDir_FarFilter<0)
             gVar.InAngle=0;
          if(gVar.InAngle == 0)
             MotorR_PID.I = MotorR_PID.I*3;
     break;
   }

}

根据gDir_Far识别出直道和弯道的标志位InAngle，然后根据这个标志来确定车速和方向PD控制参数：

• 车速指令：直道一个速度，弯道一个速度，就两个速度设置参数，简单直接有效
• 转向控制：直道一套PD控制参数，弯道一套PD控制参数，一样简单直接

首先我们看车速指令代码，就两个参数设置gParam.MinSpeed和gParam.MaxSpeed：

void GetSetPointMaxSpeed(void)
{
    int MidSpeed;

    if( gVar.InAngle)
    {
        MidSpeed = gParam.MinSpeed;//是不是太简单直接了
    }
    else
    {
        MidSpeed =gParam.MaxSpeed;

    }
    spSpeedL = CarSpeed2LSpeed(MidSpeed,angle);//考虑到转弯半径问题，左右轮速度和车速必须折算一下
    spSpeedR = CarSpeed2RSpeed(MidSpeed,angle);

    //MotorLPID_SetSpeed(spSpeedL);
    MotorRPID_SetSpeed(spSpeedR);
}

然后我们转向控制代码，死区控制必须加，否则长直道容易抖，具体大小要实验测试，直道和弯道两套控制参数很有必要，弯道Kp大点更有助于转弯，直道Kp小点，行驶会更平滑：

void SteerDirControl(void)
{   
    int MidDir;

    MidDir=gDir_Mid;
    //必须加入死区，大大减少直道抖动
    if(int_abs(MidDir)>=gParam.DIR_Dead)
    {
        if(MidDir>0)
           MidDir-=gParam.DIR_Dead;
        else
           MidDir+=gParam.DIR_Dead;
    }
    else
    {
        MidDir=0;
    }
    //直道和弯道两套控制参数，其中微分参数一致，比例参数是两个参数设置值
    if(gVar.InAngle)
       angle =(int32)((float)(MidDir)*gParam.DIR_KpInAngle+ (float)(gDir_MidFilterDiff)*gParam.DIR_Kd);
    else
       angle =(int32)((float)(MidDir)*gParam.DIR_Kp+ (float)(gDir_MidFilterDiff)*gParam.DIR_Kd);

    //角度限幅操作，防止舵机转角过大，转弯卡死
    if (angle >gParam.AngleMax)
       angle =gParam.AngleMax;
    else if (angle <-gParam.AngleMax)
       angle =-gParam.AngleMax;

    //角度变化率限幅操作，指令必须能够有效执行，不能乱下指令
    angle=int_delta_Limit(angle,angleLast, gParam.AngleDeltaMax);
    angleLast=angle;

    //输出给舵机
    Steer_Run(gParam.SteerMid,angle*gParam.SteerDeltaMax/gParam.AngleMax);//新车需要加负号
}

整体的控制思路如图7所示，没有什么过于复杂的地方，都是简单不能再简单的最小系统实现，基本的PID控制配合滤波器，简单的找中线处理，配合上Matlab/Simulink后，工作效率会大大提高。

国内的技术环境，重开发，轻测试，其实吧，两者都重要，而且我个人是不认同将开发与测试解耦开来，全周期交付最终的产品是所有工程师的职责，不是开发好代码就结束了，测试用例只有最懂这个系统的人设计才最好的，所以开发测试本就是一家。

下面聊聊，嵌入式中的测试，这个跟软件系统最大的不同，就是需要软硬件协同，比如马达控制器，比如锂电池均衡，等等，尤其是涉及到闭环控制的场合，必须要保证测试占用的CPU资源不能影响正常的控制任务执行周期，也就是说，测试消耗的系统资源越少越好。好的测试手段，能够帮助提高整个系统的开发效率。

下面就介绍目前我用到比较便利的三种嵌入式辅助软件测试手段：

• IO测试：直接用IO口表示状态，需要用到示波器观察IO电平变化，测试非常快，但是不足就是信息量有限
• 内存缓冲：内存中开二维数组用于存放测试数据，每个控制周期将测试变量保存到数组，测试结束后串口输出，不足就是受内存限制
• 串口打印：串口实时printf打印输出测试数据，测试速度会受串口速度限制，速度虽然慢一些，但是可以记录大量信息，数据量不受限制

---

系统测试与分析

1.IO测试

比如ch3中，我们要估算每个Task的时间长度，用来去平衡调度周期优化，那就可以用这种IO测试方法来玩，简单直接，不妨来看。

假设我们测Task0的执行时间，首先选一个备用IO口比如PORTA0，在测试前务必将IO口输出模式配置好，然后只要在Task0调用前，将PORTA0拉高，结束后，再将PORTA0拉低，然后上电后，将示波器探头测试PORTA0这个口，查看示波器的波形图，其中高电平的持续时间，就是Task0的执行时间，这个信号的周期时间，就是我们While循环的调度周期。

这种IO测试方法需要示波器配合，简单有效，而且IO翻转的时间一般与主频一致，都在10-100ns级别，对于一些简单的要求精确测量时间的场合，这个妥妥地够用了。

示例代码如下：

int main()
{
    Dis_Interrupt();
    System_Init();
    En_Interrupt();

    while(1)
    {
        //IO测试辅助代码
        PORTA0=1;//先把IO拉高
        Task0_Run();
        PORTA0=0;//任务结束后再把IO拉低
        Task1_Run();
        Task2_Run();
        Task3_Run();
        Task4_Run();
    }
}

void Task0_Run(void)
{
    Pot1Calc(); //加速器信号计算
    Pot2Calc(); //制动器信号计算（保留）
    TempCalc(); //电机及控制器温度计算
}
.....

2.内存缓冲

IO测试可以做简单快速的时间测量，然而我们软件逻辑中，存在大量的控制变量，系统一旦出现问题，我们需要排查整个控制链条上哪个环节的控制变量逻辑或者计算出问题了，这个时候IO测试就玩不转了，因为涉及到软件变量实时记录，尤其是在做电机等闭环控制的场合，还要求测试不能影响正常的闭环控制，也就是说，测试代码最好不要占用CPU时间。其实如果玩MBD-基于模型的设计，也是想把大部分控制上的逻辑或者计算错误直接消灭掉设计阶段。下面就简单介绍这个测试手段的玩法，因为智能车里面的主要测试手段就是这个玩法。

首先我们开辟一个二维数组DataLog，数组的大小要考虑到内存大小（这种方法适合内存比较大的场合），需要记录数据的时候，执行一下DataLog_Add函数即可，在DataLog_Add里我们添加好需要记录的变量个数和具体变量，DataLog_Add的执行时间可以控制到1us左右，等所有二维数组数据填满了之后，我们就可以通过调用DataLog_Print把测试的所有数据串口输出出来。

#include  "EIT_Log.h"             //header用户应用程序

static int32 DataLog[LOG_COUNT][LOG_NUM_EACH];
static volatile int32 cnt=0;
void DataLog_Init(void)
{
   cnt=0;
}
//记录单条测试数据
void DataLog_Add(void)
{
   if(cnt>=LOG_COUNT)
     return;
   else
   {
       DataLog[cnt][0]=MotorR_PID.fbValFilter;
       DataLog[cnt][1]=MotorR_PID.spVal;
       DataLog[cnt][2]=MotorR_PID.spValRamp;
       DataLog[cnt][3]=MotorR_PID.I;
       DataLog[cnt][4]=MotorR_PID.outVal;
       DataLog[cnt][5]=gVar.InAngle*100;
       DataLog[cnt][6]=gDir_FarFilter;
       DataLog[cnt][7]=gDir_MidFilter;
       DataLog[cnt][8]=angle;
       cnt++;
   }
}
//技术后，串口打印输出
void DataLog_Print(void)
{
    int i,j;

    for(i=0;i<LOG_COUNT;i++)
    {
        for(j=0;j<LOG_NUM_EACH;j++)
            printf("%d ",DataLog[i][j]);
        printf("\n");
    }
}

串口输出数据格式的时候要注意一点，最好是一行代表一条数据记录，用空格隔开，像下面这样的格式，好处是什么呢，就是这样的数据直接保存为txt，matlab一拖动就可以成为二维数组，直接就可以plot画图看测试效果了，比如图1所示，整个加减速过程，都可以进行分析。

309 180 5 5 5 1
311 178 5 5 5 1
313 181 5 5 4 1
315 177 4 4 4 1
317 185 4 4 4 1
319 184 4 4 4 1
321 186 4 4 4 1
323 176 3 4 3 1
325 181 3 3 3 0
327 185 4 3 3 0
329 183 4 4 4 1
331 186 4 5 4 1
333 176 4 4 4 1
335 187 3 3 3 0
337 188 3 3 2 0
339 193 4 3 4 0
341 199 3 3 3 0
343 178 2 1 2 0

3.串口打印

有时候，我们的单片机内存有限，又想分析控制变量，那怎么办，能有啥办法，实时串口打印呗。不过这里要注意，有的串口支持DMA方式，也就是说串口传输不占用CPU资源，这方式比较爽。对于没有DMA方式单片机，只能使用Printf同步打印数据，这里要注意，printf是阻塞的，也就是说在打印数据的时候，CPU啥事都不能干，所以在使用这种玩法的时候，一定要预估一下打印数据占用CPU资源的比例，不要搞得重要任务卡死，一直在打印数据就好。测试的时候，大家尽量是串口波特率设高点。

这种玩法和第二种缓冲的玩法，后面数据处理其实差不多，唯一不足就是串口打印数据会占用CPU资源，这里务必要权衡好。我得建议是，想简单直接一点，就直接将标准C库的printf函数重定向到对应的串口就可以，然后直接调用printf实时打印测试数据，其实也挺简单的。

/*!Vcan山外的代码MK60_conf.c
 *  @brief      重定义printf 到串口
 *  @param      ch      需要打印的字节
 *  @param      stream  数据流
 *  @since      v5.0
 *  @note       此函数由编译器自带库里的printf所调用
 */
int fputc(int ch, FILE *stream)
{
    uart_putchar(VCAN_PORT, (char)ch);
    return(ch);
}

这三种嵌入式辅助测试手段，足够应付山寨还是正规的大部分测试和故障分析玩法了。