智能车竞赛技术报告 | 基础四轮组 - 大连理工大学 - 霹雳小队_以采用c车模或者b车模的基础四轮车为例,请简要描述基于摄像头的光电四轮车设

 

引 言

---

  随着近几年的电子科学技术的不断发展，电子科技的影响逐渐深入到各行各业之中，越来越多的自动化设备开始逐渐渗透到人们的生产生活之中，嵌入式的迅猛发展也为智能研究提供了更为广阔的平台。并且随着5G网络的到来，无人驾驶汽车的研究与发展步入了一个新的快车道。在此背景之下，通过联合各种传感器和控制器组成的控制方式就成为了新的研究热点，控制算法和控制策略也显得更加重要。

  全国大学生杯智能汽车竞赛是国家教学质量与教学改革工程资助项目，通过使用德国英飞凌半导体公司生产的TC264单片机作为车载核心控制模块，增加道路传感器、摄像头、电机驱动电路以及编写相应的程序，制作出了一个可以自主识别道路的汽车模型。因而该竞赛是涵盖了智能控制、模式识别、传感技术、汽车电子、电气、计算机、机械等多个学科的比赛，对学生的知识融合和实践能力的提高，具有良好的推动作用。

  本文采用第十六届全国大学生智能车竞赛的汽车模型——B型车模作为研究平台，以32位单片机TC264作为主控制单元，运用AURIX Development软件作为开发工具进行智能控制策略研究。道路信息检测模块将电感线圈和矫正电容组成电磁传感器和OV7725硬件二值化摄像头组合起来进行赛道信息的获取。本届车模是B型车模，采用后置一个电机驱动，两个轮子之间使用机械差速来获取转弯时的轮速差，并且由于只有一个电机，只需要一个编码器就可以测速，在通过PID算法，将速度进行闭环控制。

  在电源模块的设计中，采用TI公司的开关电源芯片TPS565201和两个线性稳压电源AMS-1117-3.3 分别产生5V和3.3V电压，5V电压为LM324同向放大器供电，其中一个AMS-1117-3.3的输出电压为编码器、摄像头、串口通信、按键、LCD、拨码等模块供电，另一个的输出只为为单片机供电，两个3.3V供电模块的分别使用可以保证单片机的供电电压稳定，是单片机运行更加可靠、高效。最后还有一片LM2596-adj位舵机供电，因为舵机的供电范围是不固定的，虽然舵机的型号是固定的，但是不同的工作电压会让电机有不同的响应速度和扭矩等，所以选用一个可调的稳压模块方便对车模的舵机模块进行调试优化。

  单片机的算法设计部分为本论文论述的重点，主要体现了运用PID控制理论完成对车模速度控制，运用最长白列算法将获得的图像进行更精确的识别等。

  本篇技术报告将从智能车的机械结构、硬件电路、算法设计等方面全面详细的介绍整个准备过程。
 
 

第一章 方案设计

---

  本章主要介绍了智能车系统总体方案的选定和总体设计思路。

1.1系统总体方案的选定

  本届智能汽车比赛，我队为基础四轮组别。在寻迹传感器方面，选择使用电磁传感器和摄像头配合使用。电磁放大电路使用经典的LM324，摄像头选择具有硬件二值化的OV7725，使用编码器测速进行速度闭环控制。

1.2系统的总体方案设计

  系统的硬件电路采用模块化设计方式。主要分为主控制板、电机驱动及测速、电磁信号采集和图像采集五个部分，主控制板上集成单片机最小系统，主控制板与其他功能板使用灰排线功率线连接。

• 主控板部分：

  主控板上包括一片TC264作为核心控制器，一片TPS565201作为5V供电的电源芯片，两片AMS1117-3.3分别为单片机和其他元件分开供电，还有一片LM2596为舵机供电，目的是让单片机在更稳定的电压环境中工作。以及一些辅助
  模块按键、LED、拨码、LCD等用于操作，通过预留串口进行蓝牙连接，通过方便调试和数据获取。
  辅助模块通过互相配合和自己写的GUI界面进行辅助操作，方便观察车模运行状态和修改参数、调节档位等功能。
  电源部分主要包括8V到5V降压模块，3.3V稳压模块，和舵机电源电路。其中由于舵机摆角时瞬时功率较大同时对电源纹波要求并不严格，所以选用的LM2596-adj供电，并且使用可调电压输出更方便对舵机的机械性能优化。外设和单片机选择3.3V线性电源供电。由于5V电源不仅为放大器供电，还为两片AMS1117-3.3供电，所以选用TI公司的TPS565201作为5V电源芯片，利用这款芯片设计的电路理论可以输入最大电压15V，输出电流最大7A，其性能完全可以满足所有元件的工作消耗，并且绰绰有余，这也解决了我们最开始使用7805芯片所产生的过热发烫的问题。

• 信号采集：

  信号采集部分分别为电磁和摄像头两部分。

  电磁信号采集系统采用6.8nF电容和10mH电感配对采集电磁导引信号，经过LM324放大和滤波电路后得到波动较小的直流电压信号，利用TC264内部自带的ADC采集引脚进行信号的采集。
  摄像头采集使用的是硬件二值化摄像头OV7725，该摄像头可以通过写寄存器配置来改变对比度、曝光等设置，可以满足图像的采集功能。

• 电机驱动：

  电机驱动板包含为MCU和各外设供电的电源电路，电机驱动板使用20P灰排线与主控制板相连，包括一路电机驱动、一路测速以及液晶屏接口。电机驱动采用两个半桥驱动芯片BTN7971构成H桥电路，具有简单、高集成度的优点，可靠便捷。测速预留6pin的编码器接口。液晶屏选择SPI接口的ILI9341芯片控制的2.8寸TFT液晶屏。
 
 

第二章 机械结构调整与优化

---

  智能汽车各系统的控制都是在机械结构的基础上实现的，因此在设计整个软件架构和算法之前一定要对整个模型车的机械结构有一个全面清晰的认识，然后建立相应的数学模型，从而再针对具体的设计方案来调整赛车的机械结构，并在实际的调试过程中不断的改进优化和提高结构的稳定性。本章将主要介绍新B3车模的机械结构和调整方案。

2.1 智能汽车车模的选择

  本届比赛基础四轮组使用B3车模.

▲ 图2.1智能汽车B3车模

2.2 智能汽车传感器的安装

  车模中的传感器包括有：速度传感器（编码器），电磁传感器，摄像头传感器。下面分别介绍这些传感器的安装。

2.2.1 速度传感器的安装

  速度传感器我们使用了龙邱mini512线带方向编码器，安装方法如下图：

  X线含义是编码器旋转一圈产生 X个脉冲，通过脉冲计数可以读取到数值A。这个数值A和小车的速度呈线性关系，一般情况下可以不去换算具体速度直接使用即可。

  若想转换为具体速度，则需要则需要知道以下参数：车模轮子周长 X0、车模齿轮的齿数 X1、编码器齿轮的齿数 X2、编码器的线数 X3、ENC 读到数据 X4、ENC 读取时间间隔 X5，速度换算公式：V = ( (X4 / X3) * X2 / X1 ) * X0 / X5。

▲ 图2.2 编码器安装示意图

2.2.2电磁传感器的安装

  为了收集到更多的赛道信息，电磁放大板通过两根碳纤维杆固定在距离车身最前方大约10cm处，电磁电路板共有四个电磁传感器，两个横放的电磁传感器位于电路板最两侧，两个斜放着的电磁传感器位于电路板中间的两侧，通过对电磁值的处理，来判断入圆环的时刻以及小车是否冲出赛道等情况。

2.2.3摄像头传感器的安装

  对于摄像头传感器，我们使用了山外鹰眼二值化摄像头，只有黑白两色。由于比赛严格限制了摄像头高度（摄像头镜片中心的高度距离地面不超过10厘米），所以我们利用自己设计的3D打印模块，通过螺丝螺母将摄像头固定在碳纤维杆上同时用热熔胶加固。对于摄像头的安装角度，由于摄像头是120°广角摄像头，角度太靠近地面会导致画面畸变较大，太远离地面又会导致画面无用部分所占比例较大，因此对于角度的调节要恰当适中。同时虽然二值化方便了对于图像的处理，但是无法避免赛场反光的影响，进而导致车模无法正确找到赛道，该问题也一直存在于我们整个调车过程中。

2.3 智能汽车电路板及电池的安装

2.3.1 电路板的安装

  为了使智能车具有较好的稳定性及转向性能，我们在搭建智能车时将主控板放到了车模的底板上，并用热熔胶和尼龙柱牢固地固定到了车体上。电机驱动电路板则是直接安装到了电机上方，和主控板之间用灰排线和功率电源线连接。

2.3.2 电池安放

  由于我们使用的是体积和重量较大的镍镉电池，我们将电池放到了车底盘靠近电机的部分，通过橡皮筋固定，方便的同时进一步降低车模的重心。

2.4 其他机械结构的调整

2.4.1 机械差速的调整

  B车模只有一个电机，因此后轮只有机械差速，即被动差速。机械差速器为滚珠差速，需要通过松紧来调节差速效果和动力太松会动力不足，滚珠空转，不能将电机的动力输送到轮子上去，太紧会失去后轮的被动差速效果，不利于转弯。尤其是在后期提速的过程中，尤其注重差速的调节，这需要车手不断的调节去感受差速带来的变化，否则会导致转弯工程后轮的漂移或者冲出赛道。所以这部分一定需要花时间去找到感觉才能做出一辆顺滑迅捷的极速四轮车。

2.4.2 车轮表面的处理

  对于车胎，新B车模采用了全新的空心胎皮。但对于尤其是新买的车轮，因为车轮因为与赛道摩擦少，还是光亮面，会降低车轮的抓地力，所以需要对轮子的表面进行处理，以保持其粘滞性，提高抓地力。

2.5 小结

  一个良好的机械结构是一辆“神车”基础，关乎车的整体的性能，包括车的加速性能、转弯性能等。在相同程序代码的控制环境下，机械结构直接影响着车模是否可以在较高速度下正常行驶。因此我们也花了较多的时间在改进车模的机械结构上，例如调节前轮悬挂的松紧，将前轮倾角调节至内倾内八，在可调节的范围内降低车模的重心等。经过大量的理论实验与研究调节机械结构之后，小车整体的稳定性和可靠性得到了较大的提高。

 

第三章 硬件电路方案设计

---

  本系统采用模块化设计方式，主要包括单片机最小系统模块、电源模块、电磁信息采集模块、图像信息采集模块、串口通信模块、测速模块、显示操作模块等部分。

3.1单片机最小系统模块

  本设计核心控制器为英飞凌公司生产的32位单片机TC264，该单片机具有144引脚，双核设计，最大频率位200MHz，具有2.5Mbyte的program flash和96Kbyte的Data Flash.具有硬件I2C接口和SPI接口，和独立的ADC模块等。

  这些丰富的系统资源可以更方便的连接外部电路，多个独立的ADC通道也使得外围的某些ADC模块的设计得到了简化，多种通讯接口也方便了扩展更多的外设和电路功。最高200MHz的频率也让该单片机具有更快的计算速度，使得可以最大的减小数据处理时间。并且TC264双核的结构可以使两个和分别执行不同的命令，丰富功能，提高运行效率。

