智能车技术报告中国地质大学地大一对技术报告

1、第三届“飞思卡尔”杯全国大学生智能汽车邀请赛技 术 报 告学 校：中国地质大学（武汉）队伍名称：地大一对参赛队员：邹国雄庞建东何东带队教师：王勇关于技术报告和研究论文使用授权的说明 本人完全了解第三届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名： 带队教师签名： 日 期：目录第一章 引言11.1智能汽车制作概述11.2控制算法概述11.3引用文献概述11.4报告的

2、主要内容2第二章 模型车机械部分安装及改造32.1传感器的安装32.2 主控板的安装32.3光电码盘的安装4第三章 电路设计说明53.1电源电路53.2 电机驱动电路53.3 舵机驱动电路63.3.1 电源电路6信号连接电路63.4 主控办接口73.5 传感器电路7第四章 智能车的控制94.1 主要代码分析9第五章 车体调速方法115.1 智能车的质量对性能的影响115.2闭环调速125.3变速前进12第六章 结论14参考文献15附 录IA：程序流程IB：程序源代码II第一章 引言1.1智能汽车制作概述 为了不影响智能车的整体性能，在制作过程中并未对车体结构作大的改动，如：没有为安装电路板而特

3、殊钻孔，没有安装特殊的支架等。但为了安装光电传感头，我们增加两片铁片将前端光电传感器固定。在车体的后部增加了码盘以确定速度。车体前端安装第二排红外传感器，用于寻找引导线，通过对安装孔的调整，直接安装在车体前端的安装孔上。智能车的控制板安装在车体的后部，通过调整安装孔的位置和大小，直接安装在车体上方的原有的两个螺栓上，并通过车体自带的支架固定。单片机最小系统板通过排针插装在控制板上。红外线传感器的信号通过10pin的排线连接到控制板上，舵机的控制线和电源线通过3pin的排线连接到控制板的相应的接口上。为了能提高智能车的行进速度，在设计电路板时尽量减小了PCB板的使用面积，同时简化了电路的设计，在

4、不影响电路功能的前提下，减少了元器件的数量和体积。1.2控制算法概述智能车要在引导线的引导下快速行进，传感器的采样周期的大小对车体随动性能将会有很大的影响。当采用周期足够大时，数字离散系统的设计可以按照连续系统的设计思想设计，而且系统的输出性能也和连续系统近似。因此决定采用计算方便位置PID控制算法，在对实际的有智能车、电机、舵机、单片机等部分组成随动系统进行实际调试后发现，PID算式的微分部分对提高的系统的各项性能指标没有明显的作用，选取不当，还会使系统不稳定，因此只保留了比例部分和积分部分。为了提高快速性和稳定性，通过对算法的调整，加入了具有饱和特性、死区特性等特性的环节。1.3引用文献概

5、述为了能让智能车有较高的平均行进速度，控制方法主要参考了由胡寿松主编的自动控制原理和陶永华主编的新型PID控制及其应用两本书。通过试验进行相应的校准。1.4报告的主要内容本文将对智能车的制作及其控制方法的C语言实现进行论述。第一章是对全文的概述；第二章将简述模型车机械部分安装及改造、传感器的设计安装、系统电路板的固定及连接等；第三章将简述电路设计说明；第四章中主要介绍智能车的控制方法以及算法的C语言实现；第五章简要论述车体质量、速度调整方法等因素对智能车性能的影响；第六章是结论。第二章 模型车机械部分安装及改造智能车上的各部分出了光电码盘需要在车体的后部钻两个孔作为安装孔，其余所有电路板都安装

6、在车体原有的安装孔和支架上。2.1传感器的安装传感器直接安装在智能车前端的用于安装支架的安装孔上，如图：图2.1 传感器的安装2.2 主控板的安装智能车采用单片机HCS12开发板作为控制板，主控制板是用来连接单片机和外围各个功能模块的桥梁。它为各个模块提供相应的接口。根据2.1节中的叙述，将主控制板安装在智能车的后部，并且利用模型原有的安装支架，如图：图2.2 主板的安装2.3光电对管的安装用光电对管测量车速图2.3 光码盘的安装第三章 电路设计说明3.1电源电路智能车采用的镍铬充电电池供电，电池电压为7.2V，为了能为各个用电设备提供稳定的供电电压，选用低压差稳压集成芯片mc33886为系统

