智能车模块源程序+很详细

由于今年组委会光电管和摄像头分开比赛。所以传感器部分我们选择了光电管，比赛以小车的速度记成绩，为了让小车更快更稳得跑完全程，传感器的探测距离必须要远，既要有大的前瞻，普通的红外对管由于功率较小，探测距离增大时，干扰严重，所以我们自制了大功率对管，同时采用了程序控制脉冲发光的办法，有效的降低了发热，提高了系统的稳定性。系统采用采用了7.2V2000mAhNi-Cd蓄电池作为系统能源，并且通过稳压电路分出6伏，5伏已分别给舵机和单片机供电。直流电机驱动模块接收速度控制信号控制驱动电机运行，达到控制车速目的。转向伺服模块控制舵机转向，进而控制智能车转弯。速度测量模块实时测量智能车车速，用于系统的车速闭环控制，以精确控制车速。系统充分使用了MC9S12DG128单片机的外围模块，具体使用到的模块包括：ADC模拟数字转换模块、定时器模块、PWM脉冲宽度调制模块、中断模块、I/O端口和实时时钟模块等。系统调试过程中，使用了组委会提供的代码调试环境CodeWarriorIDE，同时使用了清华的Plastid2软件进行了仿真试验。图1.1系统结构框图3.1舵机部分为了使转弯更加灵活，对舵机相关部分作了部分改动。首先，我们将舵机力臂加长85mm。这样，对于同样的转弯角度值，只需更小的舵机转角，减小了舵机转弯时惯性带来的弊端。其次，我们将舵机反装，使舵机连杆水平，因为此时舵机提供的力全部用在转弯上。3.2前轮部分为了增加前轮转弯时的稳定性，对前轮相关部分进行了部分改动。首先，更改前后垫片的数量，使前轮主销后倾，这样，车轮具有更好的自动回正功能。其次，更改连杆的长度,使车轮外倾，车轮转弯时，前半部分重心上移，促使赛车转弯更加稳定。再次，我们通过更改舵机连杆的长度，增加前轮前束，同样增加了前轮的稳定性。3.3底盘部分为了提高赛车运行时的稳定性，对地盘相关部分作了部分改动。首先，前轮相关位置加垫片，降低了前轮重心。其次，更改后轮车轴处的调节块，使后轮重心升高，这样，车身前倾，一定程度上，增加了车的稳定性。3.4后轮部分首先，更换后轮轮距调节块，使后轮两轮之间间距加大。这样，车在转弯时不容易产生侧滑。其次，调节后轮差速，使赛车转弯更加灵活。4.1电源部分为了能使智能车系统能正常工作，就需要对电池电压调节。其中，单片机系统、车速传感器电路需要5V电压，路径识别的光电传感器和接收器电路电压工作为5V、伺服电机工作电压范围4.8V到6V（或直接由电池提供）直流电机可以使用7.2V2000mAhNi-cd蓄电池直接供电。考虑到由于驱动电机引起的电压瞬间下降的现象，因此采用低压降的三端稳压器成为必然。我们在采用lm7805,和lm7806作为稳牙芯片。经试验电压纹波小，完全可以满足要求。电池（7.2v）2000mAhNi-cd

稳压电路5V1*6V* 冃r7.2V单片机对管测速板 舵机电机图4.1系统电压调节图\T：ATui1VCCR1DS1LLC3-T'-CapPollilOOuFINOUTGND0.01111LM2940:T-5.0\T：ATui1VCCR1DS1LLC3-T'-CapPollilOOuFINOUTGND0.01111LM2940:T-5.0O.ObiFLED3图4.27805电路图2G4■^-CapPollCapC-=Lp-7.2Vc#=^C6100<I104MotorPowerSourceLM29401LM29401-+5VBATTERYS-7.2V丄Cl"Tim■?_LC2于斗7证MCUPowerSourceLM7806BATTERYS-7.2V丄Cl"Tim■?_LC2于斗7证MCUPowerSourceLM78061470uFJ.-6V220uFSERVOSen_oPowerSovirce图4.3电源模块示意图

