简 介： 本文详细介绍了我们为了十六届全国大学生智能车大赛室外越野组准备的车模。车模采用Infineon公司的TC377芯片作为控制器，通过电感采集数据，并离线训练神经网络，后使用nnom部署到单片机上，利用神经网络预测车模状态，并利用预测出的结果控制舵机，从而对车辆进行控制；同时通过对车模机械结构进行合理改进，以获得更好的性能；利用蓝牙、上位机、OLED屏幕等对程序进行调试，获得更好的运行效果。
关键词： 智能车，单片机，PID，Infineon，TC377，神经网络控制
队伍名称：紫丁香三队
参赛队员：周京 孙雨欣 危波
带队教师：张依 王淑娟
第一章 绪 论
1.1 智能车研究背景
智能车是一种高新技术密集型的新型汽车，以汽车电子为背景涵盖控制、模式识别、传感技术、电子、电气、计算机、机械等多科学的科技创意性设计，一般主要由路径识别、速度采集、角度控制及车速控制等模块组成。可实现自动驾驶、自动变速及自动识别道路等功能，是现代电子产业发展中一项重要的组成部分。
在我国，教育部为了加强大学生实践、创新能力和团队合作精神的培养，委托教育部高等学校自动化专业教学指导分委员会主办了每年一度的全国大学生智能汽车竞赛。全国大学生智能汽车竞赛是在竞赛组委会提供的统一汽车模型平台上，使用微控制器作为核心控制模块，通过设计道路识别传感器和电机驱动电路、编写相应软件及装配模型车，制作一个能够自主识别道路的模型汽车，按照规定路线（路线赛前未知）行进，以完成时间最短者为优胜。
1.2 方案总体介绍
比赛环境在室外，只是使用电磁引导线，不铺设专用赛道表面。车模通过采集赛道电感值进行路径检测，比赛规则限定了传感器距离车模最后端的距离不超过40cm，并且规定了可使用的传感器类型。比赛规定了赛道元素的分数以及排布规则，参赛队员的目标是使智能车按在一定时间内所取得的分数最高。
根据竞赛规则相关规定，本智能车系统采用大赛组委会统一提供的L型车模，车模采用Infineon公司的TC377芯片作为控制器，通过AD口采集电感数据，并离线训练神经网络，后使用nnom部署到单片机上；通过编码器来检测车速，并通过单片机进行正交解码进行脉冲计算获得速度和路程；驱动电机采用PI控制，通过PWM控制驱动电路调整电机的功率；而车速的目标值由默认值、运行安全方案和神经网络的优化策略进行综合控制。
根据比赛的基本要求，我们设计了系统总体思路如图1.1所示。
▲ 图1.1 系统结构图
1.3 本文结构
本文共分为六章。
- 第一章节为绪论，主要介绍研究背景和总体方案。比赛的背景及智能车系统总体方案的介绍；
- 第二章从智能车系统的机械结构出发，详细阐述了智能车系统各部分机械结构的安装和调整；
- 第三章重点介绍了系统中所涉及的硬电路件设计方案和原理；
- 第四章是介绍了智能系统的软件算法包括神经网络以及电机舵机的控制策略；
- 第五章对调试过程中的一些手段进行了讲解；
- 第六章是总结和鸣谢。
本文最后部分附有相关部分程序
第二章 机械结构设计及调整
如果说控制策略和算法是智能车的大脑，那么机械结构就是它的身体躯干，前者能拉高赛车的下限，保证赛车能够平稳运行，顺利完成比赛，甚至还能不断逼近赛车的上限，超越自我；而后者则能直接决定赛车上限，好的机械结构能够保证赛车即使在高速运行的情况下依然能保持各方面的稳定，因此它也是限制赛车速度的巨大瓶颈。如果一辆赛车的程序架构很好，但是机械部分做的不好的话，其速度也会被大大的限制。即当车速较高的时候，车模有明显的甩尾和侧滑现象，此时对车的机械结构要求很高。除了对车身姿态的影响外，机械性能影响车的加减速响应速度，运行的对称性和稳定性等，因此，我们在不违反规则的情况下对车模进行了多方面的改造以使车模具有良好的运行性能。
2.1 整车布局
我们组采用的是新L车模，底盘为高强度尼龙加纤材料，坚固耐撞、不易变形。前后齿轮差速，车模改装以后长395mm，宽350mm，高175mm，重约1.5kg，全车滚珠轴承，避震器带调整弹簧松紧的调节环。通过对减震器进行松紧调节处理，车模具有极好的减震性能。电机是定制的LS-540SM马达，DC6V-12V，空载转速17500 rpm±10%,马达后轴已加长,可安装磁性（或光栅等）编码器。舵机采用的是L车数字升级版舵机：内部全金属齿轮，带防伪易碎标签，数字伺服器，工作电压4.8-7.4V, 力矩6.2-9kg，回中更准确，定位更精确, 动作速度≤0.16±0.02sec/60°。
2.2 传感器支架
车模上面最重要的传感器就是电感，保证它的稳定性至关重要。我们车上一共使用了13个电感，根据自己的车模定做了一个铝合金支架，并且通过碳杆来固定前后电磁杆，这样可以保证稳定性的同时便于调整和拆卸。支架如图2.1所示。
▲ 图2.1 电磁杆支架图
2.3 电池选型
原装车模使用的是NiCd电池，由于其容量较小，放电能力较弱，前期我们在使用的过程中经常感觉电量不太够用，并且车模起步时加速缓慢。本届比赛支持使用端口电压在 12.6 V 以内配有电池板护板的锂电池进行供电，经过多次比较和尝试，我们最终采用了航模锂电池作为电源，因为它的容量高，续航九，放电能力强可以轻松带动540电机，加减速响应快。电池如图2.2所示。
▲ 图2.2 带保护板的航模锂电池
2.4 转向舵机安装
舵机转向是整个控制系统中延迟较大的一个环节，为了减小此时间常数，通过改变舵机的安装位置可以提高舵机的响应速度。如果舵机调整不到位，将很大程度上限制转向角度和转向响应速度。但我们今年所使用的L车模默认安装方式为卧式安装，不可调整，因此不多赘述。我们的舵机安装如图2.3所示。
▲ 图2.3 转向舵机安装
2.5 前轮倾角调节
为了使汽车直线行驶稳定，转向轻便，转向后能自动回正，减少轮胎和转向系零件的磨损等，在转向轮、转向节和前轴之间须形成一定的相对安装位置，叫车轮定位。本系统所采用的智能车通过四条轮胎与地面接触，两个后轮同轴受到限位，无法调整，与智能车的前进方向保持平行，因此要改变智能车与地面的接触方式，调试出利于车转向、直线的四轮定位，只能通过调整前轮倾角各定位参数来实现。它的安装位置由主销内倾、主销后倾和前轮前束三个项目决定。
2.5.1 主销后倾、内倾
主销后倾如图2.4 所示，是指在纵向平面内主销轴线与地面垂直线之间的夹角，向前为负，向后为正。它在车辆转弯时会产生与车轮偏转方向相反的回正力矩，使车轮自动恢复到原来的中间位置上。所以，主销后倾角越大，车速越高，前轮自动回正的能力就越强，模型车通过增减黄色垫片的数量来改变 主销后倾角。
▲ 图2.4 主销后倾示意图
由于竞赛所用的转向舵机力矩不大，过大的主销后倾角会使转向变得沉重，转弯迟滞。
主销内倾是将主销（即转向轴线）的上端向内倾斜。从车前方看去，主销轴线与通过前轮中心的垂线之间形成一个夹角，即主销内倾角。轮胎调整为倾斜以后直线行走的时候是轮胎内侧着地，而当过弯的时候，由于惯性车体会要向弯道外侧倾斜，而这时候的外侧轮胎如果倾斜角度事先调整得当则正好可以胎面着地，从而使车辆在弯道获得最佳抓地力。使车轮转向后能及时自动回正和转向轻便。经过多次尝试后我们的主销后倾、内倾如图2.5所示。
▲ 图2.5主销后倾、内倾示意图
2.5.2 前轮前束
车轮前束如图2.6所示。是指两轮之间的后距离数值与前距离数值之差，也指前轮中心线与纵向中心线的夹角。从上往下看，两个车轮指向的方向在前端指向内称为车轮前束，指向外的则称为车轮后束。前轮前束的作用是保证汽车的行驶性能，减少轮胎的磨损。前轮在滚动时，其惯性力自然将轮胎向内偏斜，如果前束适当，轮胎滚动时的偏斜方向就会抵消，轮胎内外侧磨损的现象会减少。
▲ 图2.6 车轮前束示意图
2.6 底盘高度调节
降低车模底盘可以降低重心，车模重心低可以使车模运行更加稳定，获得 更好的转弯特性。所以，在保证车模可以通过灯盘的情况下，底盘尽可能的降低，可以使车更加快速稳定。对于L车模，想要降低底盘高度有两种方式，第一种是增加底盘配重并且调节减震器的软硬程度来拉低重心；第二种是通过外力限制前摆臂的摆动幅度从而拉低底盘。我们尝试了两种方案以后最终采取了第二种方式，如图2.7所示。
▲ 图2.7 底盘高度调节
第三章 硬件电路设计
硬件是基础，只有一个良好、稳定的硬件环境才能保证车能平稳快速的行驶。我们在整个系统设计过程中严格按照规范进行。本着可靠、高效的原则，在满足各个要求的情况下，尽量使设计的电路简单，PCB的效果简洁。
3.1 单片机系统设计
单片机最小系统是智能车系统的核心控制部件。我们采用了英飞凌公司的的TC377芯片。原理图如图3.1所示：
▲ 图3.1 单片机系统原理图
3.2 电源模块设计
电源模块为系统其他各个模块提供所需要的电源。设计中，除了需要考虑电压范围和电流容量等基本参数之外，还要在电源转换效率、降低噪声、防止干扰和电路简单等方面进行优化。可靠的电源方案是整个硬件电路稳定可靠运行的基础。
全部硬件电路的电源由一节航模锂电池提供（额定电压7.4V，满电电压8.4V）。由于电路中的不同电路模块所需要的工作电压和电流容量各不相同，因此电源模块应该包含多个稳压电路，将充电电池电压转换成各个模块所需要的电压。为满足需要，本车模上存在5种供电电压：
(1) 智能车使用锂电池供电，正常使用时电压在7.4～8.4V。可直接用于电机供电。
(2) 使用转压芯片TPS563231输出电压5V，主要用于红外传感器供电。原理图如图3.2.1所示
▲ 图3.2 转压电路7.2V-5V原理图
(3) 使用转压芯片TPS76833输出电压3.3V，用于OLED、蜂鸣器、单片机、WIFI等供电
▲ 图3.3 转压电路5V-3.3V原理图
(4) 使用开关稳压芯片MIC29302输出6V电压给舵机供电，用电池直接供电容易烧毁舵机，29302具有防止电流反灌功能有效提高舵机使用寿命。
▲ 图3.5 转压电路7.2V-6V原理图
(5) 使用稳压芯片LM2663来获得-3.3V电压。运放工作需要正负3.3V供电。
▲ 图3.6 转压电路3.3V—-3.3V原理图
3.3 电机驱动电路
在驱动芯片选择方面，我们选择BTN7971B芯片，BTN7971B芯片具有大电流、低阻抗、驱动能力强、高性能、发热小等众多优点，并且电路简单，容易布局连线。使用两片BTN7971B芯片就可以构成一个全桥驱动电路，如图3.7所示。
▲ 图3.7 左侧电机驱动原理图
3.4 电磁运放电路
采用简单的同向比例放大电路，电位计可用来调节放大倍数，通过对电磁信号进行放大和检波处理和可以直接供单片机的AD口读取。运放芯片使用性价比较高的OPA4171，3M的通带带宽足以满足20kHz的电感采集需求。
▲ 图3.8电磁信号检波放大原理图
3.5传感器的选择
3.5.1 电磁杆
车模需在铺设电磁线的轨道上行驶,导线内通有20KHZ的交变电流,从而在电磁线周围产生交变电场，电磁杆(电感电容)组成的LC振荡回路能够感应磁场,产生的感应电动势与电感与中心导线的距离有关。单片机采集所有电感传感器的AD值,通过偏差计算程序能够计算出小车与赛道中线的偏差,通过控制小车的姿态和速度,保证小车平稳的在赛道上自动行驶。以下为团队自行设计的电磁杆。
▲ 图3.9 设计完成的电磁杆
3.5.2 编码器
光电编码器是一种通过光电转换将输出轴上的机械几何位移量转换成脉冲或数字量的传感器, 这也是目前应用最多的测速传感器之一。其获取信息准确、精度高、应用简单。
采用增量式512线光电编码器，其供电电压为3.3V或5V，输出为小幅值的正弦信号。为了将此信号放大整形，设计了信号调理电路，其基本原理是使用一个运放做成比较器电路，调节参考电压，使输出变为0V-3.3V的方波信号，送入单片机进行运算。
3.6 主控板
在本系统中我们的电路板的制作主要思想是分立工作、追求简洁，便于电路板的调试和安装等工作。主控由电源稳压电路，OLED屏与按键交互电路，单片机系统以及各个传感器接口等组成。实物图如图3.10所示
▲ 图3.10 主控板实物图
3.7 驱动板
驱动板在除了原有的电机驱动电路外，还增加了编码器接口，主要目的是方便编码器的连线，以减少编码器连线过长导致的接触不良等问题，驱动板的实物图如图3.12
▲ 图3.11 驱动板实物图
3.8 运放板
使用运放板和电磁杆相连，电位器可用来调节放大倍数，通过对电磁信号进行放大和检波处理和可以直接供单片机的AD口读取。
▲ 图3.12 运放板实物图
第四章 系统软件设计及算法实现
高效稳定的控制程序和高适应性的算法是智能车快速平稳运行的基础。我们设计的智能车采用Infineon公司生产的TC377芯片作为控制器。其中TC377负责主要功能有跑车参数调节，电感采集，控制电机舵机。
对于本组赛题的特色——神经网络而言，我们采取离线训练训练的方式，即：采集数据后，在利用NNOM开源工具进行转换成单片机支持的C语言代码和权重信息，并部署到单片机上。具体方式将在本章进行详细阐述
4.1软件系统结构
底层方面，TC377侧采用FreeRTOS以支持便捷的多任务开发与切换。
▲ 图4.1 程序状态转换图
4.2神经网络控制
本本届赛题是将第十五届的电磁AI赛题与十四届的电磁越野赛题相结合，其主要的赛道元素与第十四届相似，比赛没有赛道，只有电磁线，赛道元素包括有直道、弯道、坡道、十字路口，是以折线为主。但允许使用的电磁前瞻长度却限制在40cm非常短，这就限制了参赛选手可以采用的方法。在逐飞给出的官方例程中，其使用神经网络进行辅助运算，但同十五届一样，依旧没有限制神经网络的使用形式。今年受到疫情影响，东北赛区的区赛取消了，直接同国赛一同举行，但省赛与国赛赛题差异较大，造成前期很多的调试只具有启发意义。我们仍尝试了很多可能的方案。尽管有些方案没有被采用，但仍对我们的其余调试具有启发意义。这里将简要介绍我们的完整尝试思路。
本节将分为以下几个部分进行介绍：
- 初步思路
- 数据采集和处理
- 离线训练与分析
- 部署
4.2.1 使用神经网络的初步思路
我们队只尝试了几种可能的神经网络方法，主要分为时间无关和时间相关的两类。大体包括：
- 时间无关方案
- 纯神经网络方案：使用神经网络，直接获得当前时刻的短前瞻电感-舵机对应关系。
- 神经网络混合方案：使用神经网络，获取当前时刻的短前瞻电感-长前瞻电感中线偏差信息，之后使用预测的长前瞻偏差输入类似十四届长前瞻车的PID控制模块。
- 赛道种类划分：使用神经网络，通过标注赛道种类，利用神经网络提取赛道特征，针对可能的直道和弯道进行特殊处理。
- 时间相关方案
- 纯神经网络方案–简化版本的TCN：利用滚动数组存取前一段时间的短前瞻电感信息，然后利用简化TCN计算舵机当前时刻转角。
- 神经网络混合方案–简化版本的TCN：利用滚动数组存取前一段时间的短前瞻电感信息，然后利用简化TCN预测当前时刻的长前瞻电感中线偏差。之后使用预测的长前瞻偏差输入类似十四届长前瞻车的PID控制模块。
- 赛道种类划分–简化版本的TCN：使用神经网络，通过标注赛道种类，利用神经网络提取赛道特征，针对可能的直道和弯道进行特殊处理。
经过实际测试，我们现在选择的是时间无关的神经网络混合控制方案，其相较其他方案更加稳定，且易操作。
下面将简要说明每种方案的大致想法和可能的优缺点
4.2.1.1 纯神经网络方案：
这个方案是最直接的神经网络控制方案，想法也很简单：通过神经网络来直接学习短前瞻电感值在不同赛道元素/舵机转角的特征。
- 但是缺点也显而易见：其缺乏必要的时间因果联系。例如传统的PID控制中，微分和积分部分可以看成时间相关的信息，这一点对于高速运行的车非常关键。
- 测试效果也不出所料：纯神经网络在低速时表现还算不错，但高速时抖动过大甚至直接出赛道，轨迹与采数是的轨迹相差较大。通常AI车速无法高过采集数据时的车速。
4.2.1.2 神经网络混合方案
由第一个方案的启发，我们想到可以融合神经网络和PID这两种方法。神经网络提取非时间相关的信息，如赛道的折线角度；而传统PID则用来处理时间相关的方案。
在这个方案中，我们最后决定使用神经网络来预测长前瞻获取的中线偏差，然后PID模块输入这个预测偏差，控制舵机输出。
实际测试表明，抖动仍然存在，这一抖动主要来自于偏差的计算方式导致其范围过大，但大多数的偏差集中在一个很小的范围内，其量化成单片机的整型数会造成较大的精度损失；以及神经网络的拟合程度不好。后续尝试更换网络结构，改善效果不明显。该方法较上一个方法操作更为复杂且改善效果不明显。
4.2.1.3 赛道种类划分
这是一个并行与前两个方案的一个特殊处理，即对于赛道种类划分：利用神经网络提取赛道特征，针对可能的直道，苯环，45度以及直角进行特殊处理。在采集数据的时候需要用磁标进行辅助，分类结果直接影响车模运行情况，由于模型的准确率有限，以及数据采集较为繁琐，该方法并未被采用。
4.2.1.4 纯神经网络方案–简化版本的TCN
这个也是由第一个方案所启发的。若在神经网络处理赛道电磁信号信息的同时，也使用神经网络来采集时间信息，可以尝试采用诸如RNN，LSTM等网络。但受限于部署方式（NNOM转换器对RNN类型网络的量化不尽如人意，效果相较于量化前差很多），我们最后采用了一种RNN的替代方案——TCN，并对其进行了一定的结构优化。
实际测试效果显示，跟踪效果在小转角下比较好，大转角处却不尽人意。这一方面将在第三节的“离线训练与分析”中进行详细解释。
4.2.1.5 神经网络混合方案–简化版本的TCN
若结合TCN和传统PID，便是这种混合方案。实验效果和纯TCN方案差不多，并无显著优势。
4.2.1.6 赛道种类划分–简化版本的TCN：
这是一个并行与前两个方案的一个特殊处理，即对于赛道种类划分：利用神经网络提取赛道特征，针对可能的直道，苯环，45度以及直角进行特殊处理。只不过在理想情况下，它可以推断赛道的先后出现顺序。
但是实际测试表明，其和时间无关的分类网络相比并无准确度上的优势。
4.2.2. 数据采集和处理
4.2.2.1 数据采集
数据采集分为手动数据和自动数据两大类。
- 手动数据：手动推车沿赛道运行，期间可包含一些异常情况的数据（例如模拟车辆跑偏之后回正等），以增加网络的鲁棒性。过程中采集长前瞻计算出的舵机打脚，偏差和短前瞻的电感读数。
- 自动数据：让车模在长前瞻电感的控制下自动沿着跑道运行，过程中采集长前瞻计算出的舵机打脚，偏差和短前瞻的电感读数。
4.2.2.2 数据处理
此处使用我们自己制作的转换脚本进行初步数据整理，具体包括：
- 自动将从串口收集的数据（字节形式）转换为数组
- 自动对数据来源进行分类（手动，自动）
- 自动对赛道类型进行分类，并制作热编码
- 自动制作数据集
- 自动分析数据范围，进行初步数据筛选（去除抖动数据），绘制相关图表。
使用方法如下：
调试过程中，可以得到类似的图标，方便调试分析。
▲ 图4.2 某次部分电感数据
▲ 图4.3某次舵机打脚数据
4.2.3. 离线训练与分析
在收集并初步处理完数据后，将其输入到Keras神经网络中。采用的版本是：
- Tensorflow 2.3.0
- Python3.7.10
下面介绍一种经过大量实验的方案：非时间相关的纯神经网络方案。
4.2.3.1. 时间无关的混合方案：DNN网络
模型机构采用的是最简单的全连接网络。
在训练过程中，持续跟踪曲线拟合情况。从历次的训练中挑选一个拟合度较好的版本。
▲ 图4.4 全连接网络的打乱顺序拟合情况
▲ 图4.5全连接网络的所有数据拟合情况
上图中：
- origin ：是实际的舵机打脚（滤波后结果）
- Dense：是使用神经网络训练后解算的预测舵机打脚（滤波后结果）
可以看到，全连接的拟合程度较高，但是曲线的毛刺较多，需要进行适当的滤波才能使用。
我们也尝试了其他的网络结构，但在车模上的实际测试表明，全连接的拟合程度最好。
我们也尝试过TCN的方案，结果是TCN的预测结果抖动较大，大角度是会出现预测值绝对值比实际值偏小的现象，且有一定程度的“提前预测的现象”。在实际跑车测试过程中，效果不如全连接的混合方案理想，故当前没有采用。
在未来，我们期望能够更充足的时间，进行相关的测试，继续完善TCN网络。
4.2.4. 部署
部署采用的是NNOM模型转换器。NNOM模型转换器对于DNN和CNN网络的部署精度损失较小，其部署到单片机的模型与神经网络训练出来的模型的准确率几乎一致，甚至更高，但对于RNN类网络量化效果则不尽如人意。
▲ 图4.6神经网络拟合情况
▲ 图4.7经量化后的数据拟合情况
在实际使用过程中，可通过scons模块利用main.c在PC端直接查看部署的神经网络准确度，并通过数据及图表的形式展示出来，其应用效果及查错效果较好，可以节省时间，提高调车效率。
使用方法如下：
▲ 图4.8使用scons在PC端查看MCU部署结果4.3 PID控制
在工程实际中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制，又称PID调节。在通过神经网络输出前瞻后，我们使用了PID控制算法控制舵机、电机的输出。
PID控制器问世至今已有近70年历史，它以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控
制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，最适合用PID控制技术。PID控制，实际中也有PI和PD控制。
PID控制器是一种线性控制器，它根据给定值与实际输出值构成控制偏差。将偏差的比例§、积分(I)和微分(D)通过线性组合构成控制量，对被控对象进行控制，故称PID控制器，原理框图如图4.19所示。
▲ 图4.19 PID控制器原理框图
4.3.1 转向舵机的PD控制算法
对于舵机的闭环控制，我们采用了位置式PD控制算法，根据往届的技术资料和实际测试，通过电感的差值比上电感的积来算得车模距离中线的偏差。
经过反复测试，我们选择的PD调节策略是：
(1) 将微分项系数置零，单独调节Kp，发现在1.0m/s以下单独调节Kp就能取得一个良好的跑车效果；
(2)1.5m/s以上微分项系数Kd随速度增大而增大，原因是速度越快舵机在一般赛道中越需要较好的动态响应能力；
最终我们选择了一组PD参数，得到了较为理想的转向控制效果。
4.3.2 驱动电机的PI控制算法
对于速度控制，我们采用了增量式PI控制算法，基本思想是直道加速，弯道减速。经过反复调试，将采集到的电感值与参考速度值构成线性关系。在实际测试中，我们发现能够快速稳定的通过折弯而冲出赛道较少。
4.4 PID控制与AI控制的结合
对于传统的PID控制，经过多年以来工业的验证，其稳定性值得肯定，具体表现在当车模距离电磁线较远即将冲出赛道时能够比较稳定的控制车模重新运行到赛道上来，而PID控制也有其局限性，其数学模型是通过人工建立起来的，数学模型的好坏直接决定了控制效果的上限。而AI模型是通过数据驱动的方式建立起来的数学模型，其模型好坏更取决于数据的质量，对于其模型我们只需给出其基本结构。这样的优势是直接连接了输入量与输出量，使输出量对输入量更加敏感，能够比人构建出来的数学模型更加灵敏；但是正是由于其对数据敏感的特性，我们暂时没有经历仿真出所有可能出现的情况，这导致其对数据盲区有不可控的随机性，因此如何利用好这两者的优点是有一个很重要的问题。在我们车上的具体表现是靠PID算法运行的轨迹并没有AI算法得到的轨迹好，但是AI会在没有数据的地方出现不稳定性，例如冲出赛道。为此我们尝试了很多种方法，最终综合了我们车模在运行中的实际情况，在不同赛道类型中，给与了PID计算结果和AI计算结果不同的权重，达到了既改善轨迹又能保证一定稳定性的效果。
第五章 系统开发和调试工具
5.1开发环境：AURIX Development Studio
AURIX Development Studio为开发人员带来了一个易于使用的基于eclipse的开发环境，适用于基于Infineon的MCU，包括TC377微控制器。AURIX Development Studio提供高级编辑、编译和调试功能，增加了MCU特定的调试视图、代码跟踪和分析、多核调试等功能。
▲ 图 5.1 AURIX Development Studio的主界面
5.2上位机调试
为了获取车模在行驶时的电感数据，我们在车上装了WiFi，可以将电感数据实时发送到上位机采集，再通过神经网络训练。上位机我们使用系统自带的的串口调试助手。
▲ 图 5.2 串口调试助手界面
第六章 总结
本文从机械、硬件、软件等方面详细介绍了车模的设计和制作方案。从机械和硬件方面，我们力求电路的稳定可靠，尽量将车模做的轻快灵活，尽可能降低重心；在软件方面，我们努力追求最有效的行驶路径，不断优化各方面策略。自从参加智能车竞赛以来，我们小队都积极查阅资料，学习提高自己的能力，为智能车制作奉献了许多时间和经力。
在车模制作过程中，我们学到很多知识，逐渐对单片机控制、电路设计等有了比较好的认识，很感谢社团能够给我们这样的平台，感谢各位指导老师和学长学姐的指导。经过近一年的辛苦准备，我们也积累了很多经验，深刻认识到坚持不懈的重要性，经过一次次残酷的校内选拔，从开始的近百人到最后的二十几人，考验的不是谁更聪明，而是谁可以坚持努力，不断地发现问题解决问题，坚持为比赛付出时间和精力。
最后，再次感谢哈工大智能车创新俱乐部这个大家庭，感谢一直支持和关注智能车比赛的学校和学院领导以及各位老师，也感谢比赛组委会能组织这样一项很有意义的比赛。
参考文献
[1] 卓晴，黄开胜，邵贝贝．学做智能车 [M]．北京：北京航空航天大学出版社．2007.
[2] 王淑娟，蔡惟铮，齐明．模拟电子技术基础 [M]．北京：高等教育出版社．2009
[3] 张军．AVR单片机应用系统开发典型实例 [M]．北京：中国电力出版社，2005.
[4] 张文春．汽车理论 [M]．北京．机械工业出版社．2005.
[5] 殷剑宏，吴开亚．图论及其算法 [M] ．中国科学技术大学出版社，2003.
[6] 夏克俭．数据结构及算法 [M] ．北京：国防工业出版社， 2001.
[7] 邵贝贝．单片机嵌入式应用的在线开发方法 [M]．北京．清华大学出版社．2004.
[8] 蔡述庭．“飞思卡尔”杯智能汽车竞赛设计与实践 [M]．北京：北京航空航天大学出版社. 2012.
■ 附录1 部分核心代码
本节包含了该智能车的部分核心代码
- 主程序入口相关函数代码Main.c
- 控制部分代码 Control.c
● 附录1.1. Main.c
/**********************************************************************************************************************
* \file Cpu0_Main.c
* \copyright Copyright (C) Infineon Technologies AG 2019
*
* Use of this file is subject to the terms of use agreed between (i) you or the company in which ordinary course of
* business you are acting and (ii) Infineon Technologies AG or its licensees. If and as long as no such terms of use
* are agreed, use of this file is subject to following:
*
* Boost Software License - Version 1.0 - August 17th, 2003
*
* Permission is hereby granted, free of charge, to any person or organization obtaining a copy of the software and
* accompanying documentation covered by this license (the "Software") to use, reproduce, display, distribute, execute,
* and transmit the Software, and to prepare derivative works of the Software, and to permit third-parties to whom the
* Software is furnished to do so, all subject to the following:
*
* The copyright notices in the Software and this entire statement, including the above license grant, this restriction
* and the following disclaimer, must be included in all copies of the Software, in whole or in part, and all
* derivative works of the Software, unless such copies or derivative works are solely in the form of
* machine-executable object code generated by a source language processor.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
* WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE
* COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN
* CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
* IN THE SOFTWARE.
*********************************************************************************************************************/
#include<09_25_DNN_tmstp_1_nobn_tr40_epo10_bs4096_feichanghaoongyuanhuan.h>
#include <17_34_DNN_tmstp_1_nobn_tr30_epo10_bs16384_wentixiuao.h>
#include "Cpu0_Main.h"
#include "headfile.h"
#include "Menu.h"
#include "Init.h"
#include "control.h"
#include "SmartCar_FONT.h"
#include "ADC.h"
#pragma section all "cpu0_dsram"
//将本语句与#pragma section all restore语句之间的全局变量都放在CPU0的RAM中
//nnom_model_t* model;
//extern const unsigned char model1[];
//typedef struct _model_info_struct
//{
// char author[30];
// int quant_bits;
// int frac_bits;
// const char* name_string;
//}model_info_struct;
//model_info_struct inf;
//extern void* run_model(const void *model_buf, const void *in_buf, signed short int *out1_buf);
//extern void get_model_info(const void *in_buf, model_info_struct *inf);
Mpu_t* this_mpu;
#if !defined (SIIC_MPU_USE_BUTTON)
#define SIIC_MPU_USE_BUTTON (1U)
#endif
#if !defined (SEND_AD_LONG_BUTTON)
#define SEND_AD_LONG_BUTTON (0U)
#endif
#if !defined (RAMP_DISTANCE_TYPE)
#define RAMP_DISTANCE_TYPE (0U)
#endif
#define RATE 0.8f
//电感采集有关的宏定义
#if !defined (RUN_AI_BUTTON)
#define RUN_AI_BUTTON (0U)
#endif
//电感采集有关的宏定义
nnom_model_t* model;
nnom_model_t* model_a;
static float ai_data[AI_NUM]={0};
static float ai_data_last[AI_NUM]={0};
//static float send_encoder[17]={0};
static uint8 num_times=0;
int core0_main(void)
{
get_clk();//获取时钟频率 务必保留
//用户在此处调用各种初始化函数等
//等待所有核心初始化完毕
IfxCpu_emitEvent(&g_cpuSyncEvent);
IfxCpu_waitEvent(&g_cpuSyncEvent, 0xFFFF);
//用户在此处调用各种初始化函数等
//屏幕初始化
oled_init();
//GPIO初始化，20.10dididi
gpio_init(P20_10, GPO, 0, PUSHPULL);
gpio_init(P21_4, GPO, 1, PUSHPULL);
gpio_init(P20_9, GPO, 1, PUSHPULL);
gpio_init(P00_4, GPO, 1, PUSHPULL);
gpio_init(P02_1, GPO, 1, PUSHPULL);
gpio_init(P02_2, GPO, 1, PUSHPULL);
// gpio_init(Ultrasonic_Send_PIN, GPO, 0, PUSHPULL);
// gpio_init(Ultrasonic_Recive_PIN, GPI, 0, NO_PULL);
//PWM初始化
PWM_init();
//定时中断初始化
PIT_init();
//编码器初始化
Encoder_init();
//串口初始化
Uart_init();
//电感初始化
elec_init();
//陀螺仪初始化
#if defined (SIIC_MPU_USE_BUTTON) && (SIIC_MPU_USE_BUTTON == 1)
icm20602_init_spi();
this_mpu = &my_mpu;
SmartCar_ACCOffset(this_mpu);
SmartCar_GyroOffset(this_mpu);
#endif
// Read_Max();
CreatMenu();
Read_flash();
DataUpdate();
// oled_dis_bmp(134,143,gImage_caicai,150);
// systick_delay_ms(STM0, 1000);
PrintMenu();
if(!gpio_get(P02_3))
{
gpio_set(P20_9, 1);
Read_AD(save_place_flag);
}
else
{
gpio_set(P20_9, 0);//亮代表存最大值不读
}
//AI初始化
// get_model_info(model1, &inf);
enableInterrupts();
#if defined (RAMP_DISTANCE_TYPE) && (RAMP_DISTANCE_TYPE == 1U)
systick_start(STM0);
#endif
while (TRUE)
{
key_start();
#if defined (RAMP_DISTANCE_TYPE) && (RAMP_DISTANCE_TYPE == 1U)
uint32_t now_time = systick_getval(STM0);
#endif
// float var_1[20]={0};
// var_1[0]=(float)curve_distance.encoder_num/10.0f;
//// var_1[1]=x_position;
//// var_1[2]=y_position;
//// var_1[3]=Circle_angle.acc_of_complet_round_angle;
// SmartCar_VarUpload(var_1,1);
//#if defined (SEND_AD_LONG_BUTTON) && (SEND_AD_LONG_BUTTON == 0U)
// if(AI_process_flag==0&&short_process_flag==0)//改菜单
// {
// Collect_data();
//
// }
//#endif
//wifi调参
// uint8 data;
//
// if(uart_query(UART_2,&data))
// {
// if(data==0x01)
// {
//
// SmartCar_VarUpload(&encoder_remember,17);
stop();
}
}
//PIT中断函数 示例
IFX_INTERRUPT(cc60_pit_ch0_isr, CCU6_0_CH0_INT_VECTAB_NUM, CCU6_0_CH0_ISR_PRIORITY)
{
enableInterrupts();//开启中断嵌套
#if defined (SEND_AD_LONG_BUTTON) && (SEND_AD_LONG_BUTTON == 0U)
systick_start(STM1);
switch(AI_process_flag)
{
case PID_PROCESS :
if(short_process_flag==0)
{
Long_Control();
}else
{
Short_Control();
if(type_of_shortpid_road==0)
{
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
ai_data[k]=RATE*(AD_Short[k]-128.0f)+(1-RATE)*ai_data_last[k];
ai_data_last[k]=ai_data[k];
}
if(in_the_round==0)
{
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
nnom_input_data[k]=(int8_t)((int32_t)ai_data[k]);
}
// nnom_input_data[11]=0;
nnom_input_data[12]=0;
// nnom_input_data[13]=0;
// model_run(model);
// servo_p = nnom_output_data[0];
}else
{
// memcpy(nnom_input_data_a, ai_data, sizeof(nnom_input_data_a));
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
nnom_input_data_a[k]=(int8_t)((int32_t)ai_data[k]);
}
nnom_input_data_a[11]=0;
nnom_input_data_a[12]=0;
nnom_input_data_a[13]=0;
model_run(model_a);
servo_p = nnom_output_data_a[0];
// gpio_set(P20_10,1);
}
}
}
// systick_delay_ms(STM0,1);
break;
case AI_PROCESS :
NNOM_process();
// AI_recognize_road();
type_of_ai_road=0;
if(type_of_ai_road==0)
{
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
ai_data[k]=RATE*(AD_Short[k]-128.0f)+(1-RATE)*ai_data_last[k];
ai_data_last[k]=ai_data[k];
}
if(in_the_round==0)
{
// memcpy(nnom_input_data_a, ai_data, sizeof(nnom_input_data_a));
// model_run(model_a);
// servo_p = nnom_output_data_a[0];
// memcpy(nnom_input_data, ai_data, sizeof(nnom_input_data));
//正常程序
// for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
// {
// nnom_input_data[k]=(int8_t)((int32_t)ai_data[k]);
// }
//// nnom_input_data[11]=0;
// nnom_input_data[12]=0;
//// nnom_input_data[13]=0;
// model_run(model);
//#if defined(AI_ERROR_FLAG) && (AI_ERROR_FLAG== 0)
// servo_p = nnom_output_data[0];
//圆环测试程序
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
nnom_input_data_a[k]=(int8_t)((int32_t)ai_data[k]);
}
// nnom_input_data_a[11]=0;
nnom_input_data_a[12]=0;
// nnom_input_data_a[13]=0;
model_run(model_a);
servo_p = nnom_output_data_a[0];
//#elif defined(AI_ERROR_FLAG) && (AI_ERROR_FLAG!= 0)
// error_p = nnom_output_data[0];
//#endif
}else
{
// memcpy(nnom_input_data_a, ai_data, sizeof(nnom_input_data_a));
for(uint8 k=0;k<USE_SHORT_AD_NUM;k++)
{
nnom_input_data_a[k]=(int8_t)((int32_t)ai_data[k]);
}
// nnom_input_data_a[11]=0;
nnom_input_data_a[12]=0;
// nnom_input_data_a[13]=0;
model_run(model_a);
servo_p = nnom_output_data_a[0];
// gpio_set(P20_10,1);
}
}
break;
}
//检测坡道距离
#if defined (RAMP_DISTANCE_TYPE) && (RAMP_DISTANCE_TYPE == 0U)
ramp_distance=Get_distance(ADC_8,ADC8_CH11_A43,ADC_12BIT);
if(ramp_use_flag==1)
{
// if(recognize_ramp_times==0)
// {
{
ramp_flag=1;
}
// }
}
#endif
run_time =(((float)systick_getval(STM1))/10000);
#endif
PIT_CLEAR_FLAG(CCU6_0, PIT_CH0);
}
IFX_INTERRUPT(cc60_pit_ch1_isr, CCU6_0_CH1_INT_VECTAB_NUM, CCU6_0_CH1_ISR_PRIORITY)
{
enableInterrupts();//开启中断嵌套
if(out_road_flag==1)
{
out_times++;
}else
{
out_times=0;
}
#if defined (SIIC_MPU_USE_BUTTON) && (SIIC_MPU_USE_BUTTON == 1)
// get_icm20602_accdata_spi(this_mpu);
get_icm20602_gyro_spi(this_mpu);
float gyron_z=(float)(this_mpu->mpu_rawdata.gyro_z)*2000.0/32768.0;
if(type_of_shortpid_road==60||type_of_shortpid_road==70)
{
Circle_angle.acc_of_complet_round_angle+=gyron_z*5*0.001f;
if(fabsf(Circle_angle.acc_of_complet_round_angle)>=600.0f)
{
Circle_angle.acc_of_complet_round_angle=600.0f;
}
}
if(in_the_round==1)
{
AI_angle.acc_of_complet_round_angle+=gyron_z*5*0.001f;
if(fabsf(AI_angle.acc_of_complet_round_angle)>600.0f)
{
AI_angle.acc_of_complet_round_angle=600.0f;
}
}
#endif
PIT_CLEAR_FLAG(CCU6_0, PIT_CH1);
}
IFX_INTERRUPT(cc61_pit_ch0_isr, CCU6_1_CH0_INT_VECTAB_NUM, CCU6_1_CH0_ISR_PRIORITY)
{
enableInterrupts();//开启中断嵌套
if(AI_process_flag==1)
{
Servo_AI();
}else if(short_process_flag==1)
{
Servo_PID_Short();
}else
{
Servo_PID();
}
PIT_CLEAR_FLAG(CCU6_1, PIT_CH0);
}
IFX_INTERRUPT(cc61_pit_ch1_isr, CCU6_1_CH1_INT_VECTAB_NUM, CCU6_1_CH1_ISR_PRIORITY)
{
enableInterrupts();//开启中断嵌套
Moto_process();
PIT_CLEAR_FLAG(CCU6_1, PIT_CH1);
}
#pragma section all restore
Mpu_t my_mpu;
float run_time;
void elec_init(void)//ADC初始化
{
adc_init(ADC_1, ADC1_CH5_A13);
adc_init(ADC_1, ADC1_CH4_A12);
adc_init(ADC_1, ADC1_CH3_A11);
adc_init(ADC_1, ADC1_CH2_A10);
adc_init(ADC_1, ADC1_CH0_A8);
adc_init(ADC_0, ADC0_CH7_A7);
adc_init(ADC_0, ADC0_CH6_A6);
adc_init(ADC_0, ADC0_CH5_A5);
adc_init(ADC_0, ADC0_CH4_A4);
adc_init(ADC_0, ADC0_CH3_A3);
adc_init(ADC_0, ADC0_CH2_A2);
adc_init(ADC_0, ADC0_CH1_A1);
adc_init(ADC_2, ADC2_CH0_A16);
adc_init(ADC_2, ADC2_CH1_A17);
adc_init(ADC_2, ADC2_CH4_A20);
adc_init(ADC_2, ADC2_CH5_A21);
adc_init(ADC_3, ADC3_CH0_A24);
adc_init(ADC_3, ADC3_CH1_A25);
adc_init(ADC_8, ADC8_CH3_A35);
adc_init(ADC_8, ADC8_CH4_A36);
adc_init(ADC_8, ADC8_CH5_A37);
adc_init(ADC_8, ADC8_CH6_A38);
adc_init(ADC_8, ADC8_CH7_A39);
adc_init(ADC_8, ADC8_CH10_A42);
adc_init(ADC_8, ADC8_CH11_A43);
adc_init(ADC_8, ADC8_CH12_A44);
// typedef enum // 枚举ADC通道
// {
// //ADC0可选引脚
// ADC0_CH0_A0 = 0*16 + 0,
// ADC0_CH1_A1,
// ADC0_CH2_A2,
// ADC0_CH3_A3,
// ADC0_CH4_A4,
// ADC0_CH5_A5,
// ADC0_CH6_A6,
// ADC0_CH7_A7,
//
// //ADC1可选引脚
// ADC1_CH0_A8 = 1*16 + 0,
// ADC1_CH2_A10 = 1*16 + 2,
// ADC1_CH3_A11,
// ADC1_CH4_A12,
// ADC1_CH5_A13,
//
// //ADC2可选引脚
// ADC2_CH0_A16 = 2*16 + 0,
// ADC2_CH1_A17 = 2*16 + 1,
// ADC2_CH4_A20 = 2*16 + 4,
// ADC2_CH5_A21 = 2*16 + 5,
//
// //ADC3可选引脚
// ADC3_CH0_A24 = 3*16 + 0,
// ADC3_CH1_A25 = 3*16 + 1,
//
// //ADC8可选引脚
// ADC8_CH3_A35 = 8*16 + 3,
// ADC8_CH4_A36,
// ADC8_CH5_A37,
// ADC8_CH6_A38,
// ADC8_CH7_A39,
// ADC8_CH10_A42 = 8*16 + 10,
// ADC8_CH11_A43,
// ADC8_CH12_A44,
// ADC8_CH13_A45,
// ADC8_CH14_A46,
// ADC8_CH15_A47,
// }VADC_CHN_enum;
}
//uint8 channel_name[8]={ADC2_CH4_A36,ADC0_CH3_A3,ADC1_CH8_A24,ADC0_CH5_A5,ADC1_CH5_A21,ADC0_CH7_A7,ADC1_CH1_A17,ADC0_CH1_A1};
//uint8 ai_data_flag;//1：ad数据采集完成 0：ad数据未采集完成
//int8 ad_data[7]; //ad数据
//void ai_data(void)//采集adc数据
//{
// ad_data[0] = (int16)ADC_Get(ADC_0, ADC0_CH0_A0, ADC_8BIT) - 128;
// ad_data[1] = (int16)ADC_Get(ADC_0, ADC0_CH1_A1, ADC_8BIT) - 128;
// ad_data[2] = (int16)ADC_Get(ADC_0, ADC0_CH2_A2, ADC_8BIT) - 128;
// ad_data[3] = (int16)ADC_Get(ADC_0, ADC0_CH3_A3, ADC_8BIT) - 128;
// ad_data[4] = (int16)ADC_Get(ADC_0, ADC0_CH4_A4, ADC_8BIT) - 128;
// ad_data[5] = (int16)ADC_Get(ADC_0, ADC0_CH5_A5, ADC_8BIT) - 128;
// ad_data[6] = (int16)ADC_Get(ADC_0, ADC0_CH6_A6, ADC_8BIT) - 128;
// ai_data_flag = 1;
//}
//
void PWM_init(void)
{
gtm_pwm_init(Servo_PIN, 50, (uint32)(servo_mid*100));
// gtm_pwm_init(Servo_PIN, 50, (uint32)(servo_mid*100));
gtm_pwm_init(Motor_PIN_0, 13000, 0);
gtm_pwm_init(Motor_PIN_1, 13000, 0);
// SmartCar_Gtm_Pwm_Init(&IfxGtm_ATOM0_0N_TOUT54_P21_3_OUT,50, 0);
// SmartCar_Gtm_Pwm_Init(&Motor_PIN_1, 11000, 0);
// SmartCar_Gtm_Pwm_Init(&Motor_PIN_0, 11000, 0);
}
//
void PIT_init(void)
{
pit_interrupt_ms(CCU6_1, PIT_CH1, 5);//电机
pit_interrupt_ms(CCU6_1, PIT_CH0, 5);//舵机
pit_interrupt_ms(CCU6_0, PIT_CH1, 5);//陀螺仪
pit_interrupt_ms(CCU6_0, PIT_CH0, 2.2);//算偏差
}
//
void Encoder_init(void)
{
gpt12_init(GPT12_T2, GPT12_T2INB_P33_7, GPT12_T2EUDB_P33_6);
// SmartCar_Encoder_Init(GPT12_T2, IfxGpt120_T2INB_P33_7_IN, IfxGpt120_T2EUDB_P33_6_IN);
}
//
////void Eru_init(void)//外部中断初始化
////{
//// Eru_Init(CH6_P00_4, RISING);//发车启动中断
////}
//
void Uart_init(void)
{
uart_init(UART_2,921600,UART2_TX_P10_5,UART2_RX_P10_6);
// SmartCar_Uart_Init(IfxAsclin2_TX_P10_5_OUT,IfxAsclin2_RXD_P10_6_IN,921600,2);
}
//
//void MPU_Init(void)
//{
//
//}
附录1.2. Control.c
/*
* control.c
*
* Created on: 2021年5月30日
* Author: HP
*/
#include"control.h"
#include"ADC.h"
#include"stdlib.h"
#ifndef SERVO_USE_FLAG
#define SERVO_USE_FLAG (1U)
#endif
//全局变量flag
uint8 Stop_flag=0;
uint8 ramp_use_flag=0;
uint8 recognize_ramp_times=0;
float pwm_ai=0;
//写进菜单可调
uint8 AI_process_flag=0;
uint8 nnom_process_flag=1;
uint8 short_process_flag=0;
uint8 collect_max_flag=0;
uint8 acc_stop_flag=0;
uint32 out_times=0;
//全局参量
Speed_Control Up_and_down_speed,ai_or_pid_change;
Encoder Stop,Stop_Menu,ramp_min_distance,First_distance;
ANGLE_CONTROL AI_angle,Circle_angle;
Speed_Set Nomal_Speed,Out_Speed,Ramp_Speed,Round_Speed,Stop_Speed,Curve_Speed;
error_ Long_err,Short_err,Short_err_PID,Short_err_PID_cross,AI_round_err,MOTO_err,AI_normal_err,AI_servo,Circle_err;
uint8 up_speed_flag=1;
uint32 save_place_flag=1;
PID_Servo_Type Moto_PI,PID_long_normal,PID_Long_cross,PID_short_round_low,PID_short_round,PID_Short,PID_Short_Cross,PID_AI,AI_Servo,Direction_Servo;
servo_angle Long_servo_angle;
float pwm_moto = 0;
float servo_mid = 7.2;
float pwm_servo=7.2;
int8 servo_p=0;
int8 error_p=0;
float round_servo_into_l= 0.7;
float round_servo_into_r = 0.7;
//坡道距离
float ramp_distance=0;
float circle_center_x=0;
float distance_to_center=0;
//静态变量
uint8_t complet_speed_change_times = 0;
//static uint8_t complet_aipid_change_times = 0;
uint8 menu_complet_speed_times=20;
static float Moto_Speed_Goal=0;
static float Moto_Speed_Real=0;
static float Error_Moto_Now;//电机现在error
static float Error_Moto_Last;//电机前一个error
static float pwm_moto_add = 0;
static float pwm_servo_variation=0;
uint8 complet_circle_times=0;
uint8 complet_curve_times=0;
float pwm_servo_pid=0;
float pwm_servo_ai=0;
uint8 round_pid_flag=0;
float encoder_remember[20]={0};
adrc_td_t filter;
uint8 circle_control_start=0;
double x_position=0,y_position=0;
void ADRCCTRL_TD(adrc_td_t* td, float v)
{
float fv = ADRCCTRL_Fhan(td->v1 - v, td->a1, td->r, td->h);
td->v1 += td->h * td->a1;
td->a1 += td->h * fv;
}
float ADRCCTRL_Fhan(float v1, float v2, float r0, float h0)
{
float d = h0 * h0 * r0;
float a0 = h0 * v2;
float y = v1 + a0;
float a1 = sqrtf(d*(d + 8.0f*fabsf(y)));
float a2 = a0 + ADRCCTRL_Sign(y)*(a1-d)*0.5f;
float sy = (ADRCCTRL_Sign(y+d) - ADRCCTRL_Sign(y-d))*0.5f;
float a = (a0 + y - a2)*sy + a2;
float sa = (ADRCCTRL_Sign(a+d) - ADRCCTRL_Sign(a-d))*0.5f;
return -r0*(a/d - ADRCCTRL_Sign(a))*sa - r0*ADRCCTRL_Sign(a);
}
float ADRCCTRL_Sign(float val)
{
if(val >= 0.0f)
return 1.0f;
else
return -1.0f;
}
void stop(void)
{
Stop_flag=0;
Stop.encoder_num=0;
First_distance.encoder_num=0;
out_times=0;
complet_speed_change_times=0;
gpio_set(P00_4, 1);
gpio_set(P02_1, 0);
gpio_set(P02_2, 0);
}
//编码器调试距离，正常跑车时关闭
// float send_encoder=(float)First_distance.encoder_num/10;
// SmartCar_VarUpload(&send_encoder,1);
}
float F_ABS(float f_var)
{
if(f_var<0)
{
f_var= - f_var;
}
return f_var;
}
void Moto_process(void)
{
if(Stop_flag == 0)
{
Moto_Speed_Goal = 0;
Stop_flag = 0;
} else
{
//圆环和坡道
if(ramp_flag==0&&in_the_round==0&&type_of_shortpid_road!=60)
{
Moto_Speed_Goal=Get_speed_goal();
}else if(in_the_round==1)
{
Moto_Speed_Goal=Round_Speed.speed;
}else if(ramp_flag==1)
{
Moto_Speed_Goal=Ramp_Speed.speed;
}else if(type_of_shortpid_road==60)
{
Moto_Speed_Goal=Curve_Speed.speed;
}
}
int16 ecoder_num=-gpt12_get(GPT12_T2);
gpt12_clear(GPT12_T2);
Moto_Speed_Real = 0.0001 * ((float)ecoder_num) * Meter_every_Round / 0.005;
//清除编码器
Error_Moto_Now = (Moto_Speed_Goal - Moto_Speed_Real);
pwm_moto_add = (( Moto_PI.KP * (Error_Moto_Now - Error_Moto_Last)) + (Moto_PI.KD * Error_Moto_Now));
Error_Moto_Last = Error_Moto_Now;
pwm_moto = (pwm_moto + pwm_moto_add);
//电机限幅
if(pwm_moto >= LIMIT_MOTO)
{
pwm_moto = LIMIT_MOTO;
}
//去掉反向占空比
else if(pwm_moto<=0)
{
pwm_moto = 0;
if(pwm_moto>=0)
{
pwm_duty(Motor_PIN_1, 0);
pwm_duty(Motor_PIN_0,(uint32)(pwm_moto*100));
}else
{
pwm_duty(Motor_PIN_1, (uint32)(-pwm_moto*100));
pwm_duty(Motor_PIN_0,0);
}
if(Recognize_Round.encoder_num>=ROUND_ENCODER_NUM)
{
Recognize_Round.encoder_num=ROUND_ENCODER_NUM;
}else{
Recognize_Round.encoder_num+=ecoder_num;
}
if(ramp_flag==1)
{
ramp_min_distance.encoder_num+=ecoder_num;
if(ramp_min_distance.encoder_num>ramp_infrared_judge.encoder_of_ramp)
{
ramp_flag=0;
ramp_min_distance.encoder_num=0;
}
}
}
double D2R(float deg)
{
double rad=(deg / 180.0) * 3.14159265;
return rad;
}
void Servo_PID_Short(void)
{
#if defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==1)
AI_servo.error_now=(((float)servo_p)*1.8f)/127.0f;
pwm_servo_ai=1.0f*AI_servo.error_now+AI_Servo.KD*(AI_servo.error_now-AI_servo.error_last);
AI_servo.error_last=AI_servo.error_now;
if(cross_flag==1)
{
pwm_servo_pid = PID_Short_Cross.KP * Short_err_PID_cross.error_now + PID_Short_Cross.KD*(Short_err_PID_cross.error_now - Short_err_PID_cross.error_last );
Short_err_PID_cross.error_last=Short_err_PID_cross.error_now;
}
else if(in_the_round==0)//&cross_times%2==0
{
pwm_servo_pid = PID_Short.KP * Short_err_PID.error_now + PID_Short.KD*(Short_err_PID.error_now - Short_err_PID.error_last );
Short_err_PID.error_last =Short_err_PID.error_now ;
}else if(in_the_round==1)
{
pwm_servo_pid = PID_short_round.KP * Short_err_PID.error_now + PID_short_round.KD*(Short_err_PID.error_now - Short_err_PID.error_last );
Short_err_PID.error_last =Short_err_PID.error_now ;
}
if(type_of_shortpid_road==0)
{
if(pid_only_flag==0)
{
if(in_the_round==0)
{
//
// gpio_set(P20_10,0);
if(abs(servo_p)<20)
{
pwm_servo_variation = pwm_servo_pid;
}
else
{
pwm_servo_variation = pwm_servo_ai;
}
}else if(Up_and_down_speed.ai_and_pid_flag[complet_speed_change_times]==2)
{
//
// gpio_set(P20_10,1);
pwm_servo_variation = pwm_servo_ai;
}else
pwm_servo_variation = pwm_servo_ai;
}
}
}else
{
//
if(abs(servo_p)<20)
{
pwm_servo_variation = pwm_servo_pid;
//
// gpio_set(P20_10,0);
}
else
{
pwm_servo_variation = pwm_servo_ai;
//
// gpio_set(P20_10,1);
}
}
}else
{
if(round_pid_flag==0)
{
if(abs(servo_p)<20)
else
{
pwm_servo_variation = pwm_servo_ai;
}
}else if(round_pid_flag==1)
{
pwm_servo_variation = pwm_servo_pid;
}else if(round_pid_flag==2)
{
pwm_servo_variation = pwm_servo_ai;
}
}
}
pwm_servo = servo_mid-pwm_servo_variation;
}
// 丢线保护
else if(type_of_shortpid_road==20||type_of_shortpid_road==30)
{
pwm_servo=servo_mid+LIMIT_SE;
}else if(type_of_shortpid_road==21||type_of_shortpid_road==31)
{
pwm_servo=servo_mid-LIMIT_SE;
}
else if(type_of_shortpid_road==50)
{
if(left_round!=0&&right_round==0)
{
pwm_servo =servo_mid+round_servo_into_l;
// gpio_set(P00_4, 0);
// gpio_set(P02_1, 1);
// gpio_set(P02_2, 0);
}else if(left_round==0&&right_round!=0)
{
pwm_servo =servo_mid-round_servo_into_r;
// gpio_set(P00_4, 0);
// gpio_set(P02_1, 0);
// gpio_set(P02_2, 1);
}
}else if(type_of_shortpid_road==60||type_of_shortpid_road==70)//强制轨迹
{
pwm_servo_variation=Direction_Servo.KP*Circle_err.error_now+Direction_Servo.KD*(Circle_err.error_now-Circle_err.error_last);
Circle_err.error_last=Circle_err.error_now;
pwm_servo = servo_mid-pwm_servo_variation;
}
//舵
pwm_duty(Servo_PIN, (uint32)(pwm_servo*100));
#elif defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==0)
pwm_duty(Servo_PIN, (uint32)(servo_mid*100));
// pwm_duty(Servo_PIN, 720);
#endif
}
void Servo_PID(void)
{
#if defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==1)
// {
pwm_servo_variation = PID_long_normal.KP * Long_err.error_now + PID_long_normal.KD*(Long_err.error_now - Long_err.error_last );
// }
Long_err.error_last =Long_err.error_now ;
pwm_servo = servo_mid-pwm_servo_variation;
}else if (type_of_road==10)//1为直角弯道0为左拐
{
pwm_servo =servo_mid+Long_servo_angle.angle_small_left;
}
#elif defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==0)
pwm_duty(Servo_PIN, (uint32)(servo_mid*100));
// pwm_duty(Servo_PIN, 720);
#endif
}
void Servo_AI(void)
{
#if defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==1)
if(type_of_ai_road==0)//直道和45度弯
{
//// 跑一套PID结合一下
float pwm_servo_variation_short_pid = PID_AI.KP * Short_err.error_now + PID_AI.KD*(Short_err.error_now - Short_err.error_last );
Short_err.error_last =Short_err.error_now ;
float pwm_servo_short_pid = servo_mid-pwm_servo_variation_short_pid;
#if defined(AI_ERROR_FLAG)&&(AI_ERROR_FLAG==0)
// 输出舵机打角，根据打角解算出pwm
if(ai_to_pid_flag==0)
{
AI_servo.error_now=(((float)servo_p)*1.8f)/127.0f;
float change_servo=1.0f*AI_servo.error_now+AI_Servo.KD*(AI_servo.error_now-AI_servo.error_last);
pwm_servo=servo_mid-change_servo;
AI_servo.error_last=AI_servo.error_now;
// gpio_set(P00_4, 1);
// gpio_set(P02_1, 0);
// gpio_set(P02_2, 0);
}else
{
pwm_servo=pwm_servo_short_pid;
// gpio_set(P00_4, 0);
// gpio_set(P02_1, 1);
// gpio_set(P02_2, 1);
}
// 根据偏差进行PID
#elif defined(AI_ERROR_FLAG)&&(AI_ERROR_FLAG==1)
AI_normal_err.error_now=-0.25f*log(254.0f/((float)error_p+127.0f)-1.0f);
float pwm_servo_variation_short_ai = PID_AI.KP * AI_normal_err.error_now + PID_AI.KD*(AI_normal_err.error_now - AI_normal_err.error_last );
AI_normal_err.error_last =AI_normal_err.error_now ;
float pwm_servo_short_ai = servo_mid-pwm_servo_variation_short_ai;
pwm_servo=pwm_servo_short_ai;
#endif
}
else if(type_of_ai_road==50)
{
if(left_round!=0&&right_round==0)
{
pwm_servo =servo_mid+round_servo_into_l;
// gpio_set(P00_4, 0);
// gpio_set(P02_1, 1);
// gpio_set(P02_2, 0);
}else if(left_round==0&&right_round!=0)
{
pwm_servo =servo_mid-round_servo_into_r;
// gpio_set(P00_4, 0);
// gpio_set(P02_1, 0);
// gpio_set(P02_2, 1);
}
}
if(pwm_servo>servo_mid+LIMIT_SE)
{
pwm_servo=servo_mid+LIMIT_SE;
}
else if(pwm_servo<servo_mid-LIMIT_SE)
{
pwm_servo=servo_mid-LIMIT_SE;
}
pwm_duty(Servo_PIN, (uint32)(pwm_servo*100));
#elif defined(SERVO_USE_FLAG)&&(SERVO_USE_FLAG==0)
pwm_duty(Servo_PIN, (uint32)(servo_mid*100));
// pwm_duty(Servo_PIN, 720);
#endif
}
float Get_speed_goal(void)
{
float returned_speed_goal;
mes])
{
complet_speed_change_times++;
gpio_set(P20_10,0);
if(low_speed_flag==0)
{
returned_speed_goal = Nomal_Speed.speed;
}else if(low_speed_flag==1)
{
returned_speed_goal = Out_Speed.speed;
}else
{
returned_speed_goal = Stop_Speed.speed;
}
}else
{
if(low_speed_flag==0)
{
returned_speed_goal = Nomal_Speed.speed;
}else if(low_speed_flag==1)
{
returned_speed_goal = Out_Speed.speed;
}else
{
returned_speed_goal = Stop_Speed.speed;
}
}
}else
{
if(low_speed_flag==0)
{
returned_speed_goal = Nomal_Speed.speed;
}else if(low_speed_flag==1)
{
returned_speed_goal = Out_Speed.speed;
}else
{
returned_speed_goal = Stop_Speed.speed;
}
}
return returned_speed_goal;
}
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
