基于仿人智能控制算法的智能车转向舵机控制.doc

目 录
第一章 引言---------------------------------------------- 1
第二章 系统方案论证与选择-------------------------------- 2
2.1 路面黑线检测模块--------------------------------------------------- 2
2.2 电机驱动模块------------------------------------------------------- 2
2.3 测速及距离检测模块------------------------------------------------- 3
2.4 电源模块----------------------------------------------------------- 4
第三章 系统的硬件设计-------------------------------------------------5
3.1 红外检测电路设计--------------------------------------------------- 5
3.2 电机驱动电路设计--------------------------------------------------- 5
3.3 转速检测电路设计 -------------------------------------------------- 6
3.4 电源电路设计 ------------------------------------------------------ 7
3.5 加速度检测电路设计-------------------------------------------------7
3.6 辅助电路设计-------------------------------------------------------7
第四章 控制规则分析-------------------------------------- 8
4.1 一般寻迹的控制规则-------------------------------------------------8
4.2 特殊寻迹的控制规则-------------------------------------------------9
第五章 系统的软件设计------------------------------------------------11
5.1 主程序流程图------------------------------------------------------11
5.2 中断程序流程图----------------------------------------------------11
5.3 电机调速程序流程图------------------------------------------------12
5.4 保护电路程序流程图------------------------------------------------12
5.5 出线处理程序流程图------------------------------------------------12
5.6 交叉处理程序流程图------------------------------------------------13
第六章 系统设计说明 -------------------------------------------------14
6.1 开发工具----------------------------------------------------------14
6.2 制作、安装--------------------------------------------------------14
6.3 调试过程说明------------------------------------------------------14
6.3.1 测速电机调试--------------------------------------------------15
6.3.2 舵机调试------------------------------------------------------15
6.3.3 按键调试------------------------------------------------------15
第七章 车模主要技术参数说明------------------------------16
7.1 车模基本参数------------------------------------------------------16
7.2 传感器及电机个数--------------------------------------------------16
7.3 电路参数----------------------------------------------------------16
7.4 赛道信息----------------------------------------------------------16
第八章 结论----------------------------------------------17
参考文献
附录A：系统硬件原理图
附录B：主程序源代码
附录C：中断服务程序源代码
附录D：研究论文

第一章 引言
本设计采用飞思卡尔16位微控制器MC9S12DG128作为核心控制单元。赛道黑线检测使用反射式红外传感器，车速采用闭环PI控制，速度传感器使用测速电机，利用PWM实现动态电机调速。测距应用简单而精确的光电码盘。电机驱动采用H桥半桥控制。基于这些可靠的硬件设计，开发了一套完备的软件设计，包括电机PI控制算法，舵机仿人智能控制算法，低电压报警设计等。
本设计主要特色：
● 模块化设计
● 连续的电机调速，精确可靠；
● 优化的软件算法，智能化的自动控制，定位精确；
● 主板设计兼顾多种用途方案，复用性强；
● 车模重心适当，整车设计美观、合理；
● 电源指示灯、低压指示、报警，电源工作状态一目了然；

第二章 系统方案论证与选择
整个系统可以划分为以下几个基本模块，如图2.1所示：

MCU
路面检测
低电压检测
LED显示
转速检测
电机驱动
舵机调节
键盘
低压报警

图2.1 系统整体框图
2.1 路面黑线检测模块
通常用以检测黑线的传感器有CCD和红外两种。
方案一：使用CCD传感器探测赛道。原理是通过CCD传感器将赛道图像以数字信号传送给MCU。优点是CCD摄像头可以更远的探测赛道的变化，受外界光源干扰小，但其输入的数字信号处理复杂，需要对图像信号进行分割和识别，对于MCU来说处理速度很慢。不适合用在高速行驶的赛车上。
方案二：使用加载波信号的红外传感器探测赛道。原理是发射管加入高频载波信号，接收管接收到的信号通过滤波、检波和比较器，输出高低电平给单片机的IO口，其优点是不需要单片机进行信号的处理，直接通过IO口就可以读出传感器的状态，且传感器不受自然光影响，缺点是对反射物体表面反射系数要求较高，容易受到其他同频率的光源影响，且电路复杂，不容易调试。
方案三：使用直流发射的红外传感器探测赛道。原理是发射管工作在直流发射，接收管接收到的信号幅值大小表示反射面反射系数的大小，从而区别黑色和白色。优点是电路简单可靠，受高频信号干扰小，缺点是自然光影响较大。但是对于固定不变的光源，其在黑线和白线上检测的信号还是有很大的区别，完全不影响检测赛道。
基于上述方案分析，本次设计采用了方案三。
2.2 电机驱动模块
绝大多数直流电机采用开关驱动方式。开关驱动方式是使半导体功率器工作