4.2电机驱动电路电机驱动使用飞思卡尔专用电机驱动芯片MC33886。驱动电路如图4.4所示。为了增大驱动能力，减少单片发热量，电路采用两片MC33886并联的方案。系统使用PWM控制电机转速，充分利用单片机的PWM模块资源。电机PWM频率设定为8KHz。MC33886芯片的工作电压为5-40V,导通电阻为140毫欧姆，PWM频率小于lOKHz，具有短路保护、欠压保护、过温保护等功能。电机驱动芯片安装在制作的电机驱动PCB板上，在PCB板设计时，考虑到芯片散热问题，在芯片腹部设计了方型的通孔，实际运行效果表明芯片散热均匀，设计合理。为了防止电动机突然停止时产生的电磁干扰，在电动机的两端焊接了一个0.1“F滤波电容。VCCRes2PWM3PWM5+7.2VSW-SPDTRcs21K2019101118IN1OUT1IN2OUT1D2OUT2D1OUT2DNCDNCAGNDV+PGNDV+PGNDV+PGNDCcp31913PGNDFSQr5MC33886Rcs21K「DS2LED2U5IN1OUT1IN2OUT1VCCRes2PWM3PWM5+7.2VSW-SPDTRcs21K2019101118IN1OUT1IN2OUT1D2OUT2D1OUT2DNCDNCAGNDV+PGNDV+PGNDV+PGNDCcp31913PGNDFSQr5MC33886Rcs21K「DS2LED2U5IN1OUT1IN2OUT1D2OUT2D1OUT2DNCDNCAGNDV+PGNDV+PGNDV+PGNDCcp31913181520PGNDFS丄9丄I127.15C12>141421Header27.Cap47pFRes21.3K8C14MC33886ICapMC3388647pFR7Res21.3K图4.4两片MC33886并联使用

图4.5两片MC33886并联使用的实物图在图中可以看到，我们使用PWM23和PWM45作为电机驱动PWM信号，两个PWM通道级联可以使其输出更加精确。在程序中，我们把PWM值直接转换成了以米/秒为单位的绝对速度，这样使智能车的速度更加直观切易于调试。4・3测速电路由于考虑到成本需要，我们采用了红外对管和黑白码盘作为测速模块的硬件构成。其中码盘为32格的黑白相间圆盘，如下图所示：图4.5码盘红外传感器安装在正对码盘的前方，虽然这样做精度比编码器要低很多，但是成本低廉制作容易，如果智能车速度较快，可以考虑再减少码盘上黑白色条的数量即可。当圆盘随着齿轮转动时，光电管接收到的反射光强弱交替变化，由此可以得到一系列高低电脉冲。设置9S12的ECT模块，同时捕捉光电管输出的电脉冲的

上升沿和下降沿。通过累计一定时间内的脉冲数，或者记录相邻脉冲的间隔时间，可以得到和速度等价的参数值。测速电路使用自行研制的红外反射式光电测速传感器。速度测量电路使用红外反射式光电对管RPR220，自行制作的编码盘，比较电路等组成。速度测量电路图2.8所示。红外反射式光电对管的光敏三极管信号通过比RPR220较器处理后输入单片机的计数器模块，利用单片机的输入捕捉功能，处理智能车速度信息。自制的编码盘有24道黑色条纹，电机旋转一周将产生24次输入捕捉中断。RPR220单片机记录两次中断的时间间隔T。两次中断对应于智能车前进的距离S为：16.5/24cm,即0.6875cm，其中16.5cm为智能车后轮实测周长［7］智能车实时速度V(cm/s)的计算公式如下:0.6875 / 0.6875 / cm/sV= = TT4.4红外对管检测电路由于我们采用了大功率对管，所以红外对管的电路是整个电路中要求最高的，不紧要保证对管正常工作，而且还要考虑整个电路的能耗和发热问题。经测试我们发现单个对管在通以100mA到170mA电流时可以。保证抬高20到30厘米的距离。此时每个对管的管压降为1.2到1.5伏。为了进一步加大发光量，我们采用了双发射管的办法，即一个接受管对应两个发射管。为了降低整体的能耗。我们让同一对的发射管串联，14对发射管再并联。同时使用了irf540进行开关控制。控制对管脉冲发光。开关频率为200HZ。这样既保证了大前瞻探测的需要，又降低了整体的能耗和对电源的冲击