7、供电。由于传感器的耗电量较大，所以单独为期配备一路电源电路。图3.1电源电路3.2 电机驱动电路电机的驱动电路有很多种，这里使用的是单管驱动电路。这个电路实现简单，发热少，因此不用很大的散热片，驱动能力很强。图3.2电机驱动电路3.3 舵机驱动电路堆积驱动电路包括两部分，一部分是电源电路，另一部分是信号连接电路。3.3.1 电源电路为了实现方便，使用元器件少，不占用太多的电路板使用面积，同时舵机对电源要求很低，采用二极管降压的方法为舵机提供电源。信号连接电路为了降低电机对单片机的干扰，使用光电耦合器作信号隔离。3.4 主控办接口主控制板主要完成各个功能电路的连接工作，因此需要配备所有的模块的接

8、口电路，主要有传感器数据输入接口、电机驱动信号输出接口、舵机驱动信号输出接口、单片机最小系统板接口。接口用直立插针实现。图3.4主板接口电路3.5 传感器电路智能车需要沿着黑色引导线在白色的跑道上行驶，传感器的稳定性直接决定智能车能否按照正确的轨迹行进。可见光传感器容易受到光照强度的影响，因此必须采用不易收环境可见光强度影响的红外线传感器。这里使用的红外线传感器的型号是JX359，JX359对黑色和白色的分辨能力很强，用它来做寻找引导线的传感器具有很好的稳定性。图3.5传感器电路第四章 智能车的控制4.1 主要代码分析PID控制是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，