在开关状态，通过控制脉宽调制PWM来控制电动机电枢电压，实现调速。PWM调速分为：定宽调频法、调宽调频法和定频调宽法。前两种方法由于在调速时改变了控制脉冲的周期（或频率），当控制频率与系统的固有频率接近时，将会引起振荡，因此，选用定频调宽法。
直流电机PWM控制系统有可逆和不可逆系统之分。可逆系统是电机可以正反两个方向旋转；不可逆是指电动机只能单向旋转。双极性可逆系统具有低速运行平稳性的优点，但也存在着电流波动大，功率损耗较大的缺点，尤其是必须增加死区来避免开关管直通的危险。单极性可逆系统在制动时，其制动效果会受到一定的限制。不可逆系统又分为无制动与有制动之分。由于不可逆控制系统控制简单可靠，结合本设计的需要，我们选用了有制动的不可逆PWM系统。对于其实现有两种方案。
方案一：采用MC33886集成芯片进行驱动。其优点就是电路简单。芯片内部集成了自举电路、MOS管，死区保护电路等，具有短路保护、过温保护、错误标志反馈等功能。由于其集成度高，价格也就相应的比较贵。
方案二：采用分立元件驱动。其最大缺点就是需要外接电路。但其应用很灵活，可以根据不同的驱动电流选择不同的MOS管，一旦发生烧元件的事故，可以很容易的换取。并且采用分立元件构成的电路也比较便宜。
综上所述，两种方案各有千秋，难以取舍，最终我们采取折中的办法，主板上兼顾两种方案。
2.3测速及距离检测模块
2.3.1速度检测方案
方案一：采用霍尔集成片。该器件内部由三片霍尔金属板组成，当磁铁正对金属板时，由于霍尔效应，金属板发生横向导通，因此可以在车轮上安装磁片，而将霍尔集成片安装在固定轴上，通过对脉冲的计数来进行车速测量。
方案二：受鼠标的工作原理启发，采用断续式光电开关。由于该开关是沟槽结构，可以将其置于固定轴上，再在车轮上均匀地固定多个遮光条，让其恰好通过沟槽，产生一个脉冲。通过脉冲的计数，对速度进行测量。
方案三：采用测速发电机。从废弃的手机中取出一个震动小电机，将该电机固定在车轴上，后轮带动震动电机旋转并输出电压信号，将该电压放大后送入单片机进行处理。当驱动电机全速运行时，震动电机输出最大电压；而在实际运行中，单片机对电压进行处理后，即可判断当前车速的快慢。

以上三种都是比较可行的转速测量方案。尤其是霍尔元件，在工业上得到广泛应用。但是在本次设计中，由于小车的车轮较小，方案一的磁片密集安装比较困难，容易产生相互干扰。方案二较方案一的检测精度要高一些，但是当车速很快时，单片机读取的脉冲不准确，存在误差，对车速的控制有影响。而方案三采用的是模拟量来测速，无论车速快慢，单片机均能准确地检测到电压，经过大量实验，用测速发电机测速精度可以达到97.7~99%。
综上论证，最终我们选择方案三。
2.3.2 距离检测方案
在小车的设计中，距离检测只是作为一种辅助手段，用于处理特殊情况，比如十字交叉角。我们拟采用光电码盘。在车后轮的轴上安装一铝片，在铝片上打18个孔，经过测算，车轮周长接近18cm，故每两个脉冲间隔相当于1cm。
2.3.3 加速度检测方案
为了更好地控制小车，我们考虑把加速度检测作为一种辅助手段，拟选用美新公司的RD6400。该传感器是双轴的：X轴和Y轴，输出电压值。理论上讲，小车在直道行驶时，X轴输出很小，Y轴输出最大，此时可以加速；过弯时，X 轴和Y轴均有电压输出，此时可以稍稍减速。
2.4 电源模块
为了能使智能车系统能正常工作，就需要对电池电压调节。其中，单片机系统、车速传感器电路需要5V电压，路径识别的光电传感器和接收器电路电压工作为5V或直接由电池提供、伺服电机工作电压范围4.8V到6V，直流电机可以使用7.2V 2000mAh Ni-cd蓄电池直接供电。考虑到由于驱动电机引起的电压瞬间下降的现象，因此采用低压降的三端稳压器成为必然。
常见的电源滤波电路分为三种：电容滤波，RC滤波及π型滤波。电容滤波是最简单常见的滤波电路，只要把滤波电容并联在电路的输出端与负载之间即可。但对于大电流的电路，为了要让电路有稳定的直流输出、较低的涟波直流变化，往往需要加大滤波电容的容量。RC滤波的效果比电容滤波效果更佳。对于大电流的电路，会产生大的压降。π型滤波用在负载直流电流较大的场合，压降较小，就能得到更好的消除涟波效果。在本设计中，由于是用电池供电，电压的稳定性和功耗对我们的设计很重要。因此，π型滤波是我们的最佳选择。