图4.7先串联再并联的脉冲发光对管电路图。图4.8对管实物图反面图4.9对管实物图正面4.5拨码开关电路由于在智能车比赛开始后，不能够对智能车硬件及软件进行修改，在保证了硬件有效可靠的同时，软件有可能不能够适应新场地，所以设计拨码开关对智能车有关参数进行设置也是必要的。拨码开关电路如下图所示：R1 Header9VCCSW73456789,1S116SW6215彳SW513141SW4413・SW3J512*SW211片SW1710-SW0 」89[SW-DIP8图4.9拨码开关这是一个八段的拨码开关，我们把它成成上下连个部分，显然，每个部分都有16种状态,前四个来改变舵机参数，后四个改变直流电机参数，这样对于适应新的场地很有好处。5・1路径搜索算法对于本控制系统采用14对光电对管的方案，单排排列在车体头部10cm处。编号为6、7的光电对管处于正中央位置。利用14对传感器进行道路识别。传感器对白色的反射率比黑色的大。单片机ADC读入值相应也大。在程序中对传感器信号进行处理，判断传感器是否检测到黑色引导线。将单个传感器对白色和黑色路面的ADC值之差分为平均的两段，每次处理实时传感器信号时，判断本次采样的ADC值与黑色路面ADC值之差落在两段中的哪一段。如果在靠近黑色的一段，则判定该传感器检测到黑线，将该传感器对应的变量置为判定值1；如果在靠近白色的一段，则判定该传感器检测到白线,将该传感器对应的变量置为判定值1；为了增强判断的准确性，在对ADC值采样时，采用了中值滤波方法，以去除瞬间的干扰。路径检测完后，将测的的路径值暂时存储，然后将路径信息传递给舵机和电机控制部分，以选择给定合适的转角和速度。

5.2舵机、电机的控制智能车的舵机和电机都采用了经典的PID控制方法。但是由于舵机和电机性能的不同要求，分别对其进行了不同的修改。PID控制器由比例单元(P)、积分单元(I)和微分单元(D)组成。其输入e(t)与输出u(t)的关系为O二乞殆)+O二乞殆)+式中积分的上下限分别是0和t因此它的传递函数为：G(s)=U(s)/E(s)=kp(1+1/(TI*s)+TD*s)其中kp为比例系数；TI为积分时间常数；TD为微分时间常数比例KP用来控制当前，误差值和一个负常数P(表示比例)相乘，然后和预定的值相加。P只是在控制器的输出和系统的误差成比例的时候成立，KP能够快速的跟随变化量。及时的产生与之相关的调节作用。但是KP是有差调节，无法消除静态误差。积分KI来控制过去，误差值是过去一段时间的误差和，然后乘以一个负常数I，然后和预定值相加。I从过去的平均误差值来找到系统的输出结果和预定值的平均误差。一个简单的比例系统会振荡，会在预定值的附近来回变化，因为系统无法消除多余的纠正。通过加上一个负的平均误差比例值，平均的系统误差值就会总是减少。所以，最终这个PID回路系统会在预定值定下来。微分KD来控制将来，计算误差的一阶导，并和一个负常数D相乘，最后和预定值相加。这个导数的控制会对系统的改变作出反应。导数的结果越大，那么控制系统就对输出结果作出更快速的反应。这个D参数也是PID被成为可预测的控制器的原因。D参数对减少控制器短期的改变很有帮助。一些实际中的速度缓慢的系统可以不需要D参数。舵机PID由于舵机是一个具有大的延迟的执行机构，所以在PID控制中不能加入积分环节。否则会导致小车震荡。所以小车采用PD控制。同时加入一个一阶惯性环节，构成不完全微分，给小车一个超前的调节。实际使用中，为了减少计算时间，将位置式PID转化为增量式增量式PID公式：Aw(Ar)=u(k)-u(k-1)二Kp\e(k)-e(k-1)]+K用)+KD\e(k)—2e(k-1)+e(k-2)]电机PID控制小车行使过程中，随着跑道的不同，需要配合不同的速度值，因此对电机的PID是一个给定值不断变化的PID。小车的目标速度(Object_Speed)给定规则：1》 小车在直道上，Object_Speed为最大值200。2》 小车在大弯道上，Object_Speed为160。3》 小车在小弯道或S型弯道上，Object_Speed为1204》 小车冲出跑道，Object_Speed为70。5》 小车由直道进入弯道，Object_Speed逐渐减小。6》小车由弯道进入直道，Object_Speed逐渐加大。在实验中发现，PID的超调量主要在第一个波形中起作用，也即单速度由很大到很小的时候，或由低速突然加到高速的过程中，会出现很大的超调。但是这个超调并不是有害的，因为，当速度要求突变的时候，往往是小车由直道入弯道，或者由弯道入直道的过程，这个过程往往需要很快的大加减速,而由于小车的惯性,一般的PID调节难以满足要求，这时使用大的超调量可以使小车有一个加速或刹车的过程，使之更好的达到要求速度。6.1Codewarrior开发环境在整个开发调试过程中，使用Metrowerks公司为MC9S12系列专门提供的全套开发工具(FreescaleCodewarriorIDE4.6)。这是一套用C语言进行编程的集成开发环境一一本文智能车定位系统的软件设计部分就是在此开发环境下完成的。Codewarrior是由Metrowerks公司提供的专门面向Freescale所有MCU与DSP嵌入式应用开发的软件工具。其中包括集成开发环境IDE、处理器专家、全芯片仿真、可视化参数显示工具、项目工程管理、C交叉编译器、汇编器、链接器以及调试器。CodeWarriorIDE能够自动地检查代码中的明显错误，它通过一个集成的调试器和编辑器来扫描你的代码，以找到并减少明显的错误，然后编译并链接程序以便计算机能够理解并执行你的程序。每个应用程序都经过了使用象CodeWorrior这样的开发工具进行编码、编译、编辑、链接和调试的过程。 MetrowerksCodewarriorIDE中的mc9s12dg128.h文件对所有寄存器对应的存储映射地址都进行了宏定义，开发者在软件开发时直接调用这些宏就可以了。6.2软件仿真为了更好的定量分析影响小车行驶的各个因素，而且最大限度的节约时间和成本。我们采用了软件仿真和实际调试相结合的办法。仿真软件使用了清华的PLAST2。通过仿真，我们发现：1、 小车传感器的探测距离对速度有着决定的影响。所以传感器应该尽量的探测更远。但是传感器的探测距离不能超过最小转弯的半径。否则会出现盲区。2、 适度增加舵机的灵敏度，可以使转弯更加灵活。所以我们在实际调试中，加长了舵机的力臂。6.2实际调试实际调试过程中，我们发现小车在直道上会出现左右抖动的现象，通过软件设置死区或其他处理方法，效果都不是很明显，最后发现小车舵机和前轮的间隙是罪魁祸首，通过把前轮设置为内八形，完美的解决了这个问题。在调试过程中，我们加了液晶显示，还设置了蜂鸣器，这些辅助设备在比赛中为了减轻小车的重量，都是不需要的。但是在调试过程中，通过这些设备，可以及时的了解小车运行的情况，达到事倍功半的效果。表7.1模型车技术参数统计:项目参数路径检测方法（赛题组）光电组车模几何尺寸（长、宽、高）（毫米）385*220*60车模轴距/轮距（毫米）200/150车模平均电流（匀速行驶）（毫安）200电路电容总量（微法）430传感器种类及个数红外对管15个新增加伺服电机个数0赛道信息检测空间精度（毫米）9赛道信息检测频率（次/秒）200主要集成电路种类/数量9s12单片机最小系统/133886电机驱动电路/4速度检测电路/I车模重量（带有电池）（千克）0.9参考文献黄开胜、金华民、蒋狄南，韩国智能模型车技术方案分析，北京：清华大学汽车安全与节能国家重点实验室，2004.3邵贝贝著，单片机嵌入式应用的在线开发方法，北京：清华大学出版社，2004.2'LM2940datasheet',July2000,NationalSemiconductor'LM7806datasheet'NationalSemiconductor'RPR220datasheet',ROHM'SemiconductorTechnicalDataMC33887'Aug2002MotorolaInc大赛车模拼装手册CodeWarriorIDE3.1helpdatasheet'MC9S12DG128DeviceUserGuide',October2002MotorolaInc'HCS12COREdatasheet',Augt2000MotorolaInc'S12PWM8B8CV1datasheet'.Mar2002MotorolaInc'S12ATD10B8CV2datasheet',Augt2002MotorolaInc'S12ECT16B8V1datasheet',July2002MotorolaIncwww.smartcar.au.t程序电机PID控制程序typedefunsignedcharBOOL;typedefunsignedcharINT8U;〃无符号8位数typedefsignedcharINT8S;//有符号8位数typedefunsignedintINT16U;〃无符号16位数typedefsignedintINT16S;〃有符号16位数typedefunsignedlongINT32U;〃无符号32位数typedefsignedlongINT32S;//有符号32位数typedeffloatFP32;//单精度浮点数typedefdoubleFP64;//双精度浮点数