9、被广泛应用于工业控制2。因此采用数字PID控制算法作为智能车的运动控制算法。若把智能车看做是一个关心环节，那么他的时间常数将会相对较大，因此PID控制算法中的微分环节对改善系统的性能不会有太大的贡献，最终，对智能车的运动控制采用数字PI控制。数字PI算式中，将积分环节作了处理，积分时间并不是从0开始，而是从一个积分周期前到当前时刻，积分时间常数取30ms。dst = dst0 - (Kp*e + Ki*(e+eold);其中dst0为控制量为0时的PI控制器的输出，Kp为比例系数，Ki为积分系数。程序中dst04650，Kp70，Ki3。为了能使智能车的速度能够在各种情况下都能够达到或者接近理

10、想的最高速度，在速度控制上采用变速控制的方法。由于控制前轮转动的舵机的转动速度是固定不变的，因此当引导线相对车体的位置变化角速度大于前轮的转动速度后，智能车将无法稳定的跟踪引导线。在引导线的位置变化较大时，降低车速是十分必要的。速度的控制算法是按照传感器的输出的变化率作为减速与否的标准，具体方法如下：首先，单片机对从传感器采集回的位置信号进行采样，采样周期为30ms。舵机的转动角速度常数为：在工作的电压为4.8V 的情况下旋转60度耗时0.2秒即300度每秒。与其相应的引导线相对传感器每移动过一个传感器耗时不应超过6.67ms。当采样周期为30ms时，每个周期内，传感器输出的偏差变化值（程序中

11、定义为deltae）应小于4。然后，如果deltae大于或等于4，则减速，否则加速。减速为突然减速，而加速采用逐渐家速度方法以防止电机因突加给定而电流过大。相应的调速代码如下：if (time <= AbsoluteTime)deltae = e - eold;if (deltae < 0) deltae = -deltae;if (deltae < 3)speed+;if (speed > 13)speed = 13;if (deltae >= 3) speed = 2;eold = e;time += 30;第五章 车体调速方法5.1 智能车的质量对性能的影响

12、参加本次智能车大赛的队伍将进行一场速度赛，因此提高智能车的速度是本次比赛的关键所在。首先，车体的质量将会影响智能车的行进速度。车体质量的大小直接决定了智能车在行进过程中受到的滚动摩擦力的大小，车体的质量越大，滚动摩擦阻力将越大。过大的质量所引起的车轮滚动摩擦力的增加，不仅会使智能车的起动速度降低，同时也会降低智能车的最高前进线速度。其次，车体质量和重心的位置将影响智能车的转向性能。假设车体的重心在智能车车体的前端，那么，在智能车转弯时，前轮将承受较大的摩擦力，同时，前轮将分担较大的向心力为车体提供向心加速度，这将导致智能车转向灵活度降低，降低智能车进入弯道的线速度（线速度过高，会因转向不灵活而

13、冲出跑道），从而影响比赛的成绩。为了提高智能车的行进速度和转向的灵活程度，在智能车的制作上，我们尽量减小了车体的质量，同时将重心先后移动。具体实现方法如下：我们在设计传感器的PCB板和主控板时，特别注意在不影响系统的稳定性的基础上，尽量较小电路板的使用面积。电路设计上，为了减小使用面积，在完成电路功能的基础上，使用了尽量少的元器件。没有为主控制板和传感器的电路板安装特别的安装支架，而是将他们直接安装在车体原有的安装孔上。图5.15.2闭环调速为了能对智能车的行进速度进行准确的控制，并且能方便的实现加速和减速动作，采用速度闭环控制是十分必要的。典型的闭环调速系统通常采用双闭环调速系统，但是双闭环

14、调速系统在实现时相对比较复杂，同时会增加CPU的工作负担，使控制系统的采样频率受到影响。因此采用了容易实现的单闭环调速，即只用速度环。5.3变速前进正常驾驶汽车时司机都会在弯道时减速，以防止在弯道时由于车身的惯性，导致侧滑。参加本次比赛所使用的智能车虽然一般不会因为惯性在突然转向时发生侧滑，但是如果智能车在行进路径中的直线加速过程中获得了很大的动能，进入弯道的瞬间难免会因为车体惯性而冲出跑道。所以智能车的速度不能是一成不变的，应该在有弯道的地方减速。那么，减速的依据是什么、速度应该降低多少、弯道内维持多大的线速度等主管因素将直接影响智能车的平均行进速度。在进入弯道后，智能车能否正确的沿着引导线

15、前进，主要依赖控制前轮转向的舵机能否在有限的时间里转动得到指定的位置。舵机有它的固有响应速度参数：在工作的电压为4.8V 的情况下旋转60度耗时0.2秒即300度每秒，那么在速度调整周期内，如果引导线偏离智能车车体前端传感器中间位置相对车体的角速度大于300度每秒，舵机将来不及对传感器信号做出反应，智能车将冲出跑道。因此，影响智能车转向性能的因素不是弯道的存在，而是引导线位置相对车体变化的角速度。根据以上的分析，智能车能否沿引导线顺利的入弯、出弯，取决于车速的调整规律能否保证在引导线出现变化时，引导性位置变化相对车身的角速度小于300度每秒。所以在智能车前进过程中，弯道的出现不是减速标志，减速

16、与否完全取决于舵机的响应速度。加速减速的具体实现将在程序分析中具体说明。第六章 结论智能车车身的加工制作过程中，尽量的减轻了车体的质量，并后移了中心，从而提高了智能车的灵活度。经过试验，重心在智能车的后部要比在前部车速有了明显的提高，稳定性也有了一定的提高。智能车的控制方法采用了实现简单的控制方法，在程序实现上减小了时间及空间复杂度，进而提高了传感器对道路的采样频率。经过调试，只能车的平均速度可以达到1/左右，路径的形式及弯道的多少，对平均速度的影响很大。智能车使用的是红外线传感器来检测引导线的位置。红外线传感器的采样频率较高，配合有效的转换电路，能够输出稳定性很高的引导线位置信号。但是传感器

17、本身性质决定了它不能够对远距离的路况信息进行解读，因此也就不能够进行有关道路信息的预先判断。只能从采样频率上补偿信息量少的弊端。在下一步开发中，我们准备用线阵作为检测引导线的检测设备。线阵可以对相对较远的道路信息进行采集，而且分辨率高。基于线阵的道路检测可以对引导线的变化进行预先判断和精确的位置微分，为控制算法的实现会提供很大的帮助。参考文献1 自动控制原理主编：胡寿松 科学出版社出版2 新型PID控制及其应用 主编：陶永华 机械工业出版社附 录系统初始化YesNo读取传感器位置偏差信号e位置处理计算前轮转角量，并输出给舵机计算延时Deltae>=3？减速30ms加速若达到速度上限则停止

18、加速A：程序流程B：程序源代码#include <hidef.h> /* common defines and macros */#include <mc9s12dg128.h> /* derivative information */#include "1602.h" #include "PID.h" #include "Sensend.h" #include "main_asm.h" /* interface to the assembly module */#pragma LINK_I

19、NFO DERIVATIVE "mc9s12dg128b" #define uchar unsigned char #define uint unsigned int#define con0 PORTA_BIT4#define con1 PORTA_BIT7#define KEY1 PORTE_BIT0#define KEY2 PTH_PTH2#define KEY3 PTH_PTH0#define KEY4 PTH_PTH1#define D1 PTP_PTP2#define MCIN1 PTP_PTP3#define MCIN2 PTP_PTP4#define Ctr2

20、 PORTA_BIT6#define left_max 1900/1825/1900 /1700/1800/1950#define right_max 2700/2550/2650#define middle 2290/2210 /2250/2240#define e_turn 38/偏差转折点#define k1 1.5/1.5/舵机微分常数#define k2 0.8/直道加速段比率常数/0.7#define k3 0.3/弯道入直提前加速比率#define stright_limit 15/ 直道忽略阈值#define stright_v 240/直道速度#define v0 150/恒

21、速/#define Kv 0.8/0.8/弯道速降常数uchar pos_middle= 113;uchar AD_wData10; /全局变量存放 AD的结果uchar low_value10;/AD低基值uchar high_value10;/AD高基值uchar value_range10;/AD值变化范围uint AD_cData10; /AD偏差值uint stright_turn10;/直入弯地址uchar position=114;/黑线中心位置int pos8; / 黑线中心缓冲区uchar Angle8;/转角缓冲区uchar turn_save3000;/道路信息uchar

22、 road_v3000;/速度分配uchar n=0;/道路信息所分段数uchar p=0;/ 直入弯点数uint sum=0;/转角之和int angle_sum=0;/转角偏差之和uint angle ;uchar flag=0;uchar time_flag=0; /AD采样时间分段标志uchar white_flag=0;uchar stright_flag=0;/直线标志uchar memor_flag=0;uchar turn_flag1=0;uchar pos0;/和position等同int coun1=0;/测速脉冲个数uint time_count=0;/测速高频脉冲0.1

23、msuint speed=0;/实时车速uint time_100us=0;uint time_100ms=0;signed int e2;/位置偏差uchar start_cout=0;/起跑线次数uchar start_time=0;/一段时间内检测到的起跑线次数uchar start_flag=1;uchar last_maxvalue;uchar last_max=4;uchar PID_flag=1;int num;/第一圈脉冲数int num2;/第二圈脉冲数int time1;/第一圈时间int time2;/第二圈时间uchar circuit_flag=0;/一圈跑完标志uc

24、har Brake_flag=0;/刹车标志uchar spee1=0; /第一圈速度调节uchar spee2=0; /第二圈速度调节uchar project_flag1=0;/方案选择标志1uchar project_flag2=0;/方案选择标志2uchar project_flag3=0;/方案选择标志3uchar stright_flag1=0;/直线标志/int stright_con=30000;/uchar turn_flag=0;/uchar longsright_flag=0;/uchar turn_count=0;/uchar stright_count=0;/stru

25、ct road_inf /道路信息分段结构体 char road_flag;/直弯标志 uint turn_sum;/积分值 uint start; /起始位置 uint end ; /结束位置 uchar vv; /入弯/直速度road50;void delay(uint t) uint i; for (i=0;i<t;i+);void delay400ms(void) uint i; for (i=0;i<16;i+) delay(50000);void delay5s() uint i; for (i=0;i<100;i+) delay(50000);void getA

26、D0_value(void) AD_wData1 = ATD0DR0; /将结果寄存器中的值存放到数组中 AD_wData3 = ATD0DR1; AD_wData5 = ATD0DR2; AD_wData7 = ATD0DR3; AD_wData9 = ATD0DR4; void getAD1_value(void) AD_wData0 = ATD1DR0; /将结果寄存器中的值存放到数组中 AD_wData2 = ATD1DR1; AD_wData4 = ATD1DR2; AD_wData6 = ATD1DR3; AD_wData8 = ATD1DR4; void cpu_init(voi

27、d) SYNR=2; /S 16MHZ超频 REFDV=1; while(CRGFLG_LOCK=0); CLKSEL=0x80; DDRE=0xfc; DDRM=0xff; DDRB= 0xff;/0xe3; DDRP=0xff; DDRH=0xf8; DDRJ=0x3c;PORTB= 0x0f; DDRA=0xd0; /PORTA=0x20; asm nop static void MDCInit(void) MCCTL = MCCTL&0Xfb; /模数计数器禁止运行 MCCTL = 0Xe3; /允许中断，模数计数方式 /返回时重新加载所用的常数，分频常数为1 MCCTL =

28、MCCTL|0X04; /模数计数器使能 MCCNT = 0X1000; MCCTL = MCCTL|0X08 ; /把模数常数寄存器的值加载到模数计数器 void speed_init() TIOS_IOS0=0; TCTL4_EDG0A=1; TCTL4_EDG0B=0; TIE_C0I=1;void Init_ECT()TIOS_IOS7=1;TCTL1_OM7=0;TCTL1_OL7=0;TIE_C7I = 1; /tc1 开中断TSCR1= 0X80; /enable main timerTSCR2 = 0x0d;TFLG1_C7F=1;TC7=75; /中断 100微秒 = 75*

29、32/24/1000000void InitMotorPWM(void) PWME_PWME2=0; PWMCLK_PCLK2=1; /CHOOSE SB PWMSCLB=32; PWMDTY2=25; PWMPER2=200; PWMPOL_PPOL2=1; PWMCAE_CAE2=0; void InitServoPWM(void) PWME=0x00; PWMCTL_CON01=1; PWMPER01=12000; /PER=8000 PWMDTY01=2250; /DTY=1500 PWMCLK_PCLK1=0; /SOURCE IS CLOCK A PWMPRCLK=0x44; /P

30、RE=16 PWMPOL_PPOL1=1; /HIGH BEGIN PWME_PWME1=1;void InitSerialPort(void) SCI0BDH=0; SCI0BDL=13; SCI0CR2=0x2c;void Send(unsigned char dat) while(!SCI0SR1_TDRE); SCI0DRL=dat; void run(uchar v1)PWMDTY2=v1;/D1=0;PWME_PWME2=1;MCIN1=0;MCIN2=1;void stop(void)D1=1;MCIN1=0;MCIN2=1;PWME_PWME2=0;void fastrun(v

31、oid)D1=0;MCIN1=0;MCIN2=1;void brake(void)D1=1;MCIN1=1;MCIN2=1;void back(uchar v1)PWMDTY2=v1;MCIN1=1;MCIN2=0;void turn_angle(uint a)PWMDTY0= a/256; PWMDTY1= a%256;PWME_PWME1=1;void AD_Init(void) ATD0CTL3=0x28; ATD0CTL4=0x85; ATD0DIEN=0x00; ATD1CTL3=0x28; ATD1CTL4=0x85; ATD1DIEN=0x00; int abs1(int p)

32、if(p>=0)return p; else return(-p);/*黑线中心位置计算*/void road_infor() uchar i,max,max1,max2; uchar max_r=9; for(i=0;i<10;i+) if(AD_wDatai>low_valuei) AD_cDatai=AD_wDatai-low_valuei; else AD_cDatai=0; max=0; for(i=1;i<10;i+) if(AD_cDatamax<AD_cDatai) max=i; /Get the MAX_AD if(AD_cDatamax<

33、30) switch(flag) case 0:position=pos0;break; /125 case 1:position=0;white_flag=1;break; case 2:position=225;white_flag=2;break; default: break; else if(max>0&&max<9) if(AD_cDatamax-1>AD_cDatamax+1) max1=max-1;max2=max; else max1=max;max2=max+1; else if(max=0) max1=0,max2=1; else max

34、1=8;max2=9; position=(max1*AD_cDatamax1+max2*AD_cDatamax2)*25/(AD_cDatamax1+AD_cDatamax2); if(pos0>0&&pos0<225)white_flag=0; if(position<75) flag=1; else if(position>175) flag=2; else flag=0; if(abs1(int)position-pos0)>40) /40 position=pos0; pos0=position; /*转角控制*/void turn1()