第三章 系统的硬件设计
3.1 红外检测电路设计
为了检测路面黑线，我们采用红外检测方法，图4为红外检测电路。原理为：当使能端EN为高时，检测屏蔽以节约能量。当EN为低时，发射管SE导通，当检测到黑线时，接收管RE电压将发生变化。这样我们就可以根据这种变化来识别黑线。对于电压的变化，我们采用了两种方法来处理：模拟量采集和数字量采集。模拟量采集就是把AN端直接接到单片机里，用软件的方法来设定阈值。数值量的方法就是把检测到的值通过比较器后输入单片机里，进行处理。图4中，由于发射电流较大，单片机是不能驱动的，因此，控制段输出是由ULN2003输出。R42、LED7用于显示检测的情况，便于观察调试。
图3.1 红外检测电路

3.2 电机驱动电路设计
主板兼顾了两种驱动方案，下面就驱动电路进行详细介绍。MOS管驱动电路如图3.2所示。
图3.2 电机驱动电路

电路中，选用的MOS管为K2554，驱动电流为75A、内阻为4.5 mW。Q4、Q8

与R22、R23、R24、R25构成高端驱动信号；Q9和R26、R27、R28构成低端驱动信号。PW0为电机的驱动信号。C16、C18用于消除电压尖波。由于电池电压较低不能够驱动高端MOS管FET1，因此，需要对高端控制部分进行升压处理。升压电路由Q2、Q3、Q6、Q7、Q8、R20、R21和C14、C17组成。PW2为升压脉冲输出端，输出为10KHz占空比50％。原理为：当PW2为低时，Q2、Q5截止、Q6、Q7导通，Q3中与V端相的二极管导通，C14端电压接近V伏；当PW2为高时，Q2、Q5导通、Q6、Q7截止，由于电容电压不能突变，因此C14端电压接近为2*V伏，达到了升压的目的。电容C17的作用为滤波和储能。
MC33886的驱动电路如下图所示：

图3.3 MC33886驱动电路图
3.3 转速检测电路设计
速度检测采用的是测速电机，输出的电压先经过高、低通滤波器（R15、C1、C8构成）滤除电磁干扰，再对信号进行放大，完整的检测电路如图3.4所示。

图3.4 转速检测电路

3.4 电源电路设计
本电路中，我们所使用的稳压器为LM2940，其不仅外接电路简单，而且带负载能力也比较强。LM2940的输出电流为1A，在输出为1A的情况下，其典型的压降只有0.5V。图3.5为5V电源产生电路，其中C3、L2和C5组成π型滤波器，这样使输出电压更稳定；C6、C8为存能、滤波电容。图3.6为舵机电源电路，在设计中，考虑到尽量使用相同芯片的原则，由于舵机电压需6V，因此在G脚并联两个二极管（D1为肖特基管，D11为普通二极管）使输出电压升高。

图3.7 加速度检测电路
图3.5 5V电源电路
图3.6 舵机电源电路
3.5 加速度检测电路设计
为了检测车的加速度情况，以便于控制，图3.7为加速度检测电路。传感器U3我们选用美新公司的RD6400，此传感器为双轴加速度传感器，其输出单片机可以直接采集处理。
硬件电路中，还加了许多去耦电容，其作用一是集成电路的蓄能电容，提供和吸收该集成电路的瞬间充放电能量、一是去除该器件的高频噪声。
3.6辅助电路设计
为了使智能车更智能，更人性化，我们加入了按键设定、声音提示、指示灯显示等电路。