3.2电源模块

  电源模块是系统稳定工作的基础不管是单片机的稳定运行还是ADC的采集数据等硬件电路的良好运行，都离不开一个稳定的电源系统。因此，一个具有稳定的电压是粗话模块和电流的稳定性在整个的智能车系统中起着十分重要的作用。

3.2.1电池的使用

  电池采用的是2000mAh的镍镉电池，该类型电池的特点就是“结实”、价格便宜，作为官方推荐的电池，镍镉电池完全可以胜任对整个智能车系统供电的要求。

  但是由于镍镉电池电池容量小，具有记忆性的缺点，每次使用不能对电池进行过度放电，并且每次充电前必须先放电才能在充电。

  电池通常电流高，电池的爆发力会强些，但未必如电流低时饱满、持久。同时我们也必须注意充电电流不能过高，当电流过高时，不仅不能提高电池性能，反而会损坏电池，严重时会导致电池起火、爆炸。电池充满电时，电压大约为8V。在电池压小于8V时，应注意及时充电，电池过放会对其造成不可逆转的损害，电压低于7V会对电池造成毁灭性伤害，导致以后再使用时会出现放电量不足的现象，影响整个系统的运行，更严重的可能会导致单片机因为掉电而复位。

3.2.2 稳5.658V电源模块

  使用LM2596-adj将电压降到5.628V供舵机使用。电路原理图如图3.1所示。

  通过R14和R15构成的反馈回路调整输出电压的大小。因为舵机的型号种类不同，所以设计可调输出电压的稳压模块微电机供电就可以避免在更换电机的时候必须更换整个电路板的尴尬情况。

▲ 图片3.1稳5.658电源模块

3.2.3 稳5V电源模块

  由于放大器的额定工作电压为5V，所以需要一个5V的稳压模块，这里选用的是TI公司生产的TPS565201电源芯片，其特点是可以通过大电流和较宽的输入电压范围，方便后期更换输入电源。
  5V稳压电源模块如图3.2所示。

▲ 图片3.2 5V稳压模块

3.2.4稳3.3V电源模块

  由于单片机的额定电压为3.3V，串口通信等模块也为3.3V，所以将电压稳定在3.3V为各个模块供电是必不可少的。
  电路的设计原理如图3.3所示。

▲ 图图片3 3 3.3V稳压模块

3.3 电机驱动模块

  本智能车车模重量不大，并且电机型号较小，对电机驱动的输出电流要求并不苛刻，因此本设计的驱动电路由两片BTN7971芯片构成H桥电路。通过控制电流的流通方向来实现电机的正反转。并通过控制输入的PWM波的占空比来调节电机的平均输出功率，达到控制电机转速的目的。具体电路如图3.4所示。

3.4 串口通信模块

  调车的过程中，需要实时的观察变量变化情况，通过下载线在Live Watch 窗口中观察变量很受局限，所以采用蓝牙无线通信模块，波特率为115200，传输速度更快，配合上位机观察超调量，有助于调试PID参数，还可以用蓝牙来无线传输采到的数据。具体电路图如图3.5所示。

▲ 图图片3 5串口通信原理图

3.5 运算放大器

  对采集到的电磁值进行放大和稳流处理，方便单片机自带的ADC通道进行读取。电路如下图3.6所示，一共四路ADC放大电路。

3.6总结

  一个良好的、运行稳定的硬件电路是一个智能车系统的核心，电路就像是人体中的神经网络，将单片机发出的指令传给各个模块，使各个模块之间配合的更加高效。而电路的鲁棒性也需要更加的强大，才能保证智能小车可以更稳定、更持久的运行下去，尤其是今年国赛的要求是在两分钟内的积分制，这更加考验了智能小车硬件运行的稳定性和耐久性。

 

第四章 小车的控制方法

---

  基础四轮组多采用摄像头采集数据，力求以最快的速度，跑完整个赛道。不论上哪种方案，软件的总体框架总是相似的，我们的想法是先调稳定，再提速度。软件上大类分为图像采集、图像处理，电感采集，电磁控制，速度采集以及速度控制。其中以图像处理为主，电磁辅助控制。

4.1图像采集思路

  摄像头选择具有硬件二值化的OV7725，传输回的是320160的黑白图像，以16进制存在40160的数组中。使用硬件二值化的好处是处理简单，不需要设计复杂的二值化方法。但是这也成为了一种限制：在实际的调车过程中，环境亮度随时间变化明显，必须时刻调节相应的对比度阈值（宏定义）以达到稳定的图像读取效果；若赛道光照不均匀，更是束手无策（见图4.1.1，在一边调好的阈值，到另一边就完全花了）。

  在我们日常练习的赛道上，另一个更加严峻的问题是自然强光的干扰，分为直射光和反射光。由于摄像头较低，想要获得较长的前瞻就不得不将摄像头俯角尽量减小，但是这样一来只有下半部分是赛道信息，上半部分不仅没有参考意义，还会有很强的直射自然光干扰，在对比之下赛道二值化成了一团黑色，调节对比度阈值也无济于事。于是我们利用电磁板，将摄像头上半面遮住，以此缓解此问题。也是由于摄像头较低，远处的自然光在地板、墙面上的反射也对摄像头二值化干扰较大，我们唯一能做的就是加装偏振片，但是效果仍不理想。 只得在程序中采取各种过滤、补齐方法减小影响。

4.2图像处理思路

  图像处理的大思路是边扫描边处理，即每一行、每一列扫描的同时对赛道元素进行分析、操作。这样做是因为我们主要采用从上一行的中间点开始向两边扫描边界的方法，这样在十字等地方遇到向下的尖角时就面临选边问题（见图4.2.1），为了让中点有一个大致的取向，就采取了一定的预测手段，向前补线。

4.2.1边界的获取

  我们采用了先纵向扫描、再横向扫描的方法。即先隔8个像素点纵向扫描，得到40列的上下边界数据，进而得到纵向最低点、最长白列、二阶差分跃变点、前瞻行高度等信息，为横向扫描做一定的预处理，再自下而上隔一个扫一个地读取60行的左右边界数据。其中横向扫描时在判定白色点时为减少无效判别，采用隔8扫1的方法，即先取数组中的16进制数，若为0x00（定义白色为0），则直接跳过，直到不满足条件时，再取16进制数的各个位正常判别。

  补线方面有断点补线和宽度补线两种，都是在横向扫描每一行结束后的操作。断点补线就是在二阶差分足够大时认为其是断点，向下取一斜率，下一回开始以断点为依据，依斜率补线，此时关闭边界扫描及其他判定。宽度补线就是在一边丢线后依经验得到的宽度公式（这里只采用了y=ax+b的简易拟合），将另一边补出来。由于直道和弯道的宽度不同，由纵向贯通列的多少大致地区分，对应不同的预测公式。两个补线均具有预测性质，是基于人的习惯做出的模仿，断点补线的预测成分更大一些，且一开始就是为了应对十字而存在的，最后有了分辨十字元素的思路以后就显得鸡肋了，在弯道等地方会经常误触（如图4.2.1.1，二阶差分的确挺大的），加上路肩后斜率会求错（如图4.2.1.2），如果有机会继续调车的话多半会舍弃掉。

4.2.2 特殊元素的处理

  处理特殊元素的思路是抓住特征，不同阶段之间衔接灵活，最好可以在一定引导下正常循迹，同时存在冗余，保证未正常触发下一阶段时也可完成整个元素。

  以圆环为例。入环使用的是最常规的补线，将纵向二阶差分大的点与左/右下角连接，为了衔接平滑，结束补线的标志是前瞻行搭到此断点以上。出环时先借助断点补线维持一定打角，当补线结束时再以环内打角的平均值延一段距离，解除死打角的标志是判定到直道。入环时为了保证不读到旁边的直道，横向中点读到纵向二阶差分大的地方会限幅到入环方向以内。

  坡道的触发分为3个阶段，均是由纵向贯通行的数量判别的。当大于一个较小阈值时认为在上坡，可在一定容错范围内重复触发，当不满足此条件且超出容错时便认为在坡顶，当大于一个较大阈值时认为在下坡 ，超出容错时认为结束坡道。这里的容错计数皆使用帧数，个人认为比较符合人的逻辑。另外由于坡顶会不可避免地读到赛道外的干扰因素（如图4.2.2.1），于是将前瞻行限制到一定高度以下。

  三叉的判别是根据俯视图“丁”字形的横向贯通、纵向截断的特点来的。入三叉方法与入圆环方法类似，将上边界最低点与左右下角连线，由于下凸不是很明显，难以由纵向二阶差分准确判断前瞻行是否搭在断点上面，于是模糊地认为最低点到边界一定范围内就进入三叉了。三叉内由于电磁值小，特别禁用了由电磁值判定的舵机打死。

4.3 电感采集、控制策略

  电感采取两横两斜的排布方式，两个横电感主要用作丢线打死的方向判别，两个斜电感主要用作环岛的触发。四个电感数据在10ms中断里分别采集10次求平均，分辨率为12位。

  两个斜电感在经过环岛时会分别经历一次极大值，其间两个横电感之和一直比较大，但是鉴于车身在通过时不一定是理想中的在赛道中央，所以斜电感仅用作环岛的触发，而后续的入环方向等判别由摄像头完成。

  当弯道打角不足、车身探出赛道时，摄像头已经读乱，无法自主返回赛道，这时靠近赛道中央的横电感还是有区别于其余三个电感的较大的读数的，于是可以靠这一点来向正确方向打死，回到赛道。这个打死阈值最好与摄像头读乱的时机有重合或正好承接。

  当四个电感的读数都很小时，可认为赛车冲出赛道，在0.5s后开始每经过一个10ms中断将目标速度减1，不满足减速条件后就直接重置。之所以这么做是为了在失误冲出赛道后也能有机会回到赛道。
  另外，直道上的四个电感之和的最小值与三叉内四个电感之和的最大值有明显的区别，可用作出入三叉的辅助判别，如电磁值大才可触发入三叉，电磁值小才可触发出三叉。

4.4 速度采集以及速度控制

  本次采用编码器测速，优点是用齿轮啮合，测速稳定，使用中出现的问题是测得的瞬时速度波动较大，单次测速作为参考不太可靠。应对措施是采取一定的过滤方法，并取十次速度的平均值作为参考。

  电机开始采用增量式PID，可能是由于参数没调好，发现电机超调严重，例如目标速度是70时会一路加速到110以上，相比之下减速略显不足，弯道车模甚至会竖起来，电机也会容易发烫。于是改用了位置式PID，电机超调明显减缓，加减速响应及时了不少，甚至运行时的声音也低了不少。

  速度的控制策略就是简单的直道高目标速度，弯道低目标速度，其中环岛采用固定目标速度，以求稳定通过。上坡采用比直道更高的目标速度，以减小机械上被动减速的影响。

4.5 舵机打角控制策略

  在较高的电压下，普通的塑料舵机容易损坏，于是我们更换了金属舵机，之后再没有出现损坏。我们的舵机使用PWM波控制，由位置式PD给出占空比的值，频率采用50Ｈz，因为舵机的反应时间为0.1s左右，采用更高的频率意义不大（个人观点）。

  输入舵机打角的值是参考行上读取的赛道中间值和屏幕中间值的差，参考行由纵向扫描的最长白列乘以一定的比例（宏定义）获得，随着调车进程推进，速度上升，比例也逐渐加大。

  PD值的选取依然按直道和弯道加以区分，直道PD均小，弯道PD均大。考虑到一般的弯道有专门多余的打角以配合速度，环岛这种持续的打角额外采用比直道更小的PD来改善循迹。
  第五章 开发工具、制作、安装、调试过程