35、uchar e1,e_max;uint angle1;uint ang,ang_max;float kk;signed int ang1;e2=pos0-pos7;if(stright_flag)kk=0;else kk=k1; angle1=100; if(pos0>=pos_middle) e1=pos0-pos_middle; ang_max=right_max-middle; e_max=225-pos_middle; if(e1<e_turn)ang=angle1*e1/e_turn; else ang=(ang_max-angle1)*(e1-e_turn)/(e_ma

36、x-e_turn)+angle1; ang1=middle+ang+e2*kk; if(ang1>right_max)ang1=right_max; /turn_angle(ang1); else e1=pos_middle-pos0; ang_max=middle-left_max; e_max=pos_middle; if(e1<e_turn)ang=angle1*e1/e_turn; else ang=(ang_max-angle1)*(e1-e_turn)/(e_max-e_turn)+angle1; ang1=middle-ang+e2*kk; if(ang1<le

37、ft_max)ang1=left_max; turn_angle(ang1);angle=ang1; void stright_test(void) uchar i; uchar middle_flag=1; for(i=0;i<8;i+) if(posi<(pos_middle-45)|posi>(pos_middle+44)middle_flag=0; if(middle_flag&&abs1(pos0-pos7)<40) stright_flag=1; else stright_flag=0; /*if(stright_flag) if(!long

38、sright_flag) stright_con=coun1;longsright_flag=1; turn_flag=0; else longsright_flag=0; if(coun1-stright_con>15)&&speed>130) turn_flag=1; else stright_con=30000;turn_flag=0; if(pos0>80&&pos0<145)stright_flag1=1; else stright_flag1=0;*/ void speed_pid(uint v) int p; if(PID_

39、flag) PID_flag=0; sPID.vi_Ref = v; sPID.vi_FeedBack=speed; p=v_PIDCalc(&sPID )/6; if(p>200)p=200; else if(p<0)p=0; MCIN1=0;MCIN2=1; PWMDTY2=200-p; /*void v_control()int vv;if(turn_flag) PORTB=0x00;back(50);else if(stright_flag1&&!stright_flag) MCIN1=0;MCIN2=1; PWMDTY2=0; PORTB=0xf0

40、;else PORTB=0xff; MCIN1=0;MCIN2=1;/PWME_PWME2=1; if(pos0>75&&pos0<150) vv=180; else if(pos0>0&&pos0<=75) vv=142+pos0/2; else if(pos0>=150&&pos0<225) vv=255-pos0/2; else vv=180; speed_pid(vv-abs1(e2)*4/10); */int abse(int a1,int a2)if(a1>=a2) if(a1>pos_

41、middle)return (a2-a1); else return (a1-a2);else if(a1>=pos_middle)return (a2-a1); else return (a1-a2); /*void v_control1()int vv,vs; if(stright_flag=1) vv=200; vs=vv+spee1+abse(pos0,pos1)*4; speed_pid(vs); else if(speed>170) back(100);/brake(); else if(pos0>90&&pos0<165) vv=170;/

42、160 else if(pos0<=90&&pos0>0) vv=150+pos0/3; /120/140 else if(pos0>165&&pos0<255) vv=180-(pos0-165)/3; /200 else vv=180; /130 /140 vs=vv+spee1+abse(pos0,pos1)*4; speed_pid(vs); */void v_control1()int vv,vs; if(stright_flag=1) vv=200; vs=vv+spee1+abse(pos0,pos1)*4; speed_p

43、id(vs); else if(speed>170+spee1) back(100);/brake(); else if(pos0>75&&pos0<150) vv=180;/160 else if(pos0<=75&&pos0>0) vv=160+pos0/3; /120/140 else if(pos0>=150&&pos0<225) vv=185-(pos0-150)/3; /200 else vv=180; /130 /140 vs=vv+spee1+abse(pos0,pos1)*4; spee

44、d_pid(vs); /*void v_control1()int vv,vs; if(stright_flag=1) vv=200; speed_pid(vv+spee1-abs1(e2)*5/10); else if(speed>170) back(100);/brake(); else if(pos0>90&&pos0<165) vv=170;/160 speed_pid(vv+spee1-abs1(e2)*5/10); else if(pos0<=90&&pos0>0) vv=150+pos0/3; /120/140 spe

45、ed_pid(vv+spee1-abs1(e2)*5/10); else if(pos0>165&&pos0<255) vv=180-(pos0-165)/3; /200 speed_pid(vv+spee1-abs1(e2)*5/10); else vv=180; /130 /140 speed_pid(vv+spee1-abs1(e2)*5/10); */*道路信息分段处理*/void road_deal(uint numb) uchar flag0=0; uchar flag1=0; uint i; uchar m; /for(i=numb-5;i<=n

46、umb;i+) turn_savei=40; road0.road_flag=0; road0.start=0; for(i=0;i<=numb;i+) if(turn_savei<35) flag0=1; else if(turn_savei>45)flag0=2; else flag0=0; if(flag0=flag1) switch(flag0) case 1:roadn.turn_sum+=(40-turn_savei);break; case 2:roadn.turn_sum+=(turn_savei-40);break; default:break; else

47、n+; roadn-1.end=i; roadn.start=i; switch(flag0) case 1: roadn.road_flag=1; roadn.turn_sum=40-turn_savei;break; case 2: roadn.road_flag=2; roadn.turn_sum=turn_savei-40;break; default:roadn.road_flag=0;break; flag1=flag0; roadn.end=numb; if(n>2) for(i=0;i<=n;i+) if(roadi.end-roadi.start<15) i

48、f(roadi-1.road_flag=roadi+1.road_flag) roadi-1.end=roadi+1.end; roadi-1.turn_sum+=roadi+1.turn_sum; n-=2; for(m=i;m<=n;m+) roadm=roadm+2; i-; else roadi+1.start=roadi.start; roadi=roadi+1; n-; for(m=i+1;m<=n;m+) roadm=roadm+1; i-; /*入弯段标记*/void mark(void) uchar i; for(i=0;i<n;i+) if(roadi.r

49、oad_flag=0&&roadi+1.road_flag) stright_turnp=roadi.end;p+; /*速度分配*/void v_deal(void) uchar i; uint m,mid; float Kv; if(project_flag3) Kv=0.8; else Kv=1.5; for(i=0;i<=n;i+) if(!roadi.road_flag) if(i=n)for(m=roadn.start;m<=roadn.end;m+)road_vm=stright_v; else if(roadi.end-roadi.start>stright_limit) mid=(roadi.end-roadi.start)*k2+roadi.start; for(m=roadi.start;m<mid;m+) road_vm=stright_v; roadi.vv=190-(unsigned long)(Kv*roadi+1