第四章 控制规则分析
4.1 一般寻迹的控制规则
赛车寻迹的好坏关键在于对转向的控制，由于赛车在行驶的过程中是一个非线性时变系统，所以我们在自主寻迹的控制算法中运用了仿人智能控制的思想。仿人智能控制的基本思想是在控制过程中利用计算机模拟人的控制行为、功能，最大限度地识别和利用控制系统动态过程所提供的信息特征，进行启发和直觉推理，从而实现对缺乏精确数学模型的对象进行有效控制。具体来说，就是依据被控量偏差的变化(变化量大小、变化趋势等)，及时调整控制量的大小，来抑制偏差的变化。
在小车的寻迹算法中，首先要明白偏差与偏差的变化的确切含义。小车的前排检测部分装有9个传感器，行驶时要始终保证最中间的传感器与轨道重合，故将最中间的传感器定义为中位。当轨迹偏离中位时，轨迹与中位之间的距离差即为算法中的偏差。此次偏差与下一次偏差的差值称为偏差的变化。偏差不仅有大小还有方向，由于传感器部分是对称的，若设定偏差向某一方变化为正，则向相反的方向变化就为负。
智能车的自主寻迹不外乎两种：直道跟踪、弯道跟踪。小车在不同情况下的控制规则是不一样的，下面就对这两种状况下的控制规则进行详细阐述。
1) 直道跟踪
① 置一位置标志保存传感器和黑线的相对位置，始终让正中的传感器对准黑线。
② 如果出现了偏差,将根据偏差和偏差的变化对转向进行控制。
③ 若偏差增大，偏差的变化也增大，转向的调整幅度也要相应地增大，舵机角增大；若偏差减小，偏差的变化也减小，转向的调整幅度也要相应减小，舵机角可以保持不变或者稍稍减小；若偏差不变，舵机角将保持不变，直至偏差为零，最中间的传感器与轨迹重合，小车沿着轨迹行驶。
④ 若偏差反方向变化，转向的控制规则同③。
⑤ 转向的调整速度还与小车目前的速度有关。速度越快，转向的调整速度越快；速度越慢，转向的调整速度越慢。
⑥ 当偏差等于零时，直道要加速。
2) 弯道跟踪
① 判断是否进入弯道。若进弯前的速度很快，在进行刹车的同时以较快的

速度调整到适合弯道的转向。
② 进入弯道后，让小车以给定速度行驶，转向的调节尽量小，调节过大会出现 来回摆动的现象。
③ 小车出弯时加速。
4.2 特殊寻迹的控制规则
当赛车由于速度过快冲出赛道或者在行驶的过程中遇到赛道交叉，我们针对这两种特殊的情况需做特殊处理。
1）赛车冲出赛道

图4.1 小车偏离轨道
如图4.1所示，当赛车跑出赛道时，传感器检测不到黑线（即读出状态是0），
针对这种情况，我们制定以下规则：
① 置一位置标志保存传感器和黑线的相对位置。
② 当黑线出了传感器检测范围时，查询位置标志，控制舵机为和出线前位置同向的最大舵机角。
③ 保持最大舵机角直到传感器再次检测到黑线，然后进入正常寻迹。
2）十字交叉
如图4.2所示，交叉角分为两种，直线上的交叉和弯道后的交叉，针对这两种情况，我们分别制定了相应的规则。
(1) 直道交叉
① 置一位置标志保存传感器和黑线的相对位置。
② 当有多个传感器同时检测到黑线时，查询位置标志，控制舵机为交叉前位置同向的舵机角

图4.2 十字交叉角
(2) 弯道后的交叉
① 置一位置标志保存传感器和黑线的相对位置。
② 当出现传感器间隔检测到黑线时，查询位置标志，控制舵机为交叉前位置同向的最大舵机角。
③ 保持最大舵机角直到后排传感器检测到交叉，然后进入正常寻迹。

第五章 系统的软件设计
5.1 主程序流程图

开始
初始化
开中断
进入舵机算法调节程序
进入保护电路检测程序
进入驱动电机调速程序
是否到2ms
是否到10ms
是否到100ms
是
是
是
否
否
否

扫描传感器状态

图5.1 主程序流程图
5.2 中断程序流程图

中断
调速计数器加一
保护计数器加一
舵机计数器加一
出中断

AD测速
移动平均滤波算法
PI调节
输出调节后的PWM

图5.2 中断程序流程图 图5.3 电机调速程序流程图
5.3 电机调速程序流程图
5.4 保护电路程序流程图