5.1 上位机

  在调试过程中，我们使用的是由智能车实验室成员自主设计开发并适合摄像头、光电组别的上位机。大体功能如下：

5.1.1 串口功能

1、普通查看接收功能

  选择串口号，和波特率。默认串口号位所有可以使用的串口中号码最小的，默认波特率位115200（考虑速度和误码率确定为115200），然后选择“打开串口”。

2、普通发送功能

  必须打开串口此功能才能用。有“显示发送了多少字节”功能，“清空显示”功能，还可选择“包含换行符”，“16进制发送”功能。

3、直接发送字节功能

4、蓝牙调试功能

  使用此功能必须打开串口，然后“使能蓝牙调试”框勾选。
注意：

  AT+BAUD: 后边是1,2,3,4,5,6,7等。4代表9600bps，8代表115200bps
  AT+NAME: 可以是英文或者数字组合。
  AT+NAME: 可以是英文或者数字组合。
  AT+ROLE: 后边是0，1。0是从机，连接单片机，1是主机，连接电脑。

单片机程序：

【省略】

 

第六章 车模主要参数

---

6.1智能车外形参数

• 经过改装后，智能汽车外形参数为：
• 车B3
• 车长：435mm 车宽：245mm 车高：143mm
• 摄像头距地面高度：9.3mm
• 车重：1kg

6.2智能车技术参数

  智能汽车相关技术参数如表6.1所示：

• 项目 参数
• 车模轴距/轮距（mm） 200
• 车模平均电流（匀速行驶）（mA） 400
• 电容总量（uF） 2400
• 传感器种类及个数 测速编码器 1个
• LC电磁传感器 4个
• OV7725 1个
• 新增加伺服电机个数 0
• 赛道信息检测空间精度（mm） 10
• 赛道信息检测频率（次/秒） 100

 

结论

---

  在这份报告中，我们主要从智能车的机械结构、硬件电路、控制算法、调试方法等介绍了我们准备比赛的整个过程，摄像头的寻迹程序大体上是继承了前辈们的精髓，但是也做出了我们自己的创新。创新之处大体为以下几点：

  1、 我们并没有选择单一的摄像头传感器寻迹，选择使用电磁配合摄像头进行一些元素的识别。我们电磁的分布选择两个平电感和两个外八电感。平电感的作用主要是用来辅助识别车辆位置的，由于今年基础四轮组别的摄像头高度固定在了10cm以内，这就导致了视野和前瞻的远近相互制约，但是一辆车如果看的不够远那就很难跑快，所以为了获得更远的前瞻，我们不得不牺牲一部分的视野来换取更远的前瞻，所以车辆近处的信息就无法通过摄像头来获取。于是我们选择了在距离车头10cm处添加两个平电感来获取车辆近处的位置信息。另外两个外八的电感用来辅助环岛的识别，使得环岛的识别更加的准确。

  2、 读取图像的方式也进行了改进，由于摄像头过低，并且为广角摄像头，所以获得的图像会出现很大的畸变。为了可以更准确的获得赛道信息，我们选择最长白列算法进行读取，这样就可以控制赛道读取的位置。

  3、 对车模进行一些调整。今年使用的车模是B3车模，该车模后置一个电机，依靠机械差速来调整轮速差。为了提高车辆的拐弯性能，我们调整机械差速器的松紧，并且改变了前轮的倾角来提升转弯时车辆的抓地性能。
  智能车是培养大学生综合动手能力的一个很好的、很成熟的一个竞赛，在比赛里我们不仅需要编程能力，还需要对车辆机械结构有着较深的理解，也需要熟悉各种传感器和电路的性能，你会发现不管是什么专业的学生都会在这个比赛中找到自己的定位。智能车比赛中最重要的是你需要足够多的创新能力和想法，和国家已经把创新放在了国家全局的核心位置，如今的竞赛一方面是在考验大学生的基本知识，更实在考验每一个参赛大学生的创新能力，所以只有创新才可以从全国大学生中脱颖而出。

  在这个比赛中每个人都会学有所得，做有所获，通过这个比赛我也懂得了团队的重要性，学会分工合作，也找到了自己的擅长一面，同时也弥补了自己的不足之处。在长达几乎一年的准备过程中，我们团队三个人并没有一帆风顺，但我们不惧艰难，苦难与折磨只会让我们更加的强大，一年中的欢笑也会是我内心永远的花朵。
  希望这篇技术文档会对未来参与智能车这项比赛的同学有所帮助，也祝愿他们可以在比赛中收获快乐

 

参 考 文 献

---

