計(jì)算機(jī)控制技術(shù)PID課程設(shè)計(jì)報(bào)告

/課程設(shè)計(jì)課程名稱計(jì)算機(jī)控制技術(shù)題目名稱高精度直流電機(jī)調(diào)速系統(tǒng)學(xué)生學(xué)院信息工程學(xué)院專業(yè)班級應(yīng)用電子技術(shù)2班學(xué)號XXXXXXXXXXX學(xué)生姓名胡叢滟指導(dǎo)教師黃國宏20XX06月16日目錄一、方案論證3PID算法3簡易工程發(fā)整定PID參數(shù)3二、理論分析與計(jì)算32.1.系統(tǒng)設(shè)計(jì)方案32.2.數(shù)字PID控制器42.3.湊式法整定PID參數(shù)52.4.直流電機(jī)調(diào)速與測速6三、硬件電路設(shè)計(jì)63.1.電機(jī)驅(qū)動(dòng)及傳感器電路73.2.串口發(fā)送數(shù)據(jù)電路73.3.LCD1602顯示電路7四、程序設(shè)計(jì)84.1.PID增量式算法84.2.系統(tǒng)程序流程圖84.3.消除積分不靈敏的辦法94.4.抗積分飽和的辦法9五、調(diào)試過程105.1.只有比例環(huán)節(jié)105.2.加入積分環(huán)節(jié)115.3.加入積分分離115.4.加入微分環(huán)節(jié)13六、設(shè)計(jì)心得15七、參考文獻(xiàn)15八、附錄〔程序>16摘要本作品以單片機(jī)STC98C52為控制器.由電機(jī)、電機(jī)速度采集傳感器和電機(jī)驅(qū)動(dòng)組成主電路?？刂破鳟a(chǎn)生PWM脈沖送到電機(jī)驅(qū)動(dòng)電路中.經(jīng)過功率放大后控制直流電機(jī)轉(zhuǎn)速.同時(shí)利用速度檢測模塊將當(dāng)前轉(zhuǎn)速反饋到控制器中.控制器經(jīng)過數(shù)字PID運(yùn)算后改變PWM脈沖的占空比.實(shí)現(xiàn)電機(jī)精確控制轉(zhuǎn)速和改善系統(tǒng)運(yùn)行目的。實(shí)驗(yàn)數(shù)據(jù)通過LCD1602顯示并通過串口發(fā)送給PC機(jī)進(jìn)行波形顯示。ABSTRACTThisworkSTC98C52MCUasthecontroller,drivenbyamotor,motorspeedacquisitionsensorandthecompositionofthemaincircuit.ControllertogeneratePWMpulsemotordrivecircuit,afterpoweramplificationcontroldcmotorspeed,andspeeddetectionmoduleisusedtochangethecurrentspeedfeedbacktothecontroller,thecontrollerthroughadigitalPIDcomputationafterchangingthedutyratioofPWMpulse,itcanrealizeaccuratecontrolofmotorspeedandimprovethesystemoperationpurpose.TheexperimentaldatathroughtheLCD1602displayandwaveformdisplaythroughaserialportissenttothePC.方案論證1.1.PID算法方案一：采用位置式PID算法.該控制算法提供了執(zhí)行機(jī)構(gòu)的具體位置.PID輸出與整個(gè)過去的狀態(tài)有關(guān).容易產(chǎn)生大的累加誤差。方案二：采用增量式PID算法.增量算法不需要做累加.控制量增量的確定僅與最近幾次誤差采樣有關(guān).對控制量的計(jì)算影響較小.易于實(shí)現(xiàn)手動(dòng)到自動(dòng)的無沖擊切換。方案采用：在本實(shí)驗(yàn)中采用增量式PID算法。主要原因是PWM占空比和轉(zhuǎn)速不成線性關(guān)系.意思就是就算知道了現(xiàn)在電機(jī)速度.也無法準(zhǔn)確改變占空比來達(dá)到期望速度.采用增量式的好處是只要有誤差存在就可以不斷增加或者減少占空比來達(dá)到期望速度。1.2.簡易工程法整定PID參數(shù)方案一：采用擴(kuò)充臨界比例度法.只需選取一個(gè)足夠短的采樣周期.只采用比例作用.不斷改小比例度〔δ=1/Kp.直到系統(tǒng)發(fā)生持續(xù)等幅振蕩.記下使系統(tǒng)發(fā)生振蕩的臨界比例度δk及系統(tǒng)的臨界振蕩周期Tk.然后再通過選擇控制度和根據(jù)表格查找參數(shù)就可以求得T、Kp、Ti、Td的值。方案二：采用湊試法.根據(jù)參數(shù)對控制過程的影響趨勢.對參數(shù)實(shí)行先比例.后積分.再微分的整定步驟。方案采用：本實(shí)驗(yàn)采用湊試法。主要原因是擴(kuò)充臨界比例度法需要選擇足夠小的采樣周期.但本系統(tǒng)的采樣周期為1s.遠(yuǎn)大于純滯后時(shí)間.因此不能采用擴(kuò)充臨界比例度法。理論分析與計(jì)算2.1.系統(tǒng)設(shè)計(jì)方案圖2.1系統(tǒng)方案框圖根據(jù)系統(tǒng)設(shè)計(jì)的任務(wù)和要求.設(shè)計(jì)系統(tǒng)方框圖如圖2.1所示。圖中控制器模塊為系統(tǒng)的核心部件.按鍵和顯示器用來實(shí)現(xiàn)人機(jī)交互功能.其中通過按鍵將需要設(shè)置的參數(shù)和狀態(tài)輸入到單片機(jī)中.并且通過控制器顯示到顯示器上。在運(yùn)行過程中控制器產(chǎn)生PWM脈沖送到電機(jī)驅(qū)動(dòng)電路中.經(jīng)過放大后控制直流電機(jī)轉(zhuǎn)速.同時(shí)利用速度檢測模塊將當(dāng)前轉(zhuǎn)速反饋到控制器中.控制器經(jīng)過數(shù)字PID運(yùn)算后改變PWM脈沖的占空比.實(shí)現(xiàn)電機(jī)轉(zhuǎn)速實(shí)時(shí)控制的目的。2.2.數(shù)字PID控制器模擬PID調(diào)節(jié)器的控制規(guī)律為〔2.1式中.為比例系數(shù).為積分時(shí)間常數(shù).為微分時(shí)間常數(shù)。當(dāng)采樣周期足夠小時(shí).在模擬調(diào)節(jié)器的基礎(chǔ)上.通過數(shù)值逼近的方法.用求和代替積分、用后向差分代替微分.使模擬PID離散化變?yōu)椴罘址匠?。這樣.式〔2.1便可離散化以下差分方程：〔2.2式中是偏差為零時(shí)的初值；式<2.2>第一項(xiàng)起比例控制作用,稱為比例〔P項(xiàng),即〔2.3第二項(xiàng)起積分控制作用,稱為積分〔I項(xiàng)即〔2.4第三項(xiàng)起微分控制作用,稱為微分〔D項(xiàng)即〔2.5式〔2.2的輸出量u<n>為全量輸出,它對于被控對象的執(zhí)行機(jī)構(gòu)每次采樣時(shí)刻應(yīng)達(dá)到的位置。因此,式〔2.2又稱為位置型PID算式。由〔2.2可看出.位置型控制算式不夠方便.這是因?yàn)橐奂悠頴<i>,不僅要占用較多的存儲(chǔ)單元.而且不便于編寫程序.為此對式〔2.2進(jìn)行改進(jìn)。根據(jù)式〔2.2不難看出u<n-1>的表達(dá)式.即〔2.6將式〔2.2和式〔2.6相減.即得數(shù)字PID增量型控制算式為〔2.7式中：稱為比例增益；稱為積分系數(shù)；稱為微分系數(shù)。2.3.湊試法整定PID參數(shù)增大比例系數(shù)Kp.一般將加快系統(tǒng)的響應(yīng).在有靜差的情況下有利于減小靜差。但過大的比例系數(shù)會(huì)使系統(tǒng)有較大的超調(diào).并產(chǎn)生振蕩.使穩(wěn)定性變壞。增大積分時(shí)間Ti有利于減小超調(diào).減小振蕩；使系統(tǒng)更加穩(wěn)定.但系統(tǒng)靜差的消除將隨之減慢。增大微分時(shí)間Td亦有利于加快系統(tǒng)響應(yīng).使超調(diào)量減小.穩(wěn)定性增加.但系統(tǒng)對擾動(dòng)的抑制能力減弱.對擾動(dòng)有較敏感的響應(yīng)。在湊試時(shí).可參考以上參數(shù)對系統(tǒng)控制過程的影響趨勢.對參數(shù)調(diào)整實(shí)行先比例、后積分.再微分的整定步驟。〔1首先整定比例部分.將比例系數(shù)由小變大.并觀察相應(yīng)的系統(tǒng)響應(yīng).直至得到反應(yīng)快、超調(diào)小的響應(yīng)曲線。如果系統(tǒng)沒有靜差或靜差已經(jīng)小到允許范圍內(nèi).并且對響應(yīng)曲線已經(jīng)滿意.則只需要比例調(diào)節(jié)器即可?！?如果在比例調(diào)節(jié)的基礎(chǔ)上系統(tǒng)的靜差不能滿足設(shè)計(jì)要求.則必須加入積分環(huán)節(jié)。將已經(jīng)調(diào)節(jié)好的比例系數(shù)略為縮小<一般縮小為原值的0.8>.然后減小積分時(shí)間.使得系統(tǒng)在保持良好動(dòng)態(tài)性能的情況下.靜差得到消除。在此過程中.可根據(jù)響應(yīng)曲線的好壞反復(fù)改變比例系數(shù)與積分時(shí)間.以期待得到滿意的控制過程與整定參數(shù)。〔3如果在上述調(diào)整過程中對系統(tǒng)的動(dòng)態(tài)過程反復(fù)調(diào)整還不能得到滿意的結(jié)果.則可以加入微分環(huán)節(jié)。一般的系統(tǒng)D=0.1或2。只有部分滯后較大的系統(tǒng).D值才可能調(diào)大些。PID參數(shù)修改后.可以適度修改設(shè)定值<人為的階躍擾動(dòng)>觀察系統(tǒng)的跟蹤響應(yīng).以判斷PID參數(shù)是否合適。P值太小.I值太小或D值太大均會(huì)引起系統(tǒng)超調(diào)振蕩。2.4.直流電機(jī)調(diào)速與測速調(diào)速：根據(jù)PWM控制的基本原理可知.一段時(shí)間內(nèi)加在慣性負(fù)載兩端的PWM脈沖與相等時(shí)間內(nèi)沖量相等的直流電加在負(fù)載上的電壓等效.可通過改變輸出的占空比來改變電壓從而改變速度。測速：在本系統(tǒng)中由于要將電機(jī)本次采樣的速度與上次采樣的速度進(jìn)行比較.通過偏差進(jìn)行PID運(yùn)算.因此速度采集電路是整個(gè)系統(tǒng)不可缺少的部分。本次設(shè)計(jì)中應(yīng)用了比較常見的光電測速方法來實(shí)現(xiàn).其具體做法是將電機(jī)軸上固定一圓盤.且其邊緣上有N個(gè)等分凹槽如圖3.5〔a所示.在圓盤的一側(cè)固定一個(gè)發(fā)光二極管.其位置對準(zhǔn)凹槽處.在另一側(cè)和發(fā)光二極光平行的位置上固定一光敏三極管.如果電動(dòng)機(jī)轉(zhuǎn)到凹槽處時(shí).發(fā)光二極管通過縫隙將光照射到光敏三極管上.三極管導(dǎo)通.反之三極管截止.電路如圖3.4〔b所示.從圖中可以得出電機(jī)每轉(zhuǎn)一圈在P3.3的輸出端就會(huì)產(chǎn)生N個(gè)低電平。這樣就可根據(jù)低電平的數(shù)量來計(jì)算電機(jī)此時(shí)轉(zhuǎn)速了。例如當(dāng)電機(jī)以一定的轉(zhuǎn)速運(yùn)行時(shí).P1.2將輸出如圖3.5所示的脈沖.若知道一段時(shí)間t內(nèi)傳感器輸出的低脈沖數(shù)為n.則電機(jī)轉(zhuǎn)速v=n/t。<a><b>圖2.2電機(jī)速度采集方案硬件電路設(shè)計(jì)3.1.電機(jī)驅(qū)動(dòng)及傳感器電路圖3.1電機(jī)驅(qū)動(dòng)及傳感器電路3.2.串口發(fā)送數(shù)據(jù)電路圖3.2串口電路3.3.LCD1602顯示屏電路圖3.3LCD1602顯示屏電路程序設(shè)計(jì)4.1.PID增量式算法本系統(tǒng)設(shè)計(jì)的核心算法為PID算法.它根據(jù)本次采樣的數(shù)據(jù)與設(shè)定值進(jìn)行比較得出偏差e<n>.對偏差進(jìn)行P、I、D運(yùn)算最終利用運(yùn)算結(jié)果控制PWM脈沖的占空比來實(shí)現(xiàn)對加在電機(jī)兩端電壓的調(diào)節(jié)[10].進(jìn)而控制電機(jī)轉(zhuǎn)速。其運(yùn)算公式為：4.2.系統(tǒng)程序流程圖初始化包括對時(shí)鐘.中斷的初始化.檢測完數(shù)據(jù)再進(jìn)行PID調(diào)節(jié).輸出數(shù)據(jù).顯示并發(fā)送。開始計(jì)算e<n>初始化計(jì)算Ki*e<n>計(jì)算Kp*[e<n>檢測-e<n-1>]PID計(jì)算Kd[e<n>調(diào)節(jié)-2e<n-1>+e<n-2>]輸出顯示計(jì)算出△U串口發(fā)送數(shù)據(jù)得出占空比speed=speed+△U圖4.1系統(tǒng)程序流程圖en2=en1en1=en返回圖4.2PID增量式算法圖4.3.消除積分不靈敏的辦法在本實(shí)驗(yàn).因?yàn)檗D(zhuǎn)速和占空比是要成正比例關(guān)系的.當(dāng)比如占空比的最小單位是1/100的時(shí)候.可能一個(gè)最小單位對應(yīng)的轉(zhuǎn)速是〔3~4rps.那這樣的話就消除不了靜差.如果把最小單位改成1/400的時(shí)候.可能一個(gè)最小單位對應(yīng)不了1rps.但是占空比會(huì)累加.從而使轉(zhuǎn)速改變1rps。附上某個(gè)區(qū)域的對應(yīng)關(guān)系如圖4.3所示：012345678910000000111121112131415161718192021223344555711222324252627282930313216222733394449556064673334353637383940414243717578808285878990919344454647484950515253549495979899991001011021021035556104104圖4.3紅色代表占空比.比如1代表1/400占空比.白色代表轉(zhuǎn)速.黃色代表線性關(guān)系的轉(zhuǎn)速。4.4.抗積分飽和的辦法如果執(zhí)行機(jī)構(gòu)機(jī)構(gòu)已到達(dá)極限位置.仍然不能消除偏差時(shí).由于積分作用.盡管計(jì)算PID差分方程式所得的運(yùn)算結(jié)果繼續(xù)增大或者減小.但執(zhí)行機(jī)構(gòu)已無相應(yīng)動(dòng)作.這就是積分飽和。當(dāng)出現(xiàn)積分飽和時(shí).勢必使超調(diào)量增加.控制品質(zhì)變壞。作為防止積分飽和的辦法之一.可對計(jì)算出的控制量u<k>限幅.同時(shí)把積分作用切除掉。程序體現(xiàn)：if<speed>400>speed=400;if<speed<0>speed=0;調(diào)試過程5.1.只有比例環(huán)節(jié)Kp=1.8.超調(diào)量為3%.調(diào)節(jié)時(shí)間為4s.峰值時(shí)間為3s只有比例環(huán)節(jié).Kp=2.5.超調(diào)量為4%.調(diào)節(jié)時(shí)間為14s.峰值時(shí)間為3s只有比例環(huán)節(jié).Kp=3.0.波形振蕩在此只舉例三個(gè)圖.其實(shí)作者測過很多種Kp了.根據(jù)上面三個(gè)圖：選擇Kp=1.8；5.2.加入積分環(huán)節(jié)PI：T=1;Kp=1.8;Ti=1.8;出現(xiàn)此現(xiàn)象原因：在一般的PID控制中.當(dāng)有較大的擾動(dòng)或大幅度改變給定值是.由于此時(shí)有較大的偏差.以及系統(tǒng)有慣性和滯后.故在積分項(xiàng)的作用下.往往會(huì)產(chǎn)生較大的超調(diào)和長時(shí)間的波動(dòng)。為此.我采用積分分離措施.即偏差e〔k較大時(shí).取消積分作用；當(dāng)偏差e〔k較小時(shí)才將積分作用投入；程序有：en=set-count;if<en>=5&&en<=-5>{Un=Kp*<en-en1>+Ki*en;}elseUn=Kp*en;5.3.加入積分分離按Ti從大往小調(diào)情況有：T=1;Kp=1.8;Ti=8.0;T=1;Kp=1.8;Ti=1.8;超調(diào)量3%.3s內(nèi)達(dá)到穩(wěn)定值.波形與只有比例環(huán)節(jié)的圖形有明顯的改善只有比例環(huán)節(jié).與上圖進(jìn)行比較5.4.加入微分環(huán)節(jié)系統(tǒng)只有比例和積分環(huán)節(jié)PI.當(dāng)改變期望值時(shí).通過下圖可看出過度波動(dòng)很大.對系統(tǒng)不利。T=1;Kp=1.8;Ti=1.8;為此采取積分環(huán)節(jié)Td按從小調(diào)到大的規(guī)則進(jìn)行尋找合適值T=1;Kp=1.8;Ti=1.8;Td=1;[改變調(diào)速時(shí).用了6s時(shí)間達(dá)到穩(wěn)定.過度波動(dòng)還是很大]T=1;Kp=1.8;Ti=1.8;Td=1.8;[改變調(diào)速時(shí).用5s時(shí)間達(dá)到穩(wěn)定.波動(dòng)明顯變小]T=1;Kp=1.8;Ti=1.8;Td=2.5;[用1s時(shí)間達(dá)到穩(wěn)定.過度無波動(dòng).完全符合要求]設(shè)計(jì)心得為時(shí)兩周時(shí)間的課程設(shè)計(jì).不但讓我對計(jì)算機(jī)控制技術(shù)有了更深的理解.而且熟悉了數(shù)字PID的編程辦法.在做的過程中遇到的問題很多。比如一開始選PID參數(shù)整定辦法時(shí)我們是用了擴(kuò)充臨界比例度法.但是在調(diào)試過程始終達(dá)不到理想效果.后來發(fā)現(xiàn)是我們采樣周期太大的問題.遠(yuǎn)大于系統(tǒng)純滯后時(shí)間.但是我們改變采樣周期后.顯示和采樣效果均不理想.為此我們決定采用湊試法整定PID參數(shù).在調(diào)試過程中.發(fā)現(xiàn)效果很明顯.先只采用比例環(huán)節(jié).待達(dá)到理想效果之后保持Kp值不變.繼續(xù)加入積分環(huán)節(jié).但是問題出現(xiàn)了.加入積分環(huán)節(jié)不但不能消除靜差.反而使系統(tǒng)發(fā)生了較大超調(diào)和長時(shí)間的波動(dòng).為此我們查找了相關(guān)書籍.得出的結(jié)論是在一般的PID控制中.當(dāng)有較大的擾動(dòng)或大幅度改變給定值是.由于此時(shí)有較大的偏差.以及系統(tǒng)有慣性和滯后.故在積分項(xiàng)的作用下.往往會(huì)產(chǎn)生較大的超調(diào)和長時(shí)間的波動(dòng)。為此我們采用了積分分離的辦法.但達(dá)到設(shè)定誤差時(shí)才啟用積分環(huán)節(jié).調(diào)試之后效果非常理想。接著再加入微分環(huán)節(jié).此環(huán)節(jié)比較順利.安照書上辦法把Td值從小變到大從而找到合適值.從而使系統(tǒng)在期望值改變的時(shí)候不會(huì)出現(xiàn)超調(diào)或者波動(dòng).能實(shí)現(xiàn)平緩過渡。至此我們成功設(shè)計(jì)了直流電機(jī)調(diào)速系統(tǒng).對學(xué)到知識感動(dòng)非常開心與滿足。參考文獻(xiàn)[1]于海生.計(jì)算機(jī)控制技術(shù).北京：機(jī)械工業(yè)出版社[2]郭天祥.新概念51單片機(jī)C語言教程.電子工業(yè)出版社[3]湊試法整定PID參數(shù).百度文庫[4]ULN2003A數(shù)據(jù)手冊[DB/OL]..附錄〔源程序#include<reg52.h>#include<stdio.h>//LCD//sbitRS=P2^6;sbitRW=P2^5;sbitEN=P2^7;sbitkey=P2^0;sbitkey1=P2^1;sbitkey2=P2^2;sbitkey3=P2^3;//motor//unsignedinttt0;unsignedinttt1;sbitA1=P1^2;sbitA2=P1^3;unsignedcharflag;//抽樣時(shí)間unsignedintspeed=41;unsignedinta;unsignedcharreceive;unsignedintReData,sum=0,summ=0;//pidfloatKp,Ki,Kd,Ti,Td,T;//unsignedintset=100;inten,en1,en2,enx;doubleUn;voiddelay<unsignedintxms>{unsignedi,j;for<i=xms;i>0;i-->for<j=110;j>0;j-->;}bittest_busy<>{bitresult;P0=0xff;RS=0;RW=1;delay<1>;EN=1;delay<1>;result=<bit><P0&0x80>;EN=0;returnresult;}voidwrite_command<unsignedcharcommand>//寫命令{while<test_busy<>>;RS=0;RW=0;P0=command;delay<1>;EN=1;delay<1>;EN=0;}voidwrite_date<unsignedchardate>//寫數(shù)據(jù){while<test_busy<>>;RS=1;RW=0;P0=date;delay<1>;EN=1;delay<1>;EN=0;}voidwrite_string<unsignedchar*s>{while<*s>{write_date<*s>;s++;}}voidlcd_initial<>//LCD初始化{RS=1;RW=1;EN=0;write_command<0x38>;//設(shè)置16X2顯示.5X7點(diǎn)陣.8位數(shù)據(jù)接口delay<1>;write_command<0x01>;//顯示清零.數(shù)據(jù)指針清零delay<1>;write_command<0x0c>;//設(shè)置開顯示.不顯示光標(biāo)delay<1>;write_command<0x06>;//寫一個(gè)字符后地址指針加1}voidkeyscan<>{if<key==0>{delay<10>;if<key==0>{set=set+1;if<set>=138>set=138;while<!key>;receive=1;}}if<key1==0>{delay<10>;if<key1==0>{set=set-1;if<set<=90>set=90;while<!key1>;receive=1;}}}voidkeyspeed<>{if<key2==0>{delay<10>;if<key2==0>{speed=speed+1;while<!key2>;}}if<key3==0>{delay<10>;if<key3==0>{speed=speed-1;while<!key3>;}}}voidinit<>{TMOD=0x23;//0010_0011定時(shí)器0:模式3定時(shí)器1:模式2SCON=0x50;//0101_0000串行口方式110位UART波特率可變PCON|=0x80;//SMOD=1串行口波特率加倍T2CON=0x02;//計(jì)數(shù)器2僅當(dāng)定時(shí)器溢出時(shí)捕獲和重裝TH0=<256-100>;//驅(qū)動(dòng)電機(jī)TL0=<256-250>;//定時(shí)0.00025sTH1=243;//256-2^1*11.0592/<384*9600>波特率為9600TL1=243;TH2=0;TL2=0;TR0=1;//啟動(dòng)定時(shí)器TR1=1;TR2=1;ET0=1;//允許T0中斷ET1=1;ET2=1;ES=1;//串行口中斷位EA=1;//中斷允許位}//************************************************************************voidt_0<>interrupt1//定時(shí)100ms<抽樣時(shí)間>{//unsignedcharnum;TL0=<256-250>;tt0++;if<tt0==400>{tt0=0;//num++;flag=1;}}//************************************************************************voidt_1<>interrupt3//驅(qū)動(dòng)電機(jī)PWMspeed*發(fā)生一次中斷時(shí)間=低電平.〔100-speed*中斷時(shí)間=高電平{TH0=<256-100>;tt1++;if<tt1>=400>tt1=0;if<tt1<speed>//speed是控制占空比的{A1=1;A2=0;}else{A1=0;A2=0;}}/********************基本用不到,防止溢出而已****************************/voidt_2<>interrupt5//計(jì)數(shù)器{TH2=0;TL2=0;TF2=0;}/*********PID******************************************************/voidpid<unsignedintcount>//增量式{en=set-count;//Un=q0*en;//-q1*en1+q2*en2if<en>=5&&en<=-5>{Un=Kp*<en-en1>+Ki*en+Kd*<en-2*en1+en2>;}//elseUn=Kp*en;speed=speed+Un;if<speed>40