AD采集端电压分压值
采集值是否小于3.2V
红灯LV点亮，蜂鸣器报警
是
求算术平均值
否
退出

图5.4 保护电路程序流程图
5.5 出线处理程序流程图

Flag_out= =1？
保持舵机角度
并扫描传感器

计算传感器状态
比较当前值和出线
前的值是否相等
是
否
是
否
清除Flag_out标志位
进入正常寻迹处理

图5.5 传感器出线处理程序流程图
回溯之前10次传感器状态
计算传感器状态
保持舵机角度
并扫描传感器
算出运动趋势
后排传感器检测是否出现交叉
Flag_across=0
恢复正常寻迹
否
是
5.6 交叉处理程序流程图

图5.6 交叉处理程序流程图

第六章 系统调试过程说明
6.1开发工具
本次设计采用清华大学制作的BDM for S12开发工具。BDM全称背景调试模式（Background Debug Mode），优点有：
● 针对MCU在工作电压和工作频率范围内所有功能的实时仿真
● 针对目标器件的单步调试，全速运行和跟踪
● 运行时可查看和更改内部寄存器和内存的数值
● 片内多重硬件断点
● 片内硬件触发和缓存替代了昂贵的总线分析器
6.2 制作、安装
本次设计电路板为两层PCB板，测速电机和光电码盘安装于车尾，红外检测安装在车前端10cm处，主板装于车身上，车体配重适当。车模的外形如图6.1
所示：
图6.1 车模外形图
6.3调试过程说明

6.3.1测速电机调试
驱动电机采用闭环控制，应用PI调节器，速度值通过测速电机后经AD采集，送入MCU处理。经过反复试验，确定了PI值。
6.3.2测速电机调试
经过试验，舵机右转极限位置对应脉冲宽度为1.15ms，左转极限位置对应脉冲宽度为1.85ms，中心位置对应脉冲宽度为1.5ms。
6.3.3按键调试
调试中，为了方便一些基本参数的调节，使用了MCU中EEPROM，将按键设定的基本参数存入EEPROM中，方便读取和修改，而不需重新烧写程序。

第七章 车模主要技术参数说明
7.1车模基本参数
表6.1 车模基本参数
长
(cm)
宽
(cm)
高
(cm)
总重
(kg)
40
20
10
1.25
7.2传感器及电机个数
表6.2 检测元器件清单
器件名称
反射式红外传感器
MEMSIC MXR6400G加速度传感器
测速发电机
数量
14对
1个
1个
7.3 电路参数
静态电流：0.5A
最大电流：2.5A
电容总容量：1920uF
7.4赛道信息
传感器检测精度：传感器检测分17个等级.
赛道采点频率：1次/1cm.

第八章 结论
经过我们三名队员及指导老师3个月的设计、制作、调试，赛车已经达到较高水平，在本对自行设计长25米的赛道上，能够顺利过急弯、交叉弯等，且速度达1.2m/s以上。红外传感器工作稳定，能够适应不同环境和赛道，适应能力强。驱动电机实现动态调速，速度控制稳定。舵机控制采用仿人智能控制，控制效果好，寻迹精确。但赛车仍存在需要改进的地方，主要改进方面是舵机的转向不足，车轮附着力不够强等硬件部件，另外软件设计上仍有优化空间，可以进一步优化，提高单片机使用效率。

图A 系统硬件原理图

#include "Cpu.h"
#include "Events.h"
#include "keys.h"
#include "protect.h"
#include "motor.h"
#include "wheel.h"
#include "IEE1.h"
#include "AD2.h"
#include "AD3.h"
#include "Word1.h"
#include "systime.h"
#include "sensorback.h"
#include "Bit1.h"
#include "Cap.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
#include "MATH.H"

