1、第一届“飞思卡尔”杯全国大学生 智能汽车邀请赛技 术 报 告附录 D：基于仿人智能控制算法的智能车转向舵机控制学 校：重庆大学队伍名称：重庆大学二队 参赛队员：林引 陈皎 付强带队教师：盛朝强 关于技术报告和研究论文使用授权的说明本人完全了解第一届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名：林引 陈皎 付强带队教师签名：盛朝强日 期：2006 年 8 月 6-
2、I -目 录第一章 引言- 1第二章 系统方案论证与选择- 22.1 路面黑线检测模块- 22.2 电机驱动模块- 22.3 测速及距离检测模块- 32.4 电源模块- 4第三章 系统的硬件设计-53.1 红外检测电路设计- 53.2 电机驱动电路设计- 53.3 转速检测电路设计 - 63.4 电源电路设计 - 73.5 加速度检测电路设计-73.6 辅助电路设计-7第四章 控制规则分析- 84.1 一般寻迹的控制规则-84.2 特殊寻迹的控制规则-9第五章 系统的软件设计-115.1 主程序流程图-115.2 中断程序流程图-115.3 电机调速程序流程图-125.4 保护电路程序流程图-
3、125.5 出线处理程序流程图-12- II -5.6 交叉处理程序流程图-13第六章 系统设计说明 -146.1 开发工具-146.2 制作、安装-146.3 调试过程说明-146.3.1 测速电机调试-156.3.2 舵机调试-156.3.3 按键调试-15第七章 车模主要技术参数说明-167.1 车模基本参数-167.2 传感器及电机个数-167.3 电路参数-167.4 赛道信息-16第八章 结论-17参考文献附录 A：系统硬件原理图附录 B：主程序源代码附录 C：中断服务程序源代码附录 D：研究论文- III -第一章 引言- 1 -第一章 引言本设计采用飞思卡尔 16 位微控制器
4、MC9S12DG128 作为核心控制单元。赛道黑线检测使用反射式红外传感器，车速采用闭环 PI 控制，速度传感器使用测速电机，利用 PWM 实现动态电机调速。测距应用简单而精确的光电码盘。电机驱动采用 H 桥半桥控制。基于这些可靠的硬件设计，开发了一套完备的软件设计，包括电机 PI 控制算法，舵机仿人智能控制算法，低电压报警设计等。本设计主要特色： 模块化设计 连续的电机调速，精确可靠； 优化的软件算法，智能化的自动控制，定位精确； 主板设计兼顾多种用途方案，复用性强； 车模重心适当，整车设计美观、合理； 电源指示灯、低压指示、报警，电源工作状态一目了然；第一届全国大学生智能汽车邀请赛技术报告
5、- 2 -第二章 系统方案论证与选择整个系统可以划分为以下几个基本模块，如图 2.1 所示：图 2.1 系统整体框图2.1 路面黑线检测模块通常用以检测黑线的传感器有 CCD 和红外两种。方案一：使用 CCD 传感器探测赛道。原理是通过 CCD 传感器将赛道图像以数字信号传送给 MCU。优点是 CCD 摄像头可以更远的探测赛道的变化，受外界光源干扰小，但其输入的数字信号处理复杂，需要对图像信号进行分割和识别，对于 MCU 来说处理速度很慢。不适合用在高速行驶的赛车上。方案二：使用加载波信号的红外传感器探测赛道。原理是发射管加入高频载波信号，接收管接收到的信号通过滤波、检波和比较器，输出高低电平
6、给单片机的 IO 口，其优点是不需要单片机进行信号的处理，直接通过 IO 口就可以读出传感器的状态，且传感器不受自然光影响，缺点是对反射物体表面反射系数要求较高，容易受到其他同频率的光源影响，且电路复杂，不容易调试。方案三：使用直流发射的红外传感器探测赛道。原理是发射管工作在直流发射，接收管接收到的信号幅值大小表示反射面反射系数的大小，从而区别黑色和白色。优点是电路简单可靠，受高频信号干扰小，缺点是自然光影响较大。但是对于固定不变的光源，其在黑线和白线上检测的信号还是有很大的区别，完全不影响检测赛道。基于上述方案分析，本次设计采用了方案三。2.2 电机驱动模块绝大多数直流电机采用开关驱动方式。
7、开关驱动方式是使半导体功率器工作MCU路面检测低电压检测LED 显示 转速检测电机驱动舵机调节键盘低压报警第二章 2.3 测速及距离检测模块-3 -在开关状态，通过控制脉宽调制 PWM 来控制电动机电枢电压，实现调速。PWM调速分为：定宽调频法、调宽调频法和定频调宽法。前两种方法由于在调速时改变了控制脉冲的周期（或频率） ，当控制频率与系统的固有频率接近时，将会引起振荡，因此，选用定频调宽法。直流电机 PWM 控制系统有可逆和不可逆系统之分。可逆系统是电机可以正反两个方向旋转；不可逆是指电动机只能单向旋转。双极性可逆系统具有低速运行平稳性的优点，但也存在着电流波动大，功率损耗较大的缺点，尤其是
8、必须增加死区来避免开关管直通的危险。单极性可逆系统在制动时，其制动效果会受到一定的限制。不可逆系统又分为无制动与有制动之分。由于不可逆控制系统控制简单可靠，结合本设计的需要，我们选用了有制动的不可逆 PWM 系统。对于其实现有两种方案。方案一：采用 MC33886 集成芯片进行驱动。其优点就是电路简单。芯片内部集成了自举电路、MOS 管，死区保护电路等，具有短路保护、过温保护、错误标志反馈等功能。由于其集成度高，价格也就相应的比较贵。方案二：采用分立元件驱动。其最大缺点就是需要外接电路。但其应用很灵活，可以根据不同的驱动电流选择不同的 MOS 管，一旦发生烧元件的事故，可以很容易的换取。并且采
9、用分立元件构成的电路也比较便宜。综上所述，两种方案各有千秋，难以取舍，最终我们采取折中的办法，主板上兼顾两种方案。2.3 测速及距离检测模块2.3.1 速度检测方案方案一：采用霍尔集成片。该器件内部由三片霍尔金属板组成，当磁铁正对金属板时，由于霍尔效应，金属板发生横向导通，因此可以在车轮上安装磁片，而将霍尔集成片安装在固定轴上，通过对脉冲的计数来进行车速测量。方案二：受鼠标的工作原理启发，采用断续式光电开关。由于该开关是沟槽结构，可以将其置于固定轴上，再在车轮上均匀地固定多个遮光条，让其恰好通过沟槽，产生一个脉冲。通过脉冲的计数，对速度进行测量。方案三：采用测速发电机。从废弃的手机中取出一个震
10、动小电机，将该电机固定在车轴上，后轮带动震动电机旋转并输出电压信号，将该电压放大后送入单片机进行处理。当驱动电机全速运行时，震动电机输出最大电压；而在实际运行中，单片机对电压进行处理后，即可判断当前车速的快慢。第一届全国大学生智能汽车邀请赛技术报告-4 -以上三种都是比较可行的转速测量方案。尤其是霍尔元件，在工业上得到广泛应用。但是在本次设计中，由于小车的车轮较小，方案一的磁片密集安装比较困难，容易产生相互干扰。方案二较方案一的检测精度要高一些，但是当车速很快时，单片机读取的脉冲不准确，存在误差，对车速的控制有影响。而方案三采用的是模拟量来测速，无论车速快慢，单片机均能准确地检测到电压，经过大
11、量实验，用测速发电机测速精度可以达到 97.799%。综上论证，最终我们选择方案三。2.3.2 距离检测方案在小车的设计中，距离检测只是作为一种辅助手段，用于处理特殊情况，比如十字交叉角。我们拟采用光电码盘。在车后轮的轴上安装一铝片，在铝片上打 18 个孔，经过测算，车轮周长接近 18cm，故每两个脉冲间隔相当于1cm。2.3.3 加速度检测方案为了更好地控制小车，我们考虑把加速度检测作为一种辅助手段，拟选用美新公司的 RD6400。该传感器是双轴的：X 轴和 Y 轴，输出电压值。理论上讲，小车在直道行驶时，X 轴输出很小，Y 轴输出最大，此时可以加速；过弯时，X 轴和 Y 轴均有电压输出，此
12、时可以稍稍减速。2.4 电源模块为了能使智能车系统能正常工作，就需要对电池电压调节。其中，单片机系统、车速传感器电路需要 5V 电压，路径识别的光电传感器和接收器电路电压工作为 5V 或直接由电池提供、伺服电机工作电压范围 4.8V 到 6V，直流电机可以使用 7.2V 2000mAh Ni-cd 蓄电池直接供电。考虑到由于驱动电机引起的电压瞬间下降的现象，因此采用低压降的三端稳压器成为必然。常见的电源滤波电路分为三种：电容滤波，RC 滤波及 型滤波。电容滤波是最简单常见的滤波电路，只要把滤波电容并联在电路的输出端与负载之间即可。但对于大电流的电路，为了要让电路有稳定的直流输出、较低的涟波直流
13、变化，往往需要加大滤波电容的容量。RC 滤波的效果比电容滤波效果更佳。对于大电流的电路，会产生大的压降。 型滤波用在负载直流电流较大的场合，压降较小，就能得到更好的消除涟波效果。在本设计中，由于是用电池供电，电压的稳定性和功耗对我们的设计很重要。因此， 型滤波是我们的最佳选择。第三章 3.1 红外检测电路设计- 5-第三章 系统的硬件设计3.1 红外检测电路设计为了检测路面黑线，我们采用红外检测方法，图 4 为红外检测电路。原理为：当使能端 EN 为高时，检测屏蔽以节约能量。当 EN 为低时，发射管 SE 导通，当检测到黑线时，接收管 RE 电压将发生变化。这样我们就可以根据这种变化来识别黑线
14、。对于电压的变化，我们采用了两种方法来处理：模拟量采集和数字量采集。模拟量采集就是把 AN 端直接接到单片机里，用软件的方法来设定阈值。数值量的方法就是把检测到的值通过比较器后输入单片机里，进行处理。图 4 中，由于发射电流较大，单片机是不能驱动的，因此，控制段输出是由 ULN2003 输出。R42、LED7 用于显示检测的情况，便于观察调试。3.2 电机驱动电路设计主板兼顾了两种驱动方案，下面就驱动电路进行详细介绍。MOS管驱动电路如图3.2所示。电路中，选用的MOS管为K2554，驱动电流为75A、内阻为4.5 m 。Q4、Q8图 3.1 红外检测电路R37SERER38ENVCV+PA6
15、VREF54 2U3BLM339R41 R42R39LED7R40EN6 VCCAN图 3.2 电机驱动电路R20 R21+C17+C14bceQ6bceQ7PW2bceQ5bceQ2Q3VbceQ4bceQ8bceQ9R22 R23 R24R26R25R27R28C16C18V231FET2PW0231FET1OUT1VMO第一届全国大学生智能汽车邀请赛技术报告- 6-与R22、R23、R24、R25构成高端驱动信号；Q9和R26、R27、R28构成低端驱动信号。PW0为电机的驱动信号。C16、C18用于消除电压尖波。由于电池电压较低不能够驱动高端MOS管FET1，因此，需要对高端控制部分进
16、行升压处理。升压电路由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 构成）滤除电磁干扰，再对信号进行
17、放大，完整的检测电路如图 3.4 所示。第一届全国大学生智能汽车邀请赛技术报告- 6-图 3.4 转速检测电路第三章 3.4 电源电路设计- 7-3.4 电源电路设计本电路中，我们所使用的稳压器为 LM2940，其不仅外接电路简单，而且带负载能力也比较强。LM2940 的输出电流为 1A，在输出为 1A 的情况下，其典型的压降只有 0.5V。图 3.5 为 5V 电源产生电路，其中 C3、L2 和 C5 组成 型滤波器，这样使输出电压更稳定；C6、C8 为存能、滤波电容。图 3.6 为舵机电源电路，在设计中，考虑到尽量使用相同芯片的原则，由于舵机电压需 6V，因此在 G 脚并联两个二极管（D1
18、 为肖特基管，D11 为普通二极管）使输出电压升高。3.5 加速度检测电路设计为了检测车的加速度情况，以便于控制，图 3.7 为加速度检测电路。传感器 U3 我们选用美新公司的 RD6400，此传感器为双轴加速度传感器，其输出单片机可以直接采集处理。硬件电路中，还加了许多去耦电容，其作用一是集成电路的蓄能电容，提供和吸收该集成电路的瞬间充放电能量、一是去除该器件的高频噪声。3.6 辅助电路设计为了使智能车更智能，更人性化，我们加入了按键设定、声音提示、指示灯显示等电路。图 3.5 5V 电源电路VIN 12R1POW1 2L2+C3 +C5G2Vin1 Vout 3U1+C6C8104R21K
19、LED1VCCV V+G2Vin1 Vout 3U2+C7C9104R3LED2VDDV+D1D11图 3.6 舵机电源电路PD1TP2COM3NC4 NC 5Yout 6Xout 7VDD 8U3 R12R13 C12C11AN14AN15VCC图 3.7 加速度检测电路第一届全国大学生智能汽车邀请赛技术报告- 8 -第四章 控制规则分析4.1 一般寻迹的控制规则赛车寻迹的好坏关键在于对转向的控制，由于赛车在行驶的过程中是一个非线性时变系统，所以我们在自主寻迹的控制算法中运用了仿人智能控制的思想。仿人智能控制的基本思想是在控制过程中利用计算机模拟人的控制行为、功能，最大限度地识别和利用控制系
20、统动态过程所提供的信息特征，进行启发和直觉推理，从而实现对缺乏精确数学模型的对象进行有效控制。具体来说，就是依据被控量偏差的变化(变化量大小、变化趋势等)，及时调整控制量的大小，来抑制偏差的变化。在小车的寻迹算法中，首先要明白偏差与偏差的变化的确切含义。小车的前排检测部分装有 9 个传感器，行驶时要始终保证最中间的传感器与轨道重合，故将最中间的传感器定义为中位。当轨迹偏离中位时，轨迹与中位之间的距离差即为算法中的偏差。此次偏差与下一次偏差的差值称为偏差的变化。偏差不仅有大小还有方向，由于传感器部分是对称的，若设定偏差向某一方变化为正，则向相反的方向变化就为负。智能车的自主寻迹不外乎两种：直道跟
21、踪、弯道跟踪。小车在不同情况下的控制规则是不一样的，下面就对这两种状况下的控制规则进行详细阐述。1) 直道跟踪 置一位置标志保存传感器和黑线的相对位置，始终让正中的传感器对准黑线。 如果出现了偏差,将根据偏差和偏差的变化对转向进行控制。 若偏差增大，偏差的变化也增大，转向的调整幅度也要相应地增大，舵机角增大；若偏差减小，偏差的变化也减小，转向的调整幅度也要相应减小，舵机角可以保持不变或者稍稍减小；若偏差不变，舵机角将保持不变，直至偏差为零，最中间的传感器与轨迹重合，小车沿着轨迹行驶。 若偏差反方向变化，转向的控制规则同。 转向的调整速度还与小车目前的速度有关。速度越快，转向的调整速度越快；速度
22、越慢，转向的调整速度越慢。 当偏差等于零时，直道要加速。2) 弯道跟踪 判断是否进入弯道。若进弯前的速度很快，在进行刹车的同时以较快的第四章 4.2 特殊寻迹的控制规则-9 -速度调整到适合弯道的转向。 进入弯道后，让小车以给定速度行驶，转向的调节尽量小，调节过大会出现 来回摆动的现象。 小车出弯时加速。4.2 特殊寻迹的控制规则当赛车由于速度过快冲出赛道或者在行驶的过程中遇到赛道交叉，我们针对这两种特殊的情况需做特殊处理。1）赛车冲出赛道图 4.1 小车偏离轨道如图 4.1 所示，当赛车跑出赛道时，传感器检测不到黑线（即读出状态是0），针对这种情况，我们制定以下规则： 置一位置标志保存传感器
23、和黑线的相对位置。 当黑线出了传感器检测范围时，查询位置标志，控制舵机为和出线前位置同向的最大舵机角。 保持最大舵机角直到传感器再次检测到黑线，然后进入正常寻迹。2）十字交叉如图 4.2 所示，交叉角分为两种，直线上的交叉和弯道后的交叉，针对这两种情况，我们分别制定了相应的规则。(1) 直道交叉 置一位置标志保存传感器和黑线的相对位置。 当有多个传感器同时检测到黑线时，查询位置标志，控制舵机为交叉前位置同向的舵机角第一届全国大学生智能汽车邀请赛技术报告-10 -图 4.2 十字交叉角(2) 弯道后的交叉 置一位置标志保存传感器和黑线的相对位置。 当出现传感器间隔检测到黑线时，查询位置标志，控制
24、舵机为交叉前位置同向的最大舵机角。 保持最大舵机角直到后排传感器检测到交叉，然后进入正常寻迹。第五章 5.1 主程序流程图-11 -第五章 系统的软件设计5.1 主程序流程图图 5.1 主程序流程图5.2 中断程序流程图扫描传感器状态开始初始化开中断进入舵机算法调节程序进入保护电路检测程序进入驱动电机调速程序是否到 2ms 是否到 10ms 是否到 100ms 是是是否否否第一届全国大学生智能汽车邀请赛技术报告- 12 -图 5.2 中断程序流程图 图 5.3 电机调速程序流程图5.3 电机调速程序流程图5.4 保护电路程序流程图图 5.4 保护电路程序流程图 5.5 出线处理程序流程图中断调
25、速计数器加一保护计数器加一舵机计数器加一出中断AD 测速移动平均滤波算法PI 调节输出调节后的 PWMAD 采集端电压分压值采集值是否小于 3.2V红灯 LV 点亮，蜂鸣器报警是求算术平均值否退出第五章 5.6 交叉处理程序流程图- 13 -图 5.5 传感器出线处理程序流程图5.6 交叉处理程序流程图图 5.6 交叉处理程序流程图Flag_out= =1？保持舵机角度并扫描传感器计算传感器状态比较当前值和出线前的值是否相等是否是否清除 Flag_out标志位进入正常寻迹处理回溯之前 10 次传感器状态计算传感器状态保持舵机角度并扫描传感器算出运动趋势后排传感器检测是否出现交叉Flag_acr
26、oss=0恢复正常寻迹否是第一届全国大学生智能汽车邀请赛技术报告- 14 -第六章 系统调试过程说明6.1 开发工具本次设计采用清华大学制作的BDM for S12开发工具。BDM全称背景调试模式（Background Debug Mode），优点有： 针对MCU在工作电压和工作频率范围内所有功能的实时仿真 针对目标器件的单步调试，全速运行和跟踪 运行时可查看和更改内部寄存器和内存的数值 片内多重硬件断点 片内硬件触发和缓存替代了昂贵的总线分析器6.2 制作、安装本次设计电路板为两层 PCB 板，测速电机和光电码盘安装于车尾，红外检测安装在车前端 10cm 处，主板装于车身上，车体配重适当。车
27、模的外形如图 6.1所示：图 6.1 车模外形图第一届全国大学生智能汽车邀请赛技术报告- 14 -6.3 调试过程说明第六章 6.3 调试过程说明- 15 -6.3.1 测速电机调试驱动电机采用闭环控制，应用 PI 调节器，速度值通过测速电机后经 AD 采集，送入 MCU 处理。经过反复试验，确定了 PI 值。6.3.2 测速电机调试经过试验，舵机右转极限位置对应脉冲宽度为 1.15ms，左转极限位置对应脉冲宽度为 1.85ms，中心位置对应脉冲宽度为 1.5ms。6.3.3 按键调试调试中，为了方便一些基本参数的调节，使用了 MCU 中 EEPROM，将按键设定的基本参数存入 EEPROM
28、中，方便读取和修改，而不需重新烧写程序。第一届全国大学生智能汽车邀请赛技术报告- 16 -第七章 车模主要技术参数说明7.1 车模基本参数表 6.1 车模基本参数长(cm)宽(cm)高(cm)总重(kg)40 20 10 1.257.2 传感器及电机个数表 6.2 检测元器件清单器件名称 反射式红外传感器 MEMSIC MXR6400G 加速度传感器测速发电机数量 14 对 1 个 1 个7.3 电路参数静态电流：0.5A最大电流：2.5A电容总容量：1920uF7.4 赛道信息传感器检测精度：传感器检测分 17 个等级.赛道采点频率：1 次/1cm.第八章 结论- 17 -第八章 结论经过我
29、们三名队员及指导老师 3 个月的设计、制作、调试，赛车已经达到较高水平，在本对自行设计长 25 米的赛道上，能够顺利过急弯、交叉弯等，且速度达 1.2m/s 以上。红外传感器工作稳定，能够适应不同环境和赛道，适应能力强。驱动电机实现动态调速，速度控制稳定。舵机控制采用仿人智能控制，控制效果好，寻迹精确。但赛车仍存在需要改进的地方，主要改进方面是舵机的转向不足，车轮附着力不够强等硬件部件，另外软件设计上仍有优化空间，可以进一步优化，提高单片机使用效率。附录 A 系统硬件原理图-I -123456ABCD654321D C B AVIN12SWITCH1R11K POWER112L2 L-2P +
30、C310u+C510u12L1L-2P+C1220u+C2220uG 2Vin1Vout3U1LM2940+ C6E/CC8104R21KLED1VCC4104VVMOVV+G 2Vin1Vout3U2LM2940+ C7E/CC9104R31KLED2VDV+D11N40713579111315171921232527293133353739414345474951535557596163246810121416182022242628303234363840424446485052545658606264P1 64PIN13579111315171921232527293133353739
31、414345474951535557596163246810121416182022242628303234363840424446485052545658606264P2 64PINVCPT0PT1PT2PT3P13aPW3PW2PW1PW0PK3PK2PK1FSPS2PS3PS4PS5PS6PS7P97PJ7PJ6PM5PM4P107PK7PW7PW6PW5PW4P65aAN00AN08AN01AN09AN02AN10AN03AN11AN04AN12AN05AN13AN06AN14AN07AN15P83aP84P85PM7PM6VCVCPM0PM2PM1PM3PT4PT5PT6PT7PK5
32、PK4PJ1PJ0PB0PB1PB2PB3PB4PB5PB6PB7PH7PH6PH5PH4PE7PE4P41aP43aP4aP48PH3PH2PH1PH0PE3PE2PE1PE0PA0PA1PA2PA3PA4PA5PA6PA7AGND1FS2IN13V+4V+5OUT16OUT17DNC8PGND9PGND10PGND11PGND12D213OUT214OUT215V+16CCP17D118IN219DNC20U4 MC386C1533nFGNDGNDGND VMO OUT1VMOOUT2R14 1KVCFSIN2D1 D2IN112SW1R41KVC AN00 12 SW2R51KVC AN
33、01 12 SW3R61KVCAN0212SW4R71KVC AN03123CON1 CON31234CON2 CON4R290.05R VDPW7VCAN0812POWER-IN1 VIN12SW5R81KVC AN04bc eQ1R15470VCIN2IN1R161KR181KVCD1D2PW1 PW0PW2PW3PD1TP2COM3NC4NC5Yout6Xout7VD8U3 R640R1210K R1310KC12104C1104AN14 AN15VCR174.7KR194.7K1234J1 CON4OUT1 OUT2R11KR91K R101KVINLOWV1PS11234567891
34、011121314151617181920J4CON20V+GNDVCPM4PE5 PM5PE6 PM6PM1PE2 PM2PE3PE1 PM0PE012JP1 JP122 JP12 JP3JPR201KR214.7K+C1710u+C144.7ubc eQ6bc eQ7PW2bc eQ5 Component_1bc eQ2HIT9013 Q3Vbc eQ4bc eQ8 bc eQ9R24.7KR231KR2410R261KR2510K R2710KR2810C16102 C18102V2 31FET22SK2545PW02 31 FET12SK2545OUT1VMOC10104VCVCAN0
35、9 PJ01234567891011121314151617181920J3CON20V+GNDVCPA0PA1PA2PA3PA4PA5PA6PA7 PH0PS4PS5PS6PS7PS3PS2PS1C13C/C1 2J2JUPPM3PE4PE7 PM7bc eQ10VCPJ11 2BEP BUZERD11N407PE5PE6 PT7图 A 系统硬件原理图附录 B 主程序源代码- I -#include “Cpu.h“#include “Events.h“#include “keys.h“#include “protect.h“#include “motor.h“#include “wheel.
36、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 “M
37、ATH.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_record3=9,9,9;tU08 count_key=0;tU08 spdmin_temp=10;tU08 spdmax_te
38、mp=20;tU08 sps=2;tU08 strs=30;tU16 wheel=1550;tU16 loca=0;附录 B 主程序源代码- II -tU16 AD_con11;tU16 b20;tU16 ADspeed;tU16 ADprotect;tU16 spd=260;tU16 spdpwm;tU16 spdmax;tU16 spdmin;tU16 AD_result9;tU16 sv_min9;tU16 sv_max9;tU16 sv_level9;tS16 i;tS32 newe;tS32 sum1=0;tS32 un;extern tU16 count_wheel;extern
39、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
40、=0;for (i=1;i= sv_level9-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;附录 B 主程序源代码- III -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;ca
41、se 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 67
42、8: 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(AD3_Measure(1);AD3_GetValue16(for (i=0;i6;for (i=5;i6;ADspeed=AD_con36; ADprotect=AD_con46;/=config wheel direction=void wheel_config()附录 B 主程序源代码- IV -tS1
43、6 error_p;tS16 error_d;if (!flag_out)if(un1400)else if (spd=spdmin) spd=spdmin;spdpwm=pid(spd,acspeed();motor_SetRatio16(spdpwm); /=protect=void protect()if (ADprotect=656) protect_PutVal(2); /=save parameter=void minAD() while(keys_GetBit(3) scan();for (i=0;i9;i+) sv_mini=AD_resulti;for (i=0;i9;i+) IEE1_SetWord(0x800+2*i,sv_mini