#defineMAX_32(signed#defineMIN_32(signedlong)0x7fffffffLlong)0x80000000L#defineMAX_16(signed#defineMIN_16(signedint)0x7fffint)0x8000typedefstruct{intsigneintsignesignedintProportionalGain;signedintProportionalGainScale;signedintIntegralGain;signedintIntegralGainScale;signedintDerivativeGain;signedintDerivativeGainScale;signedintPositivePIDLimit;signedintNegativePIDLimit;signedintIntegralPortionK_1;signedintInputErrorK_1;}sCaiXinBoPID;sCaiXinBoPIDSpdPID;L_sub(registersignedlongsrc_dst,registerL_sub(registersigned{return(src2-src_dst);}staticsignedlongL_deposit_l(registersignedintssrc){return(signedlong)(ssrc);}staticsignedintextract_l(registersignedlonglsrc){return(signedint)lsrc;intsinpl,register}intsinpl,registerstaticsigned.longLmult(registersignedsigned&intsinp2)6 - 6 &{registersignedlonglaccum;laccum=sinp1;laccum*=sinp2;returnlaccum;}staticsignedlongL_add(registersignedlongsrc_dst,registersignedlongsrc2){return(src_dst+src2);}

signedsignedintCaiXinBoPJDContrpllgr(^igflgdo厂intDesiredValue,

mtMeasuredValue,sCaiXmBoPID*pParams)signedsignedsignedlongProportionalPortion,IntegralPortion,PIDoutput;signedintInputError;/*Saturationmodemustbeset*/errorInputError=sub(DesiredValue,MeasuredValue);*//*input/*Saturationmodemustbeset*/errorInputError=sub(DesiredValue,MeasuredValue);*//*input/* *//*inputerror^calculation-16bitrange,withandwithoutsaturationmode*/L_PuD(巴弧sitl(DesiredValue),L_deposit_l(MeasuredValue));/*inputerror-32biFrange*/grea(PIDa»)(>xMf_!2767 -32bitrange/ 吨亟.InputError=MAX_16* /*inputerror=max.positive16bitsignedvalue*/elsethanfPBfu还7MJN_l6)32bitrange*/ /*⑴何叫is应.InputError=M【N_16； /*inputerror=min.negative16bitsignedvalue*/else^InputErrorrange*/=extract_l(PIDoutput);/*inputerror-16bit^InputErrorrange*/*//*proportionalportioncalculation*/TProportionaiportion=L_mult((pParams.-> 1ProportionalGain),InputError)>>(pParams->ProportionalGamScale+1); *//*integralportioncalculation*//IntegralPortion=L_muJt((pParams->IntegralGain),InputError)>>(pParams->IntegralGainScaie+1);/*integralportioninstepk+integralportioninstepk-1*/TIntegralPortjpn=L_add(IntegralPortiftn,八、L_deposit_l(pParams->IntegralPortionK_1));/*integralportionlimitation*/if(IntegralPortion>(pParams->PositivePIDLimit))(pParams->IntegralPortionK_l)=(pParams->PositivePIDLimit);elseif(IntegralPortionvpParams->NegativePIDLimit)pParams->IntegralPortionK_l=pParams->NegativePIDLimit;elsepParams->IntegralPortionK_l=extract_l(IntegralPortion);匕二二二二二两/*derivativeportioncalculation*/PpD禺槪MWoe^inpuErt磁册0gppm〉1