[1] 童诗白，华成英．模拟电子技术基础[M]．北京. 高等教育出版社．2000
[2] 邵贝贝. 嵌入式实时操作系统[LC／OS-Ⅱ(第2版)[M]. 北京．清华大学出版社．2004
[3] 阎石.数字电子技术基础[M].北京:高等教育出版社,1998.
[4] 雷霏霖,梁志毅.基于CMOS传感器 OV7620 采集系统设计[J].电子测量技术,2008,12(31):110-112.
[5] 谭浩强.C程序设计[M].北京:清华大学出版社,2005.
[6] 郭芳,曹桂琴.数据结构基础[M].大连：大连理工大学出版社，1994.
[7] 邵贝贝.单片机嵌入式应用的在线开发方法[M].北京:清华大学出版社,2004.
[8] 胡寿松.自动控制原理(第六版)[M].科学出版社,2014.
[9] 张文春．汽车理论[M]．北京．机械工业出版社．2005
[10] YUAN Quan,ZHANG YunZhou,WU Hao,et al. Fuzzy Control Research In The Courses Of Smart Car[C]. Machine Vision and Human-Machine Interface (MVHI), Kaifeng,China, 2010: 764-767.
[11] 侯虹.采用模糊PID控制律的舵机系统设计[J].航空兵器，2006,2(1):7-9.
[12] 孙浩,程磊,黄卫华,等.基于HCS12的小车智能控制系统设计[J].单片机与嵌入式系统应用,2007,03(16):51-54.
[13] LU Zhenlin,LI Jingjiao,Zhang Minghui.Intelligent Control Research based on the Smart Car[C]. Advanced Computer Control (ICACC),Shenyang,2010:292-297.

 

附录

---

附录A：程序源码

PID控制：

void Motor_PID()//电机PID
{
    static int error[2] = {0};   //历史误差     左边变量
    long P_data = 0, I_data = 0, D_data = 0;
    static long I_count = 0; //积分累计值

    error[0] = Speed_Target - speed;
    P_data = MotorP * error[0];
    I_count += error[0];
    if(I_count > I_COUNT_MAX)
        I_count = I_COUNT_MAX;
    else if(I_count < -I_COUNT_MAX)
        I_count = - I_COUNT_MAX;
    I_data = MotorI * I_count;
    D_data = MotorD * (error[0] - error[1]);
    Motor_Output = P_data + I_data + D_data;  //计算输出

    if (Motor_Output > 60000)                //限幅,Motor_Output=10000时速度就很快了，所以限制在30000以下，避免反复启动时电流过大，对元件造成损伤
        Motor_Output = 60000;
    else if (Motor_Output < -60000)
        Motor_Output = -60000;
//    if(error[0]<0 && flag_podao==1) //坡道溜坡
//        Motor_Output=0;
    //更新误差
    error[1] = error[0];

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

舵机PD：

void Dir_PD()   //无平均
{
    static double error[2] = {0};  //历史误差
    int angle;

    error[0] = -((double) MID - (double) mid_eve);
    error[0]=error[0]*4/MID;//相當於除以40，使大於80的經平方變大，小於80的更小

    if(flag_island)
    {
        angle = (int) (Dir_mid + 210 * (error[0]) + 470 * (error[0] - error[1]));   //计算增量
    }
    else if((startline>=xiang+5))//(((Mid_H) > (xiang-20)))  //參考中綫的高度來確定用哪一套PD，即入彎靈敏度,||((flag_island!=0)&&(flag_island!=15)),||(flag_island) count_lst>=5 &&
    {
//        if (abs(error[0]) >= 40) && flag_island!=1 && flag_island!=2 && flag_island!=12
        angle = (int) (Dir_mid + DirP_L * (error[0]) + DirD_L * (error[0] - error[1]));   //计算增量
//        else
//            angle = (int) (Dir_mid + DirP_L * 0.8 * (error[0]) + DirD_L * 0.9 * (error[0] - error[1]));   //计算增量
    }
    else
    {
//        if (abs(error[0]) >= 40)
        angle = (int) (Dir_mid + DirP_S * (error[0]) + DirD_S * (error[0] - error[1]));   //计算增量
//        else
//            angle = (int) (Dir_mid + DirP_S * 0.8 * (error[0]) + DirD_S * 0.9 * (error[0] - error[1]));   //计算增量
    }
    if(angle > Dir_mid)
        Dir_output = (int)(1.05*(float)(angle-Dir_mid))+Dir_mid;
    else
        Dir_output = angle;
        if (Dir_output > Dir_max)                //限幅
                Dir_output = Dir_max;
        else if (Dir_output < Dir_min)
                Dir_output = Dir_min;
 
    error[1] = error[0];  //更新误差
    if (adc1 > LOW && adc2 > LOW && adc3 > LOW && adc4 > LOW)
    {
        if (Dir_output < Dir_mid)
            flag_lastdir = 0;
        else
            flag_lastdir = 1;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

扫描：

void image_process ()                 //图像处理
{
    int liubian = 2;
    int i=0;
    int j=0;
    int count_black=0;//黑色连续点计数
    int mid_sum = 0;
    int L_temp=CAMERA_W-3;//儅預測補綫時，暫存數據用，防止更改邊界后對之後的二階差分造成影響
    int R_temp=2;
    int U_max=0;//上邊界最大值對應的角標
    int H_duandian = 0;        //上斷點
    int mid_temp=mid_eve;//记录中间值
    int start = mid_eve;                      //第一行中点开始扫描的点
    int start_H = 2+DOWN_SEE;                      //縱向第一行中点开始扫描的点
    int point = 12;                       //可调，丢线打死参考行
    int flag_island_temp=flag_island;   //記錄標志位，保證處理時不會變化
//    int startline_temp=startline;
    int flag_zaodian=0;//噪點標志位，通過一階差分的大小來判斷有無噪點，0為無，1為有，則不補綫
    int count_lst_temp=0;
    int daozhao; //倒著找斜率

    flag_buxian_L=0;
    flag_buxian_R=0;
    U_lst_H=40;//縱向白列最大值對應的角標
    L_duandian1 = 0;         //记录断点
    R_duandian1 = 0;
    L_duandian_1=0;
    R_duandian_1=0;
    temp_L = 0;
    temp_R = 0;
    flag_m = 1;
    End_L=0;//左补线终点
    End_R=0;//右补线终点
    TOP[0]=CAMERA_W-3;//左边界赋初值
    TOP[1]=2;//右边界赋初值

    //若右補綫，反向掃描，方便判別；平時走，中點在左，從左往右掃更好
    if ((flag_island_temp == 2)||(flag_island_temp==3)||(flag_island_temp==6)||(flag_cheku==3)||(((flag_island_temp == 0)||(flag_island_temp==15)||(flag_island_temp==12))&&(mid_eve>159)))//
    {
        U_max=39; //反向掃描，反向賦初值
//        U_lst_H=39;
        for (j = CAMERA_W - 8; j >= 0; j=j-8)                         //縱向隔一個讀取上下邊界
        {
            D_edge[j/8] = 2+DOWN_SEE;                         //先设置默认值，若未找到边界，也可有对应值
//            U_edge[j/8] = DOWN_SEE-3;
            U_edge[j/8] = 2+DOWN_SEE;
            if (count_black < 3)        //连续黑色点计数过多，不再找边界
            {
                /****************************************************************************************/
                /*此处扫描程序，扫描方式为：从第一行中点扫描边界，找到第一行中点后，第二行按第一行中点开始扫描*/
                /*如果第一行中点的左右两边各为两个黑点，就意味着下边的for循环语句上来就会读错，所以开头加了一个if判断*/
                /*如果开始判断出第一行中间是黑色的，就会开始左右读白色的，然后生成中点*/
                /*郝:个人认为可拓展至任意行丢线*/
                /*******************************************************************************************/
                if (IMG_PIXEL(2+DOWN_SEE,j) == 0)          //如果最下端為白，那麽就不必要從中間找了
                    start_H = 2+DOWN_SEE;

                if ((IMG_PIXEL(start_H,j) == 1) && (IMG_PIXEL(start_H+1,j) == 1) && (IMG_PIXEL(start_H+2,j) == 1)\
                        && (IMG_PIXEL(start_H-1,j) == 1) && (IMG_PIXEL(start_H-2,j) == 1))
                {
//                    if((IMG_PIXEL(2,j+1) == 0)||(IMG_PIXEL(2,j+2) == 0)||(IMG_PIXEL(2,j+3) == 0))//已經掃到賽道才可以放棄
                    if(U_lst_H!=40)
                        count_black++;
                    else
                        count_black=0;
                    for (i = 0; i <= MAX_SEE - 3; i++)
                    {
                        //賽道肯定在下，不需要對稱地向上找了
                        if (((start_H - i) >= DOWN_SEE) && (IMG_PIXEL(start_H-i,j) == 0) && (IMG_PIXEL(start_H-i-1,j) == 0)\
                                && (IMG_PIXEL(start_H-i-2,j) == 0)) //向下同时为白
                        {
                            U_edge[j / 8] = start_H - i;
                            D_edge[j / 8] = 2+DOWN_SEE;
                            break;
                        }
                    }
                }
                else      //同时向上下读取
                {
                    count_black=0;
                    for (i = 0; i <= MAX_SEE - 3; i++)
                    {
                        if ((D_edge[j / 8] == 2+DOWN_SEE) || (U_edge[j / 8] == 2+DOWN_SEE))//還沒全找到就繼續找
                        {
                            if ((U_edge[j / 8] == 2+DOWN_SEE) && ((start_H + i+2) <= UP_SEE - 1) && (IMG_PIXEL(start_H+i,j) == 1)) //向上同时为黑,\
                                && (IMG_PIXEL(start_H+i+1,j) == 1) && (IMG_PIXEL(start_H+i+2,j) == 1)
                            {
                                U_edge[j / 8] = start_H + i;
                            }
                            else if ((U_edge[j / 8] == 2+DOWN_SEE) && ((start_H + i+2) ==UP_SEE - 1))
                            {
                                U_edge[j / 8] = start_H + i;
                            }
                            if ((D_edge[j / 8] == 2+DOWN_SEE) && ((start_H - i-2) >= DOWN_SEE) && (IMG_PIXEL(start_H-i,j) == 1)) //向下同时为黑,\
                                && (IMG_PIXEL(start_H-i-1,j) == 1) && (IMG_PIXEL(start_H-i-2,j) == 1)
                            {
                                D_edge[j / 8] = start_H - i;
                            }
                        }
                        else
                            break;
                    }
                }

                if ((U_edge[j / 8] <= D_edge[j / 8]) && ((CAMERA_W / 8 - 1 - j / 8)>0))                      //防止上邊界讀到下邊界的下邊
                {
                    U_edge[j / 8] = U_edge[j / 8 + 1];                             //若上边界小于下边界，上下边界都按上一列的来
//                D_edge[j/8] = D_edge[j/8 + 1];
                }
                if ((flag_island_temp == 2)||(flag_island_temp == 3)||flag_podao == 1 ||flag_podao == 4)
                {
                    if ((CAMERA_W / 8 - 1 - j / 8)>0)                                                     //從第二列開始求一階差分
                    {
                        U_edge_1[j / 8+1] = U_edge[j / 8 + 1] - U_edge[j / 8];
//                D_edge_1[j/8] = D_edge[j/8+1] - D_edge[j/8];
                    }
                    if ((CAMERA_W / 8 - 1 - j / 8) > 1)                                                   //從第三列開始求二階差分
                    {
                        U_edge_2[j / 8+2] = U_edge_1[j / 8 + 2] - U_edge_1[j / 8+1];
//                D_edge_2[j/8] = D_edge_1[j/8 + 1] - D_edge_1[j/8];

                        if (((((U_edge[j / 8 + 1] - U_edge[j / 8]) >= -30) && (j / 8 + 1 > 19))||(j / 8 + 1 <= 19)) && (j / 8 + 1 < 39) && (U_edge[j / 8 + 2] >= 5+DOWN_SEE))         //上邊界也別太低了, (U_edge[j / 8 + 2] <= MAX_SEE - 21)
                        {
                            if ((!flag_buxian_R)  && (U_edge_2[j / 8+2] >= m_H)) //找上斷點，范围是40~311列，10~99行,&&(U_edge_1[j / 8 + 2]>0)&&(U_edge_1[j / 8+1]<0 && (U_edge_2[j / 8+2] <= m_H*10)
                            {
                                if(H_duandian == 0)
                                    H_duandian = j / 8 + 1;                          //多留一點，反正斜率別太離譜就行
                                if (flag_island_temp == 2)
                                {
                                        R_duandian1 = (U_edge[H_duandian]-DOWN_SEE)/2-3;
                                        R_xielv = ((double) (H_duandian * 8)) / ((double) R_duandian1); //求右斜率 +CAMERA_W/8
//                                if (R_xielv <= -0.5)           //斜率限幅
//                                    R_xielv = -0.5;
//                                else if (R_xielv >= 3.0)
//                                    R_xielv = 3.0;
                                        flag_buxian_R = 1;
                                }
                            }

                        }
                    }
                }
                if (U_edge[j / 8] > UP_SEE - 3)                            //限幅
                    U_edge[j / 8] = UP_SEE- 3;
                else if (U_edge[j / 8] < 2+DOWN_SEE)
                    U_edge[j / 8] = 2+DOWN_SEE;

                if (D_edge[j / 8] > UP_SEE- 3)                            //限幅
                    D_edge[j / 8] =UP_SEE - 3;
                else if (D_edge[j / 8] < 2+DOWN_SEE)
                    D_edge[j / 8] = 2+DOWN_SEE;

                middle_all_H[j / 8] = (U_edge[j / 8] + D_edge[j / 8]) / 2;            //使每一列的起始扫描点是上一列的中点
                start_H = middle_all_H[j / 8];

                //不是限幅，防止未發現邊界時的默認值影響上限，当找到边界时，寻找上边界最大值
                if ((U_edge[U_max] >= UP_SEE - 3) || ((U_edge[j / 8] <= UP_SEE - 4) && (U_edge[j / 8] >= U_edge[U_max])))
                    U_max = j / 8;
                //尋找最長白列
                if((((U_edge[j / 8]-D_edge[j / 8])<=MAX_SEE-5)&&((U_edge[j / 8]-D_edge[j / 8])>(U_edge[U_lst_H]-D_edge[U_lst_H]))))//((U_edge[U_lst_H]-D_edge[U_lst_H])>=MAX_SEE-5)\
                    ||
                {
                    if(!count_black)
                    {
                        if (H_duandian)            //若有断点，則不需向右找最長白列
                        {
                            if (j / 8 > H_duandian)
                                U_lst_H = j / 8;
                        }
                        else
                            U_lst_H = j / 8;
                    }
                }
            }
            if((U_edge[j / 8]-D_edge[j / 8])>=MAX_SEE-10)//阈值可调，目前是差5个满就会计入，调的是直道判定难度，越大越难触发
            {
                count_lst_temp++;
            }
        }
    }
    else
    {
        for (j = 0; j < CAMERA_W; j=j+8)                         //縱向隔一個讀取上下邊界
        {
            D_edge[j/8] = 2+DOWN_SEE;                         //先设置默认值，若未找到边界，也可有对应值
            U_edge[j/8] = 2+DOWN_SEE;

            if (count_black < 3)        //连续黑色点计数过多，不再找边界
            {
                /****************************************************************************************/
                /*此处扫描程序，扫描方式为：从第一行中点扫描边界，找到第一行中点后，第二行按第一行中点开始扫描*/
                /*如果第一行中点的左右两边各为两个黑点，就意味着下边的for循环语句上来就会读错，所以开头加了一个if判断*/
                /*如果开始判断出第一行中间是黑色的，就会开始左右读白色的，然后生成中点*/
                /*郝:个人认为可拓展至任意行丢线*/
                /*******************************************************************************************/
                if (IMG_PIXEL(2+DOWN_SEE,j) == 0)          //如果最下端為白，那麽就不必要從中間找了
                    start_H = 2+DOWN_SEE;

                if ((IMG_PIXEL(start_H,j) == 1) && (IMG_PIXEL(start_H+1,j) == 1) && (IMG_PIXEL(start_H+2,j) == 1)\
                        && (IMG_PIXEL(start_H-1,j) == 1) && (IMG_PIXEL(start_H-2,j) == 1))
                {
//                    if((IMG_PIXEL(2,j-1) == 0)||(IMG_PIXEL(2,j-2) == 0)||(IMG_PIXEL(2,j-3) == 0))//已經掃到賽道才可以放棄
                    if(U_lst_H!=40)
                        count_black++;
                    else
                        count_black=0;
                    for (i = 0; i <= MAX_SEE - 3; i++)
                    {
                        //賽道肯定在下，不需要對稱地向上找了
                        if (((start_H - i) >= DOWN_SEE) && (IMG_PIXEL(start_H-i,j) == 0) && (IMG_PIXEL(start_H-i-1,j) == 0)\
                                && (IMG_PIXEL(start_H-i-2,j) == 0)) //向下同时为白
                        {
                            U_edge[j / 8] = start_H - i;
                            D_edge[j / 8] = 2+DOWN_SEE;
                            break;
                        }
                    }
                }
                else      //同时向上下读取
                {
                    count_black=0;
                    for (i = 0; i <= MAX_SEE - 3; i++)
                    {
                        if ((D_edge[j / 8] == 2+DOWN_SEE) || (U_edge[j / 8] == 2+DOWN_SEE))      //還沒全找到就繼續找
                        {
                            if ((U_edge[j / 8] == 2+DOWN_SEE) && ((start_H + i+2) <= UP_SEE - 1) && (IMG_PIXEL(start_H+i,j) == 1)) //向上同时为黑,\
                                && (IMG_PIXEL(start_H+i+1,j) == 1) && (IMG_PIXEL(start_H+i+2,j) == 1)
                            {
                                U_edge[j / 8] = start_H + i;
                            }
                            else if((U_edge[j / 8] == 2+DOWN_SEE) && ((start_H + i+2) == UP_SEE - 1))
                            {
                                U_edge[j / 8] = start_H + i;
                            }
                            if ((D_edge[j / 8] == 2+DOWN_SEE) && ((start_H - i-2) >= DOWN_SEE) && (IMG_PIXEL(start_H-i,j) == 1)) //向下同时为黑,\
                                && (IMG_PIXEL(start_H-i-1,j) == 1) && (IMG_PIXEL(start_H-i-2,j) == 1)
                            {
                                D_edge[j / 8] = start_H - i;
                            }
                        }
                        else
                            break;
                    }
                }

                if ((U_edge[j / 8] <= D_edge[j / 8]) && ((j / 8)>0))                                 //防止上邊界讀到下邊界的下邊
                {
                    U_edge[j / 8] = U_edge[j / 8 - 1];                             //若上边界小于下边界，上下边界都按上一列的来
//                D_edge[j/8] = D_edge[j/8 - 1];
                }

                if ((flag_island_temp == 1)||(flag_island_temp == 4)||flag_podao == 1 ||flag_podao == 4)                       //触发左補綫入環补线模式
                {
                    if ((j / 8)>0)                      //從第二列開始求一階差分
                    {
                        U_edge_1[j / 8 - 1] = U_edge[j / 8] - U_edge[j / 8 - 1];
//                    D_edge_1[j/8 - 1] = D_edge[j/8] - D_edge[j/8 - 1];
                    }
                    if (j / 8 > 1)                                                   //從第三列開始求二階差分
                    {
                        U_edge_2[j / 8 - 2] = U_edge_1[j / 8 - 1] - U_edge_1[j / 8 - 2];
//                    D_edge_2[j/8 - 2] = D_edge_1[j/8 - 1] - D_edge_1[j/8 - 2];
                        if (((((U_edge[j / 8 - 1] - U_edge[j / 8]) >= -30) && (j / 8 - 1< 19))|| (j / 8 - 1>= 19)) && (j / 8 - 1> 0) &&(U_edge[j / 8 - 2] >= 5+DOWN_SEE))           //((U_edge[j / 8 - 2] - U_edge[j / 8 - 1]) >= -6) && (U_edge[j / 8 - 2] <= MAX_SEE - 21)
                        {
                            if ((!flag_buxian_L) && (U_edge_2[j / 8 - 2] >= m_H)) //找上斷點，范围是8~280列，99行以下,(j > 8) && (j < 280)  && (U_edge_2[j / 8 - 2] <= m_H*10)
                            {
                                if(H_duandian == 0)
                                    H_duandian = j / 8 - 1;
                                if (flag_island_temp == 1)
                                {
                                        L_duandian1 = (U_edge[H_duandian]-DOWN_SEE)/2-3;
                                        L_xielv = ((double) (H_duandian * 8 - CAMERA_W - 1)) / ((double) L_duandian1); //求左斜率
//                                if (L_xielv >= 0.5)              //斜率限幅
//                                    L_xielv = 0.5;
//                                else if (L_xielv <= -3.0)
//                                    L_xielv = -3.0;
                                        flag_buxian_L = 1;
                                }
                            }

                        }
                    }
                }
                if (U_edge[j / 8] > UP_SEE - 3)                            //限幅
                    U_edge[j / 8] = UP_SEE- 3;
                else if (U_edge[j / 8] < 2+DOWN_SEE)
                    U_edge[j / 8] = 2+DOWN_SEE;

                if (D_edge[j / 8] > UP_SEE- 3)                            //限幅
                    D_edge[j / 8] =UP_SEE - 3;
                else if (D_edge[j / 8] < 2+DOWN_SEE)
                    D_edge[j / 8] = 2+DOWN_SEE;

                middle_all_H[j / 8] = (U_edge[j / 8] + D_edge[j / 8]) / 2;            //使每一列的起始扫描点是上一列的中点

                if (middle_all_H[j / 8] > UP_SEE - 3)                            //限幅
                    middle_all_H[j / 8] = UP_SEE - 3;
                else if (middle_all_H[j / 8] < 2+DOWN_SEE)
                    middle_all_H[j / 8] = 2+DOWN_SEE;

                start_H = middle_all_H[j / 8];

                //不是限幅，防止未發現邊界時的默認值影響上限，当找到边界时，寻找上边界最大值
                if ((U_edge[U_max] >= UP_SEE- 3) || ((U_edge[j / 8] <= UP_SEE - 4) && (U_edge[j / 8] >= U_edge[U_max])))
                    U_max = j / 8;
                if((((U_edge[j / 8]-D_edge[j / 8])<=MAX_SEE-5)&&((U_edge[j / 8]-D_edge[j / 8])>(U_edge[U_lst_H]-D_edge[U_lst_H]))))//((U_edge[U_lst_H]-D_edge[U_lst_H])>=MAX_SEE-5)\
                    ||
                {
                    if (!count_black)
                    {
                        if (H_duandian)                            //若有断点，則不需向左找最長白列
                        {
                            if (j / 8 < H_duandian)
                                U_lst_H = j / 8;
                        }
                        else
                            U_lst_H = j / 8;
                    }
                }
            }
            if ((U_edge[j / 8] - D_edge[j / 8]) >= MAX_SEE - 10)                     //阈值可调，目前是差5个满就会计入，调的是直道判定难度，越大越难触发
            {
                count_lst_temp++;
            }
        }
    }

//    if(flag_island_temp == 4)//方便環内循迹
//        U_lst_H=1;
//    else if(flag_island_temp == 3)
//        U_lst_H=38;
//
//    if (flag_island_temp == 13)//坡上不要乱打-反而亂打了
//        U_lst_H = 19;

    count_lst=count_lst_temp;//更新值

    start=U_lst_H*8;
//
//    Xishu_startline=0.8-0.01*(double)(U_edge[U_lst_H] - D_edge[U_lst_H]-90);//90为入弯时的最长白列，需调
//    if(Xishu_startline>0.8)//限幅
//        Xishu_startline=0.8;
//    else if(Xishu_startline<0.6)//限幅
//        Xishu_startline=0.6;

//    if((flag_island_temp==5||flag_island_temp==6)&&(U_edge[U_lst_H] - D_edge[U_lst_H]>=110))
//    {
//        s = 0;
//        flag_island = -1;
//    }
    //取前瞻行為最長白列的一定比例
    startline = (int)((double)(U_edge[U_lst_H] - D_edge[U_lst_H]) * FORESEE);//倍數可調
//    if(startline_temp<startline)//减的时候直接赋值，加的时候逐渐加起，来防止出弯时大打角+加速引起的漂移
//        startline=startline_temp;
//    else if(startline_temp>startline)
//        startline++;

    if(startline<11)//不能低於10
        startline = 11;
    if(flag_podao == 1 || flag_podao == 4)
    {
        if(startline > 40)
            startline = 40;
    }

    if ((((startline) / 2 - 3) > (L_duandian1+5)&& (L_duandian1) && \
            flag_island_temp == 1 ) || (((int)((float)s*s_xishu))>3000 && flag_island_temp == 1))//),&& (U_edge_2[H_duandian]<= m_H*3) && (U_edge_2[H_duandian]>= m_H)
    //                            else if ((j < CAMERA_W - 8) && (j >= 280) && (U_edge_2[j / 8 - 2] >= m_H + 1)) //找上斷點，范围是280~312列，99行以下，防止誤觸觸發，所以閾值加1
    {
//        if(H_duandian * 8>CAMERA_W/4 )
            flag_island = 4;
            flag_jiansu=0;
//        else
//            flag_island=0;
        s = 0;
        //                                gpio_set(P13_3, 0);              //蜂鳴器
    }
    if (((((startline) / 2 - 3) > (R_duandian1+5)) && (R_duandian1) &&\
            flag_island_temp == 2) || (((int)((float)s*s_xishu))>3000 && flag_island_temp == 2))//,&& (U_edge_2[H_duandian]>= m_H)*3 && (U_edge_2[H_duandian]>= m_H)
//                            else if ((j > 8) && (j <= 39) && (U_edge_2[j / 8+2] >= m_H + 1)) //找上斷點，范围是8~39列，10~99行，防止誤觸觸發，所以閾值加1
    {
//        if(H_duandian * 8<CAMERA_W*3/4 )
            flag_island = 3;
            flag_jiansu=0;
//        else
//            flag_island=0;
        s=0;
//            gpio_set(P13_3,0);              //蜂鳴器
    }

//    if((flag_cheku>-40)&&(flag_cheku<0))//第一次发现车库，延0.2秒不打角
//        startline=30;

    for (j = 6; j <= MAX_SEE-2; j+=2)
    {
        if(j>=U_edge[U_lst_H]-DOWN_SEE)//高於最長白列，肯定讀亂
            break;
        int k=j+DOWN_SEE;
        int count_L=0;
        int count_R=0;
        int flag_L=0;
        int flag_R=0;
        int flag_black=0;
        int distance=(int)(480.0-3.6*j);//左右間隔估計值
//        if(flag_sancha)
//        {
//            distance=(int)(340.0-2.80*j);//不正常赛道左右間隔估計值
//        }
//        else
        if(flag_island_temp||(count_lst>=9 && startline>=xiang+5 && flag_podao != 3 && flag_podao != 5) || flag_sancha)//&&flag_island_temp!=1&&flag_island_temp!=2 ||(Mid_H) > (xiang-10) && Mid_H > xiang-20
        {
            distance=(int)(490.0-3.5*j);//正常赛道左右間隔估計值
        }
        else if(flag_podao == 3 || flag_podao == 5)
        {
            distance=(int)(490.0-1.0*j);//podao道左右間隔估計值
        }
        else
        {
//            distance=470.0-2.7*j;//弯道左右間隔估計值
            distance=((96-startline)*1)+(int)(480.0-2.9*j);//弯道左右間隔估計值
//            distance=((90-startline)*2)+(int)(450.0-2.9*j);//弯道左右間隔估計值
        }

        if (flag_buxian_L==0)
            L_edge[j/2-3] = CAMERA_W-3;//先设置默认值，若未找到边界，也可有对应值
        if (flag_buxian_R==0)
            R_edge[j/2-3] = 2;

        if (j <= (startline + 25))               //參考綫以上20行無意義
        {
            //有最長白列，不需要掃黑的啦
            if((IMG_PIXEL(k,start) == 1) && (IMG_PIXEL(k,start+1) == 1) &&(IMG_PIXEL(k,start+2) == 1)\
                    &&(IMG_PIXEL(k,start-1) == 1) && (IMG_PIXEL(k,start-2) == 1) && ((start + 2) < CAMERA_W) && ((start - 2) >= 0))
                flag_black = 1;
            else
                flag_black = 0;

            if (((flag_buxian_R==0)||(flag_buxian_L==0))) //補綫就不用找了
            {
                if(flag_sancha !=1 && flag_sancha != 2)// && flag_cheku!=1 && flag_cheku!=2
                {
                    for (i = 0; i < CAMERA_W; i++)
                    {
                        if ( (L_edge[j/2-3] == CAMERA_W - 3) || (R_edge[j/2-3] == 2) ) //還沒全找到就繼續找
                        {
                            /*先扫黑点*/
                            if(flag_black==1)
                            {
                                if ((flag_buxian_L == 0))
                                {
                                    if (flag_L == 0)
                                    {
                                        if ((start+i*8<CAMERA_W) && IMG_LONG_PIXEL(k,start+i*8) == 0xFF)
                                            count_L++;
                                        else
                                        {
                                            flag_L = 1;
                                            count_L++;
                                        }
                                    }
                                    if ((flag_L == 1)&&(R_edge[j / 2 - 3] == 2) && ((start +count_L*7-8+ i + 2) < CAMERA_W) && (IMG_PIXEL(k,start+count_L*7-8+i) == 0)\
                                            && (IMG_PIXEL(k,start+count_L*7-8+i+1) == 0) && (IMG_PIXEL(k,start+count_L*7-8+i+2) == 0)) //向左三点同时为白
                                    {
                                        R_edge[j / 2 - 3] = start+count_L*7-8+i;
                                        L_edge[j / 2 - 3] = R_edge[j / 2 - 3] + distance;
                                        break;
                                    }
                                }
                                if ((flag_buxian_R == 0))
                                {
                                    if (flag_R == 0)
                                    {
                                        if ((start-i*8-8>=0) && IMG_LONG_PIXEL(k,start-i*8-8) == 0xFF)
                                            count_R++;
                                        else
                                        {
                                            flag_R = 1;
                                            count_R++;
                                        }
                                    }
                                    if ((flag_R == 1)&&(L_edge[j / 2 - 3] == CAMERA_W - 3) && ((start -count_R*7+8- i - 2) >= 0) && (IMG_PIXEL(k,start-count_R*7+8-i) == 0)\
                                            && (IMG_PIXEL(k,start-count_R*7+8-i-1) == 0) && (IMG_PIXEL(k,start-count_R*7+8-i-2) == 0)) //向右同时为黑
                                    {
                                        L_edge[j / 2 - 3] = start - count_R*7+8 - i;
                                        R_edge[j / 2 - 3] = L_edge[j / 2 - 3] - distance;
                                        break;
                                    }
                                }
                            }
                            else
                            {
                                if ((flag_buxian_L == 0))
                                {
                                    if (flag_L == 0)
                                    {
                                        if ((start+i*8<CAMERA_W) && IMG_LONG_PIXEL(k,start+i*8) == 0)
                                            count_L++;
                                        else
                                        {
                                            flag_L = 1;
                                            count_L++;
                                        }
                                    }
                                    if ((flag_L == 1)&& (L_edge[j / 2 - 3] == CAMERA_W - 3) && ((start +count_L*7-8+ i + 2) < CAMERA_W) && (IMG_PIXEL(k,start+count_L*7-8+i) == 1)\
                                            && (IMG_PIXEL(k,start+count_L*7-8+i+1) == 1) && (IMG_PIXEL(k,start+count_L*7-8+i+2) == 1)) //向左三点同时为黑
                                    {
                                        L_edge[j / 2 - 3] = start+count_L*7-8 + i;
                                    }
                                }
                                if ((flag_buxian_R == 0))
                                {
                                    if (flag_R == 0)
                                    {
                                        if ((start-i*8-8>=0) && IMG_LONG_PIXEL(k,start-i*8-8) == 0)
                                            count_R++;
                                        else
                                        {
                                            flag_R = 1;
                                            count_R++;
                                        }
                                    }
                                    if ((flag_R == 1) && (R_edge[j / 2 - 3] == 2) && ((start -count_R*7+8- i - 2) >= 0) && (IMG_PIXEL(k,start-count_R*7+8-i) == 1)\
                                            && (IMG_PIXEL(k,start-count_R*7+8-i-1) == 1)&& (IMG_PIXEL(k,start-count_R*7+8-i-2) == 1)) //向右同时为黑
                                    {
                                        R_edge[j / 2 - 3] = start -count_R*7+8- i;
                                    }
                                }
                            }

                        }
                        else
                            break;
                    }
                }
                else if(flag_sancha == 1)//只向左扫,|| flag_cheku==2
                {
                    L_edge[j/2-3] = CAMERA_W - 5;
                    if(!flag_zaodian)
                    {
                        for (i = 0; i < CAMERA_W; i++)
                        {
//                        R_edge[j/2-3] = 2;
                            if ((flag_buxian_L == 0) && (L_edge[j / 2 - 3] == CAMERA_W - 5)) //還沒全找到就繼續找
                            {
                                /*先扫黑点*/
                                if (flag_black==1)
                                {
                                    if (flag_R == 0)
                                    {
                                        if ((start-i*8-8>=0) && IMG_LONG_PIXEL(k,start-i*8-8) == 0xFF)
                                            count_R++;
                                        else
                                        {
                                            flag_R = 1;
                                            count_R++;
                                        }
                                    }
                                    if ((flag_R == 1) && (L_edge[j / 2 - 3] == CAMERA_W - 3) && ((start - count_R * 7 + 8 - i - 2) >= 0) && (IMG_PIXEL(k,start-count_R*7+8-i) == 0)\
                                            && (IMG_PIXEL(k,start-count_R*7+8-i-1) == 0)&& (IMG_PIXEL(k,start-count_R*7+8-i-2) == 0)) //向右同时为黑
                                    {
                                        L_edge[j / 2 - 3] = start - count_R * 7 + 8 - i;
                                        R_edge[j / 2 - 3] = L_edge[j / 2 - 3] - distance;
                                        break;
                                    }

//                                    if ((flag_buxian_R == 0) && (R_edge[j / 2 - 3] == 2) && ((start - i - 2) >= 0) && (IMG_PIXEL(k,start-i) == 0)\
//                                            && (IMG_PIXEL(k,start-i-1) == 0)&& (IMG_PIXEL(k,start-i-2) == 0)) //向右同时为黑
//                                    {
//                                        L_edge[j / 2 - 3] = start - i;
//                                        break;
//                                    }

                                }
                                else
                                {
                                    if (flag_L == 0)
                                    {
                                        if ((start+i*8<CAMERA_W) && IMG_LONG_PIXEL(k,start+i*8) == 0)
                                            count_L++;
                                        else
                                        {
                                            flag_L = 1;
                                            count_L++;
                                        }
                                    }
                                    if ((flag_L == 1) && ((start + count_L * 7 - 8 + i + 2) < CAMERA_W)&& (IMG_PIXEL(k,start+count_L*7-8+i) == 1)\
                                            && (IMG_PIXEL(k,start+count_L*7-8+i+1) == 1)&& (IMG_PIXEL(k,start+count_L*7-8+i+2) == 1)) //向左三点同时为黑
                                    {
                                        L_edge[j / 2 - 3] = start + count_L * 7 - 8 + i;
                                        break;
                                    }
                                }

                            }
                            else
                                break;
                        }
                    }
                }
                else if(flag_sancha == 2)//只向右扫,|| flag_cheku==1
                {
//                    L_edge[j/2-3] = CAMERA_W - 3;
                    R_edge[j/2-3] = 5;
                    if(!flag_zaodian)
                    {
                        for (i = 0; i < CAMERA_W; i++)
                        {
                            if ((flag_buxian_R == 0) && (R_edge[j / 2 - 3] == 5)) //還沒全找到就繼續找
                            {
                                /*先扫黑点*/
                                if (flag_black==1)
                                {
                                    if (flag_L == 0)
                                    {
                                        if ((start+i*8<CAMERA_W) && IMG_LONG_PIXEL(k,start+i*8) == 0xFF)
                                            count_L++;
                                        else
                                        {
                                            flag_L = 1;
                                            count_L++;
                                        }
                                    }
                                    if ((flag_L == 1) && (R_edge[j / 2 - 3] == 2) && ((start + count_L * 7 - 8 + i + 2) < CAMERA_W)&& (IMG_PIXEL(k,start+count_L*7-8+i) == 0)\
                                            && (IMG_PIXEL(k,start+count_L*7-8+i+1) == 0)&& (IMG_PIXEL(k,start+count_L*7-8+i+2) == 0)) //向左三点同时为白
                                    {
                                        R_edge[j / 2 - 3] = start + count_L * 7 - 8 + i;
                                        L_edge[j / 2 - 3] = R_edge[j / 2 - 3] + distance;
                                        break;
                                    }
//                                    if ((flag_buxian_L == 0) && (L_edge[j / 2 - 3] == CAMERA_W - 3) && ((start + i + 2) < CAMERA_W) && (IMG_PIXEL(k,start+i) == 0)\
//                                            && (IMG_PIXEL(k,start+i+1) == 0) && (IMG_PIXEL(k,start+i+2) == 0)) //向左三点同时为白
//                                    {
//                                        R_edge[j / 2 - 3] = start + i;
//                                        break;
//                                    }

                                }
                                else
                                {
                                    if (flag_R == 0)
                                    {
                                        if ((start-i*8-8>=0) && IMG_LONG_PIXEL(k,start-i*8-8) == 0)
                                            count_R++;
                                        else
                                        {
                                            flag_R = 1;
                                            count_R++;
                                        }
                                    }
                                    if ((flag_R == 1) && ((start - count_R * 7 + 8 - i - 2) >= 0)&& (IMG_PIXEL(k,start-count_R*7+8-i) == 1)\
                                            && (IMG_PIXEL(k,start-count_R*7+8-i-1) == 1) && (IMG_PIXEL(k,start-count_R*7+8-i-2) == 1)) //向右同时为黑
                                    {
                                        R_edge[j / 2 - 3] = start - count_R * 7 + 8 - i;
                                        break;
                                    }
                                }

                            }
                            else
                                break;
                        }
                    }
                }
                if ((j==startline)&&(startline <= point))/*如果參考行小於piont，直接控制赛道的中点计算*/
                {
                    flag_m = 0;
                }
                if (flag_m)
                {
                    if ((L_edge[j / 2 - 3] < R_edge[j / 2 - 3]) && (j / 2 - 3))                         //防止左邊界讀到右邊界的右邊
                    {
                        L_edge[j / 2 - 3] = L_edge[j / 2 - 5];
                        R_edge[j / 2 - 3] = R_edge[j / 2 - 5];
                    }

                    if (((j / 2 - 3) % 2 == 0) && ((j / 2 - 3) > 0))                                       //從第六行開始求一階差分
                    {
                        if (flag_buxian_L == 0 )
                            L_edge_1[(j / 2 - 3) / 2 - 1] = L_edge[j / 2 - 3] - L_edge[j / 2 - 5];
                        if (flag_buxian_R == 0)
                            R_edge_1[(j / 2 - 3) / 2 - 1] = R_edge[j / 2 - 3] - R_edge[j / 2 - 5];

                        if ((R_edge_1[(j / 2 - 3) / 2 - 1] > 20) || (L_edge_1[(j / 2 - 3) / 2 - 1] < -20)) //一阶差分过大视为噪点，不再补线
                        {
//                            if(temp_L==0 && flag_cheku_temp==4 && j<80)//右出库找左噪点
//                                temp_L=L_edge[j / 2 - 5];
//                            else  if(temp_R==0 && flag_cheku_temp==3 && j<80)//左出库找右噪点
//                                temp_R=R_edge[j / 2 - 5];
                            if ((R_edge_1[(j / 2 - 3) / 2 - 1] > 40) || (L_edge_1[(j / 2 - 3) / 2 - 1] < -40)) //一阶差分过大视为噪点，不再补线
                            {
                                flag_zaodian = 1;
                                if(flag_sancha==1)
                                {
                                    L_edge[j / 2 - 3]=CAMERA_W - 5;
                                    L_edge[j / 2 - 4]=CAMERA_W - 5;
                                    middle_all[j / 2 - 4]= CAMERA_W - 5 - (int)(480.0-3.5*(j-2))/2;//正常赛道左右間隔估計值
//                                    L_edge[j / 2 - 5]=CAMERA_W - 5;
//                                    middle_all[j / 2 - 5]= CAMERA_W - 5 - (int)(480.0-3.5*(j-4))/2;//正常赛道左右間隔估計值
                                }
                                else if(flag_sancha==2)
                                {
                                    R_edge[j / 2 - 3]=5;
                                    R_edge[j / 2 - 4]=5;
                                    middle_all[j / 2 - 4]= 5 + (int)(480.0-3.5*(j-2))/2;//正常赛道左右間隔估計值
//                                    R_edge[j / 2 - 5]=5;
//                                    middle_all[j / 2 - 5]= 5 + (int)(480.0-3.5*(j-4))/2;//正常赛道左右間隔估計值
                                }
                            }
                        }

                        if (((j / 2 - 3) / 2 - 1) > 0)                                                   //從第七行開始求二階差分
                        {
                            if (flag_buxian_L == 0)
                                L_edge_2[(j / 2 - 3) / 2 - 2] = L_edge_1[(j / 2 - 3) / 2 - 1] - L_edge_1[(j / 2 - 3) / 2 - 2];
                            if (flag_buxian_R == 0)
                                R_edge_2[(j / 2 - 3) / 2 - 2] = R_edge_1[(j / 2 - 3) / 2 - 1] - R_edge_1[(j / 2 - 3) / 2 - 2];
                            /***********************************这里没丢括号*********************************************/
                        }
                    }
                }
            }

            if ((j >= 20)&&flag_m && flag_sancha == 0 && (flag_podao<=0 || flag_podao == 6) && !(flag_insancha == 1 && (((int)((float)s*s_xishu))<4500))) //從第七行開始求二階差分，范围是20行到起始行+10,&& (j <= startline + 20),&& flag_cheku!=1 && flag_cheku!=2

            {
                if (!flag_buxian_L)                                     //只能進一次
                {
                    if ((flag_zaodian==0 || flag_island_temp==12) && (L_edge_2[(j/2-3)/2-2] >= m)) //找左下斷點，范围是20行到起始行+5
                    {
                        if(L_duandian_1 == 0 && flag_island_temp==12 && (L_edge_2[(j/2-3)/2-2] >= m*2))
                            L_duandian_1 = j/2-5;
                        L_duandian1 = j/2-5;
                        if ((L_edge[L_duandian1]< CAMERA_W*15/16) &&(IMG_PIXEL(k + 3,L_edge[L_duandian1]-5) == 0)\
                                && L_edge_1[(j/2-3)/2-1] >0 && L_edge_1[(j/2-3)/2-2] < 2)     //第一個條件判斷是否噪點，角太小也不行&&(L_edge_1[j - 6]>0)&&(L_edge_1[j - 7]<0)，最後一個條件防止離路肩太近時補出竪綫 &&(L_edge[L_duandian1]<CAMERA_W*15/16)
                        {//&& (L_edge[L_duandian1 - 9] < CAMERA_W - 3)\
                            && (L_edge[L_duandian1 - 10] < CAMERA_W - 3) && (IMG_PIXEL(k + 1,L_edge[L_duandian1]-5) == 0)
                                if (j<startline && flag_island_temp!=2 && flag_island_temp!=3 && flag_island_temp!=12)//不是噪點才行,低于参考行才有意义  && ((flag_cheku >= -60 && flag_cheku <= -40)||(flag_cheku <= 0 && flag_cheku >= -20))
                                {
                                    for(daozhao = L_duandian1; daozhao >= 0;daozhao --)
                                    {
                                        if(L_edge[daozhao] > CAMERA_W - 5 || daozhao == 0)
                                        {
//                                            if(daozhao==0)
//                                                L_xielv = ((double)(L_edge[L_duandian1] - (CAMERA_W - 5))) / ((L_duandian1 - daozhao)); //逐差法求左斜率，此處即要求j/2-4-4>=5
//                                            else
                                            L_xielv = ((double)(L_edge[L_duandian1] - L_edge[daozhao])) / ((L_duandian1 - daozhao)); //逐差法求左斜率，此處即要求j/2-4-4>=5
                                            flag_buxian_L = 1;
                                            flag_buxian_L_temp=1;
                                            if (temp_L == 0 && flag_cheku_temp == 3 && j<80) //左出库找左断点
                                                temp_L = L_edge[L_duandian1 - 4];
                                            j -= 4; //准确补线
                                            break;
                                        }
                                    }

//                                    if((L_edge[L_duandian1]<startline)&&(flag_island_temp<=0)&&(L_edge_2[(j/2-3)/2-2] >= m*4))//二阶差分大，则视为十字，防止三叉误触
//                                    {
//                                        s=0;
//                                        flag_island=16;
//                                    }

                                }
                        }
                    }
                }
                else
                {

                    if (flag_island_temp == 1)
                        L_edge[j / 2 - 3] = H_duandian * 8 + (j / 2 - 3 - L_duandian1) * L_xielv;            //入环左補綫
                    else
                        L_edge[j / 2 - 3] = L_edge[L_duandian1 - 1] + (j / 2 - 3 - L_duandian1) * L_xielv;     //左補綫

                    if((L_edge[j/2-3]<CAMERA_W-3)&&(L_edge[j/2-3]>2)&&(IMG_PIXEL(k,L_edge[j/2-3]) == 1)&&(!(End_L))&&((L_duandian1+4)*2+10<j))//遇到黑点，即说明补到上边界，最後一個條件防止一出來就撞點上
                        End_L=j+5;

//                    if((L_edge[j/2-3]>(MID-4))&&(L_edge[j/2-3]<(MID+4)))          //補綫后的上邊界也要更新
//                        U_edge[19]=k;
                }

                if (!flag_buxian_R)
                {
                    if ((flag_zaodian == 0 || flag_island_temp==12) && (R_edge_2[(j/2-3)/2-2] <= -m))   //找右下斷點，范围是20行到起始行加+5
                    {
                        if(R_duandian_1 == 0 && flag_island_temp==12 && (R_edge_2[(j/2-3)/2-2] <= -m*2))
                            R_duandian_1 = j/2-5;
                        R_duandian1 = j/2-5;
                        if ((R_edge[R_duandian1]>  CAMERA_W/16) &&(IMG_PIXEL(k+ 3,R_edge[R_duandian1]+5) == 0)\
                                && R_edge_1[(j/2-3)/2-1] < 0 && R_edge_1[(j/2-3)/2-2] > -2)    //第一個條件判斷是否噪點,&&(R_edge_1[j - 6]<0)&&(R_edge_1[j - 7]>0)  (R_edge[R_duandian1]>CAMERA_W/16)
                        {//&& (R_edge[R_duandian1 - 9] > 2)\
                            && (R_edge[R_duandian1 - 10] > 2) && (IMG_PIXEL(k+ 1,R_edge[R_duandian1]+5) == 0)
                                if (j<startline  && flag_island_temp!=1 && flag_island_temp!=4 && flag_island_temp!=12) //&& ((flag_cheku >= -60 && flag_cheku <= -40)||(flag_cheku <= 0 && flag_cheku >= -20))
                                {
                                    for(daozhao = R_duandian1; daozhao >= 0;daozhao --)
                                    {
                                        if(R_edge[daozhao] < 5 || daozhao == 0)
                                        {
//                                            if(daozhao==0)
//                                                R_xielv = ((double) (R_edge[R_duandian1] - 5)) / ((R_duandian1 - daozhao)); //逐差法求右斜率，此處即要求j-6-7>=5
//                                            else
                                            R_xielv = ((double) (R_edge[R_duandian1] - R_edge[daozhao])) / ((R_duandian1 - daozhao)); //逐差法求右斜率，此處即要求j-6-7>=5
                                            flag_buxian_R = 1;
                                            flag_buxian_R_temp=1;
                                            if (temp_R == 0 && flag_cheku_temp == 4 && j<80) //右出库找右断点
                                                temp_R = R_edge[R_duandian1 - 4];
                                            j -= 4; //准确补线
                                            break;
                                        }
                                    }
//                                    if((R_edge[R_duandian1]<startline)&&(flag_island_temp<=0)&&(R_edge_2[(j/2-3)/2-2] <= -m*4))//二阶差分大，则视为十字，防止三叉误触
//                                    {
//                                        s=0;
//                                        flag_island=16;
//                                    }

                                }
                        }
                    }
                }
                else
                {
                    if (flag_island_temp == 2)
                        R_edge[j / 2 - 3] = H_duandian * 8 + (j / 2 - 3 - R_duandian1) * R_xielv;            //入环右補綫
                    else
                        R_edge[j / 2 - 3] = R_edge[R_duandian1 - 1] + (j / 2 - 3 - R_duandian1) * R_xielv;     //右補綫

                    if((R_edge[j/2-3]<CAMERA_W-3)&&(R_edge[j/2-3]>2)&&(IMG_PIXEL(k,R_edge[j/2-3]) == 1)&&(!(End_R))&&((R_duandian1+4)*2+10<j))//遇到黑点，即说明补到上边界，最後一個條件防止一出來就撞點上
                        End_R=j+5;
//                    if((R_edge[j/2-3]>(MID-4))&&(R_edge[j/2-3]<(MID+4)))          //補綫后的上邊界也要更新
//                        U_edge[19]=k;
                }
            }
            if (flag_m)
            {

                if ( (flag_podao == 0 || flag_podao == 6 || flag_podao == 3) &&(flag_buxian_R+flag_buxian_L<2)) //坡道關閉預測補綫 || (flag_island_temp>2)
                {
                    R_temp = R_edge[j / 2 - 3]; //不管補不補綫都暫存邊界數據
                    L_temp = L_edge[j / 2 - 3];

                    if ((R_edge[j / 2 - 3] < liubian + 1) && (L_edge[j / 2 - 3] < CAMERA_W - 2 - liubian)) /********預測補綫******/
                    {
                        R_edge[j / 2 - 3] = L_edge[j / 2 - 3] - (int) distance;
//                    if((R_edge[j/2-3]>(MID-5))&&(R_edge[j/2-3]<(MID+5)))          //補綫后的上邊界也要更新
//                        U_edge[19]=j-5;
                    }
                    else if ((R_edge[j / 2 - 3] > liubian + 1) && (L_edge[j / 2 - 3] > CAMERA_W - 2 - liubian))
                    {
                        L_edge[j / 2 - 3] = R_edge[j / 2 - 3] + (int) distance;
//                    if((L_edge[j/2-3]>(MID-5))&&(L_edge[j/2-3]<(MID+5)))          //補綫后的上邊界也要更新
//                        U_edge[19]=j-5;
                    }

                    middle_all[j / 2 - 3] = (L_edge[j / 2 - 3] + R_edge[j / 2 - 3]) / 2;           //每一行都更新中点

//                    if (middle_all[j / 2 - 3] > CAMERA_W - 1 - liubian)                            //限幅
//                        middle_all[j / 2 - 3] = CAMERA_W - 1 - liubian;
//                    else if (middle_all[j / 2 - 3] < liubian)
//                        middle_all[j / 2 - 3] = liubian;

                    L_edge[j / 2 - 3] = L_temp;       //回復邊界數據
                    R_edge[j / 2 - 3] = R_temp;

                }
                else
                {
                    if ((L_edge[j / 2 - 3] <= R_edge[j / 2 - 3]))                                 //防止左邊界讀到右邊界的右邊
                    {
                        if ((j / 2 - 3) > 0)
                        {
                            L_edge[j / 2 - 3] = L_edge[j / 2 - 5];
                            R_edge[j / 2 - 3] = R_edge[j / 2 - 5];
                        }
                        else
                        {
                            //第一行就讀反了，只好賦囘初值了
                            L_edge[j / 2 - 3] = CAMERA_W - 3;
                            R_edge[j / 2 - 3] = 2;
                        }
                    }

                    middle_all[j / 2 - 3] = (L_edge[j / 2 - 3] + R_edge[j / 2 - 3]) / 2;           //每一行都更新中点

                }

                if (middle_all[j / 2 - 3] > CAMERA_W - 1 - liubian)                            //限幅
                    middle_all[j / 2 - 3] = CAMERA_W - 1 - liubian;
                else if (middle_all[j / 2 - 3] < liubian)
                    middle_all[j / 2 - 3] = liubian;

                if (flag_scan == 1 || (flag_buxian_R || flag_buxian_L))
                {
                    start = middle_all[j / 2 - 3];
                }

                if (L_edge[j / 2 - 3] > CAMERA_W - 1 - liubian)                            //限幅
                    L_edge[j / 2 - 3] = CAMERA_W - 1 - liubian;
                else if (L_edge[j / 2 - 3] < liubian)
                    L_edge[j / 2 - 3] = liubian;

                if (R_edge[j / 2 - 3] > CAMERA_W - 1 - liubian)                            //限幅
                    R_edge[j / 2 - 3] = CAMERA_W - 1 - liubian;
                else if (R_edge[j / 2 - 3] < liubian)
                    R_edge[j / 2 - 3] = liubian;
            }
        }
    }

    if(flag_buxian_L==0)
        flag_buxian_L_temp = 0;
    if(flag_buxian_R==0)
        flag_buxian_R_temp = 0;

    if (flag_island > 0 || (flag_podao >0 && flag_podao != 6))
    {
        flag_scan = 0;
    }
//    else if ((startline) < (xiang - 20))//弯道中间值循迹
//    {
//        flag_scan = 1;
//    }
    else
//        flag_scan = flag_scan?0:1;
        flag_scan=1;
/*
 * huandao
 */
    if (flag_island_temp == 12)
    {
        if (((abs((R_duandian_1 + 4) * 2 - U_edge[0] + DOWN_SEE) < abs((L_duandian_1 + 4) * 2 - U_edge[39] + DOWN_SEE) && R_duandian_1 && L_duandian_1) ||\
                (R_duandian_1 && !L_duandian_1))&& U_edge[0] > MAX_SEE/2 + DOWN_SEE)          //(R_duandian_1 + 4) * 2 <= (startline + 10) &&
        {
            flag_island = 1;
            s = 0;
//                    gpio_set(P13_3, 0);
        }
        else if (((abs((R_duandian_1 + 4) * 2 - U_edge[0] + DOWN_SEE) > abs((L_duandian_1 + 4) * 2 - U_edge[39] + DOWN_SEE) && R_duandian_1 && L_duandian_1) ||\
                (!R_duandian_1 && L_duandian_1))&& U_edge[39] > MAX_SEE/2 + DOWN_SEE)          //(L_duandian_1 + 4) * 2 <= (startline + 10) &&
        {
            flag_island = 2;
            s = 0;
//                    gpio_set(P13_3, 0);
        }
    }
    if ((flag_island_temp == 3) && flag_buxian_R  && (((int)((float)s*s_xishu)) > 5000) &&( (R_edge[R_duandian1] < CAMERA_W / 3)|| ((R_duandian1+4)*2 < MAX_SEE/3)))/* && (R_duandian1 <= 75)&&(R_edge[R_duandian1]<80)*/ //若右邊綫出現尖角，則開始延時打角
    {
        int x,count_break = 0;
        for (x = 8; x < 160; x += 8)
        {
            if (IMG_LONG_PIXEL((R_duandian1+4)*2+6 , 159-x) == 0)
            {
                count_break++;
            }
        }
        if(count_break > 15)
        {
            s = 0;
            flag_island = 5;
        }//尖角还在就不用打死 && (R_edge[R_duandian1+1]<10 || R_edge[R_duandian1+3]<10 || R_edge[R_duandian1+5]<10)
    }
    else if ((flag_island_temp == 4) && flag_buxian_L && (((int)((float)s*s_xishu)) > 5000) && ((L_edge[L_duandian1] > CAMERA_W *2 / 3)|| ((L_duandian1+4)*2 < MAX_SEE/3))) //若左邊綫出現尖角，則開始延時打角
    {
        int x, count_break = 0;
        for (x = 8; x < 160; x += 8)
        {
            if (IMG_LONG_PIXEL((L_duandian1+4)*2+6 , 159+x) == 0)
            {
                count_break++;
            }
        }
        if (count_break > 15)
        {
            s = 0;
            flag_island = 6;
        } // && (L_edge[L_duandian1+1]>CAMERA_W-10 || L_edge[L_duandian1+3]>CAMERA_W-10 || L_edge[L_duandian1+5]>CAMERA_W-10)
    }

//    if((flag_island_temp == 5) && !flag_buxian_R && ((U_edge[39]<10+ DOWN_SEE && U_edge[38]<10+ DOWN_SEE)|| (U_edge[18] < 20+ DOWN_SEE && U_edge[19] < 20 + DOWN_SEE && U_edge[20] < 20+ DOWN_SEE)) )// || (U_edge[18] < 10 && U_edge[19] < 10 && U_edge[20] < 10)
//    {
//        flag_island = -1;
//        s=0;
//    }
//    else if((flag_island_temp == 6) && !flag_buxian_L && ((U_edge[0]<10+ DOWN_SEE && U_edge[1]<10+ DOWN_SEE)|| (U_edge[18] < 20+ DOWN_SEE && U_edge[19] < 20+ DOWN_SEE && U_edge[20] < 20)+ DOWN_SEE ))//|| (U_edge[18] < 10 && U_edge[19] < 10 && U_edge[20] < 10)
//    {
//        flag_island = -1;
//        s=0;
//    }

//                if ((flag_island_temp == 7) && flag_buxian_R == 0)/* && (R_duandian1 <= 75)&&(R_edge[R_duandian1]<80)*/ //若右邊綫出現尖角，則開始延時打角
//                {
//                    s = 0;
//                    flag_island = 5;     //尖角还在就不用打死，没了再打死
//                }
//                else if ((flag_island_temp == 8) && flag_buxian_L == 0) //若左邊綫出現尖角，則開始延時打角
//                {
//                    s = 0;
//                    flag_island = 6;
//                }

    /*
     * 崭新出环方式：当两边界全白的时候就该出环了，这时候打死
     * 出环之后再等其中对应的边界有一大半变黑，就取消打死
     */
//    if (s >4000 && (flag_island_temp == 3) && L_edge[5] > 315 && L_edge[10] > 315 && L_edge[15] > 315\
//            && L_edge[20] > 315 && L_edge[25] > 315 && L_edge[30] > 315 && R_edge[5] <3 \
//            && R_edge[10] <3 && R_edge[15] <3 && R_edge[20] <3 && R_edge[25] <3 && R_edge[30] <3 )/*&& (R_duandian1 <= 75)*/     //若右邊綫出現尖角，則開始延時打角
//    {
//        s = 0;
//        flag_island = 5;
//    }
//    else if (s >4000 && (flag_island_temp == 4) && L_edge[5] > 315 && L_edge[10] > 315 && L_edge[15] > 315\
//                && L_edge[20] > 315 && L_edge[25] > 315 && L_edge[30] > 315 && R_edge[5] <3 \
//                && R_edge[10] <3 && R_edge[15] <3 && R_edge[20] <3 && R_edge[25] <3 && R_edge[30] <3 )/*&& (R_duandian1 <= 75)*/     //若左邊綫出現尖角，則開始延時打角
//    {
//        s = 0;
//        flag_island = 6;
//    }

    if (flag_m)
    {
        for (i=5,j = -2; j < 3; j++)                                                           //求5行的中间值
        {
            if((L_edge[(startline)/2-3 + j] == CAMERA_W-1 - liubian)&&(R_edge[(startline)/2-3 + j] == liubian))                    //未找到邊界時不參與計算
                i--;
            else
                mid_sum += middle_all[(startline)/2-3+ j];
        }
        if(i>0)
            mid_eve = mid_sum / i;
        else
            mid_eve=mid_temp;                               //若無有意義的參考行，取中間值

        mid_sum=0;

        for (i=5,j = -2; j < 3; j++)                                                           //求5行中间值的斜率
        {
            if ((L_edge[(startline)/2-3 + j] == CAMERA_W-1 - liubian) && (R_edge[(startline)/2-3 + j] == liubian))                    //未找到邊界時不參與計算
                i--;
            else
                mid_sum += middle_all[(startline)/2-3 + j + 1] - middle_all[(startline)/2-3 + j];
        }
        if(i>0)
            Middle_xielv = ((double)mid_sum) /(i);    //求中間參考行斜率
        else
            Middle_xielv=0;
    }

    Mid_H=U_edge[19] - D_edge[19];//中间行高度

    if(Middle_xielv>=3)             //最多只能影響40點
        Middle_xielv=3;
    else if(Middle_xielv<=-3)
        Middle_xielv=-3;

    if (mid_eve > CAMERA_W-1 - liubian)
        mid_eve = CAMERA_W-1 - liubian;
    else if (mid_eve < liubian)
        mid_eve = liubian;

    /*
     * 入库，补线标志位
     */

    if(flag_podao == 6 &&(( s > 3000) || flag_countsancha == sancha_num))
        sancha_ruku(flag_island_temp,End_R, End_L);             //三叉及入库

    if(((adc1+adc4+adc2+adc3)>2000) && (!flag_sancha)&&(!flag_insancha)&&((flag_cheku == -40) || (flag_cheku == 0)))//防止三叉误触
        podao(flag_island_temp);    /*坡道检测控制*/
//    if(((((flag_sancha)||(flag_insancha))&&flag_podao != 6)||flag_island_temp > 0))
//        flag_podao=0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072

---

● 相关图表链接:

• 图2.1智能汽车B3车模
• 图2.2 编码器安装示意图
• 图片3.1稳5.658电源模块
• 图片3.2 5V稳压模块
• 图图片3 3 3.3V稳压模块
• 图图片3 5串口通信原理图