typedef unsigned char tU08;
typedef unsigned int tU16;
typedef unsigned long tU32;
typedef signed char tS08;
typedef int tS16;
typedef long tS32;
const k1=300;
const k2=4;
bool flag_break=0;
bool flag_out=0;
bool flag_across=0;
tU08 wheel_record[3]={9,9,9};
tU08 count_key=0;
tU08 spdmin_temp=10;
tU08 spdmax_temp=20;
tU08 sps=2;
tU08 strs=30;
tU16 wheel=1550;
tU16 loca=0;
tU16 AD_con[11];
tU16 b[20];
tU16 ADspeed;
tU16 ADprotect;
tU16 spd=260;
tU16 spdpwm;
tU16 spdmax;
tU16 spdmin;
tU16 AD_result[9];
tU16 sv_min[9];
tU16 sv_max[9];
tU16 sv_level[9];
tS16 i;
tS32 newe;
tS32 sum1=0;
tS32 un;
extern tU16 count_wheel;
extern tU16 count_delay;
extern tU16 count_motor;
extern tU16 count_protect;
extern tU16 line;
extern bool flag_abs;
tU16 ad(tS16 data);
void scan();
void calculate();
void wheel_config();
void motor_pid();
void protect();
void Initial();
//==========calculate the state of sensor===========
void calculate()
{ loca=0;
flag_across=0;
flag_out=0;
for (i=1;i<10;i++) if (AD_result[9-i] >= sv_level[9-i]) loca=i+loca*10;
switch(loca)
{
case 0: loca=0;flag_out=1;break;
case 1: loca=1;break;
case 12: loca=2;break;
case 2: loca=3;break;
case 23: loca=4;break;
case 3: loca=5;break;
case 34: loca=6;break;
case 4: loca=7;break;
case 45: loca=8;break;
case 5: loca=9;break;
case 56: loca=10;break;
case 6: loca=11;break;
case 67: loca=12;break;
case 7: loca=13;break;
case 78: loca=14;break;
case 8: loca=15;break;
case 89: loca=16;break;
case 9: loca=17;break;
case 123: loca=1;break;
case 234: loca=3;break;
case 345: loca=5;break;
case 456: loca=5;break;
case 567: loca=5;break;
case 678: loca=7;break;
case 789: loca=9;break;
default: flag_across=1;break;
}
PORTB=(byte)(loca);
}
//==================start AD_con=====================
void scan()
{
AD2_Measure(1);
AD2_GetValue16(&AD_con[0]);
AD3_Measure(1);
AD3_GetValue16(&AD_con[3]);
for (i=0;i<3;i++) AD_result[i]=AD_con[i]>>6;
for (i=5;i<11;i++) AD_result[i-2]=AD_con[i]>>6;
ADspeed=AD_con[3]>>6;
ADprotect=AD_con[4]>>6;
}
//================config wheel direction===============
void wheel_config()
{tS16 error_p;
tS16 error_d;
if ((!flag_out)&&(!flag_across)&&(((loca-wheel_record[0])<8)||((wheel_record[0]-loca)<8))) wheel_record[0]=(byte)(loca);
error_p=wheel_record[0]-wheel_record[1];
error_d=error_p-(wheel_record[1]-wheel_record[2]);
wheel=wheel+error_p*27+error_d*20;
if ((wheel_record[0]>15)&(wheel_record[1]<=15)) wheel=wheel+80;
if ((wheel_record[1]>15)&(wheel_record[0]<=15)) wheel=wheel-80;
if ((wheel_record[0]>3)&(wheel_record[1]<=3)) wheel=wheel+40;
if ((wheel_record[1]>3)&(wheel_record[0]<=3)) wheel=wheel-40;
wheel_SetDutyUS(wheel);
wheel_record[2]=wheel_record[1];
wheel_record[1]=wheel_record[0];
}
void wheel_out()
{
if (sensorback_GetVal()!=31) flag_out=0;
}
//=========================avg=============================
tU16 ad(tS16 data)
{
tU16 sum;
for(i=0;i<19;i++){b[i]=b[i+1];}
b[19]=data;
sum=0;
for(i=0;i<20;i++){sum+=b[i];}
sum/=20;
return sum;
}
//========================motor PID======================
tU16 pid(tS16 n, tS16 m)
{
newe=n-m;
sum1=sum1+newe;
un=k1*newe+sum1/k2;
if(un>65000){un=65000;}
if(un<0){un=0;}
return((tU16)(un));
}
//========================ADcon=========================
tU16 acspeed()
{
ADspeed=ad(ADspeed);
return (ADspeed);
}
//=====================motor============================
void motor_pid()
{
if ((wheel>1400)&&(wheel<1700)&&(wheel_record[0]>5)&&(wheel_record[0]<13)) spd=spdmax;
else spd=spdmin;
if (spd>=spdmax) spd=spdmax;
else if (spd<=spdmin) spd=spdmin;
spdpwm=pid(spd,acspeed());
motor_SetRatio16(spdpwm);
}
//=================protect==========================
void protect()
{
if (ADprotect<=656) protect_PutVal(2);
}
//=====================sav