pParams->InputErrorK_l=InputError;/*e(k-l)=e(k)*/gflDhanOMAXf^^? -32bitrang/*/[e(k)-e(k-1)]isInputError=MAX_16:max.positive16bitsignedvalue-InputError=MAX_16:max.positive16bitsignedvalue-16bitrange*[e(k)e(k-1)]=elsethanfffPU-<MIN_16)-32bitrange/以k)-*1)]is贬.InputError=MIN_16;—i. _/*[e(k)-e(k-1)]=min.negative16bitsignedvalue-16bitrange*/elsere.InputError=extract_l(PIDoutput);/*[e(k)-e(k-1)]16bitrange*//*drivativeportioninstepk-integer*/PIDoutput=L_mult(.(pParams->DerivativeGain),InputError)>>(pParams->DerivatTveGainScale+1);/*controlleroutputcalculation*//*controlleroutputcalculation*/ProportionalPortion);/*derivativeLoSlSp^F^^^EionK-1))；/*+integralportion/*controlleroutputlimitation*/if(PIDoutput>pParams->PositivePIDLimit)PIDoutput=pParams->PositivePIDLimit;elseif(PIDoutputvpParams->NegativePIDLimit)

PIDoutput=pParams->NegativePIDLimit;、初始化程序voidRTI_Init(void)/*setupoftheRTIinterruptfrequency*/.」*adjustedtoget1millisecond(1.024ms)with16MHzoscillator*/setRTIableRTIinterrupts;〃低四setRTIableRTIinterrupts;}voidSET_PLL(void){CLKSEL=0x00;PLLCTL=0xel;SYNR=2;REFDV=1;PLLCTL=0x60;asmNOP;asmNOP;asmNOP;while((CRGFLG&0x08)==0);CLKSEL=0x80;} /*锁相环程序*/

voidATD_Init(void){...AATDC队列转换完成不开断使能；〃〃上电’标志位快速清零度8ATD0CTL存储模0式，，equencelength:80; //转换队列长bus_cATD0C+£=°x85;//8位精度’AD时钟为ch1bATS?CTJ織多通道对齐无通道数'开次转换队列模式'ATDlCTL2=0xc0;//ADPU,fastflagclearATDlCTL3=0x30;//sequencelength:6,notfifomode,clock:ATD1CT2*4=+X8产〃8 bitdecision’ATDch1bAS?ChO織多通道对齐无通道数'开次转换队列模式'程序*/ /*atd初始化voidServo_Init(void) 〃舵机16位PWM控制{PWME_PWME1=0;//disablePWM1servoPWMPRCLKI=0x03;//CLKA:8分频A时钟源PWMCLK|=0x00; 〃这就是默认值〃通道1用clock〃24MHZ/8=3MPWMPOL_PPOL1=1;〃先高电平PWMCAE=0x00;〃对齐方式默认左对齐

PWMCAE=0x00;〃对齐方式默认左对齐PWMCTL_CON01=1;II通道1:16BIT连接周期是KEMPER!1*x=ii38F；〃3°°00:10ms；；；3°°00〃舵机的II1ms脉冲到2ms脉冲II1.5ms脉冲对应8度限5588右PWMDTY881正中8央;II4588:1-5ms；〃控制3588左极PWMDTyXWMWRX进入入锁存器,写计数寄存瓠止会使I*舵机初始PWME_PWME1=1;IIPWM通道1I*舵机初始化程序*/voidForward_Init(void)//假定前进方向，初始化函数{IIPWM3:IN1IIPWM5:IN2=1IIPWM7:ENIIDDRP=8xff; 〃控制输出PWME_PWME3=8;PWME_PWME5=8;PWMPRCLKI=8x28;//clockb总线频率的4分频〃通道3用clockB时钟源II24MHZ/4=6M] ]PWMPOJ^_PPpL5l=8;I/hwele3886l丿eve]£尬匚上electricallevelfirst,buthighlevelmakesmc33886disabledPWMCAE//PWMCAE//对齐方式默认左对齐PWMCTL_CON45=1;//16BIT连接i/i.5m*x=PWKHER45i/i.5m*x=PWKHER45=750;〃电机频率8kHzPWMDTY45=500;//占空比精度1/750PWMCNT45=0; 〃启动PWMPWME_PWME5=1;//PWM通道3输出}/*电机初始化程序*/voidECT_Init(void) //ECT初始化，使用输入捕捉功能{TSCR1_TFFCA=1;//automaticallyclearflag. TI0S_I0S0=0;asaninputcapture. TI0S_I0S0=0;asaninputcapture//ONCH0PT0;onchannelTConchannelTCTL4_EDG0A=1;//captureonrisingedgeonlyTCTL4_EDG0B=0;TIE_C0I=1; //EnableCh1InterruptsTSCR1_TEN=1;//ENABLETIMER}主程序#includevhidef.h> /*commondefinesandmacros*/#includeVmc9sl2dgl28.h>/*derivativeinformation*/#pragmaLINK_INFODERIVATIVE"mc9sl2dgl28b"#include"pid.h"#include"init.h"#defineAD_LEVEL130#defineMID4785#defineMAXTURN750#defineDC_Limit600#defineKP1(-40)#defineKI10#defineKD1(0)#defineSPEED70〃变量定义inttimes,exit,exit1,exit_flag;〃停车用变量charzanshi=0;〃调速intSensor=4785;〃舵机intave_weight[3]={0,0,0};intcount0=0;intAD_Value[14];inti;intatd_value[14];charboma=0; 〃拨码开关intzhidao_speed=0;intwangdao_speed=0;intchongchu_speed=0;〃速度设定intcha0,cha1;〃不完全微分

//2.083msoneintvolatileunsignedintIC0Counter=0;volatileunsignedintIC0Counter_l=0;//2.083msoneintvolatileunsignedintstop_flag=0;volatileintNowSpeed;volatileunsignedintGearWidth[4]={0};volatileunsignedintGearIndex=O;unsignedlongGearWidthSum;unsignedlongAveGearWidth;unsignedlongAveSpeed;//测速unsignedintpulse_counter=0;〃计数intDCDuty;intObject_Speed=SPEED;//100cm/sintstop_stop=0xff,stop_stop1=0;intpid1(inta,intb,intc)intpid1(inta,intb,intc)intx=0;x=(KP1*(a-b)+KI1*a+KD1*(a-2*b+c));return(x);}}voidmain(void){1.841.84(oSpdPIDjPropor1ipnalGainScale=13; 〃0.76 的系数〃(0-2):gainscale=130-1280-256.. 〃(°-4)0-80-160-320-64SpdPID.IntegralGain=25000;//20480;15000i.45?dPDd>92!Gaj55c0ii=140>〃0.625的系数 1.25SpdPID.DerivativeGain=20000;〃0SpdPID.DerivativeGainScale=14;SpdPID.PositivePIDLimit=DC_Limit;SpdPID.NegativePIDLimit=0;SpdPID.IntegralPortionK_1=0;//=0SpdPID.InputErrorK_1=0;//=0RTI_Init();SET_PLL();ATD_Init();Servo_Init();Forward_Init();ECT_Init();DDRT=0x00;boma=PTIT;〃读入拨码开关；boma>>=4;boma&=0b00001111;zhidao_speed=90+7*boma;wangdao_speed=75+4*boma;chongchu_speed=50;//60+boma;Enablelnterrupts;for(;;){/////////////////////调速/////////////////////if(abs(ave_weight[2])<3){Object_Speed=zhidao_speed;}elseif(abs(ave_weight⑵)vl4)Object_Speed=wangdao_speed;elseObject_Speed=chongchu_speed;/////////////////////转弯/////////////////////chal=pidl(ave_weight[2],ave_weight[l],ave_weight[0]);chal=(chal*100+cha0*0)/100;chaO=cha1;Sensor=4.785-38*],ave_weight[0]);ave_weight[2];//pid1(ave_weight[2],ave_weight[1Sensor=4.785-38*],ave_weight[0]);if(Sensor>MID+MAXTURN)Sensor=MID+MAXTURN;

}if(SensorvMID-MAXTURN){Sensor=MID-MAXTURN;}PWMDTY01=Sensor;ave_weight[O]=ave_weight[l];ave_weight[1]=ave_weight[2];count0=0;}}1ms中断1ms中断/*x* ~n—1-*/不不不不不不不不不不不不不不不不不不不不不不 二 忖、*x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x**x*I， ptvoidinterrupt7RTI_int(){inti,j,k,l,m;staticintpulse_t;CRGFLGI=0x80;pulse_t++;if(pulse_t>5){pulse_t=0;DDRA=0xff;PORTA_BITO=1;for(i=0;ivl500;i++);ATD0CTL5=0x90;〃启动一个新的转换，chObeginATD1CTL5=0x90;while(!(ATDOSTATO&0x80));〃等待队列转换结束应结果寄存器值清相应转换完成通道标志AJiDDRAM读取相AD_Value[1]=ATD0DR1L;AD_Value[2]=ATD0DR2L;AD_Value[3]=ATD0DR3L;AD_Value[4]=ATD0DR4L;AD_Value[5]=ATD0DR5L;AD_Value[6]=ATD0DR6L;AD_Value[7]=ATD0DR7L;//beginATD1,ch0beginwhile(!(ATD1STAT0&0x80));/等待ATD1队列转换结束AD_Value[8]=ATD1DR0L;AD_Value[9]=ATD1DR1L;AD_Value[10]=ATD1DR2L;AD_Value[11]=ATD1DR3L;AD_Value[12]=ATD1DR4L;AD_Value[13]=ATD1DR5L;//ATD1DR5L;PORTA_BIT0=0;for(