簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置

1、 簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置設(shè)計(jì)報(bào)告及總結(jié) 摘要 倒立擺系統(tǒng)機(jī)理的研究不僅具有重要的理論價(jià)值，而且具有重要的現(xiàn)實(shí)意義，是控制類中經(jīng)久不衰的經(jīng)典題型。本題中，簡(jiǎn)易旋轉(zhuǎn)倒立擺，在C8051F040單片機(jī)的基礎(chǔ)上，使用ZGB42FM直流減速電機(jī)，BTN7971B電機(jī)驅(qū)動(dòng)，可變電阻（角度傳感器），機(jī)械擺桿等模塊。通過(guò)編寫(xiě)、燒入程序，調(diào)控硬件協(xié)調(diào)工作，使擺桿首先實(shí)現(xiàn)一定角度的轉(zhuǎn)動(dòng)，再完成圓周運(yùn)動(dòng)，以及保持豎直向上的倒立狀態(tài)。用以滿足題目的基本要求,進(jìn)而深一步探究倒立擺在保持運(yùn)動(dòng)姿態(tài)方面的發(fā)展與應(yīng)用。關(guān)鍵字：?jiǎn)纹瑱C(jī)，倒立擺，擺桿，可變電阻。引言：本題整體上只由一個(gè)電機(jī)A提供動(dòng)力，電機(jī)直接控制旋轉(zhuǎn)臂C做往復(fù)旋

2、轉(zhuǎn)運(yùn)動(dòng)，而通過(guò)轉(zhuǎn)軸D連接在旋臂C上的擺桿E是非常靈活的。旋臂C轉(zhuǎn)動(dòng)一定角度時(shí)，擺桿E由于向心力會(huì)使擺桿E繼續(xù)向上旋轉(zhuǎn)，以達(dá)到E桿轉(zhuǎn)動(dòng)一個(gè)角度的效果。相似，當(dāng)C的轉(zhuǎn)動(dòng)速度比較快，停下后，E下端處的速度和向心力都比較大，能夠使E桿完成圓周運(yùn)動(dòng)。 為了使擺桿能夠倒立，就要求擺桿轉(zhuǎn)動(dòng)到上半圓周面時(shí)，要通過(guò)單片機(jī)控制電機(jī)A不斷的調(diào)整使旋轉(zhuǎn)臂C轉(zhuǎn)動(dòng)多個(gè)角度，盡量的使擺桿E與豎直面的角度變小，并能夠受力平衡，這樣就可以保持一段時(shí)間的倒立狀態(tài)。為達(dá)到角度的調(diào)整，就要測(cè)量出E桿與豎直面間的角度差，經(jīng)過(guò)單片機(jī)的控制，使電機(jī)A做出相應(yīng)的旋轉(zhuǎn)動(dòng)作，減小這個(gè)角度差。 1、方案設(shè)計(jì)與討論： 1.1結(jié)構(gòu)框圖 單片機(jī) 執(zhí)行電

3、機(jī) 電機(jī)驅(qū)動(dòng) 角度傳感器 擺桿 1.2方案論證： 1.21控制器模塊 本題，單片機(jī)只要接收來(lái)自傳感器的信號(hào)，向電機(jī)驅(qū)動(dòng)輸入信號(hào)處理后計(jì)算出的高低電平即可。 方案一：用ATMEL公司生產(chǎn)的AT89S52單片機(jī)，低功耗，高性能CMOS 8位處理器，使用廣泛,算法較為簡(jiǎn)單，但是在執(zhí)行復(fù)雜動(dòng)作時(shí)，處理速度不夠高。 方案二:用宏晶公司生產(chǎn)的STC89C52RC單片機(jī)，STC的單片機(jī)性能與ATMEL的單片機(jī)相似，但是價(jià)格相對(duì)便宜。缺點(diǎn)是易受潮濕影響，在調(diào)用子程序是頻繁出錯(cuò)。 方案三:使用C8051F單片機(jī)該芯片與標(biāo)準(zhǔn)的8051芯片兼容，擁有高速指令處理能力，增加了中斷源，復(fù)位源，內(nèi)部有兩個(gè)12位的ADC子

4、系統(tǒng)，有JTAG調(diào)試和邊界掃描，片內(nèi)集成的SPI接口，方便系統(tǒng)外設(shè)擴(kuò)展。 經(jīng)過(guò)討論后選擇更適合本題要求的方案三，使用C8051F單片機(jī)。 1.22電機(jī)模塊 電機(jī)是本題的唯一動(dòng)力裝置，要求能夠帶動(dòng)旋轉(zhuǎn)臂C往復(fù)旋轉(zhuǎn)一定角度，再使擺桿E發(fā)生擺動(dòng)。 方案一、使用28BYJ-48 5V驅(qū)動(dòng)的步進(jìn)電機(jī)，步進(jìn)電機(jī)可以滿足角度旋轉(zhuǎn)的要求，轉(zhuǎn)速較慢，易于控制，但是扭力較小。 方案二、采用ZGB42FM直流減速電機(jī)，具有比一般直流電機(jī)更大的扭力和力矩，控制也相對(duì)簡(jiǎn)單。 方案三、采用無(wú)刷電機(jī)，三廂的無(wú)刷電機(jī)可以做到較為精確的轉(zhuǎn)角。但是不易控制，輸出小，易磨損，成本也相對(duì)較高。 在本題中，我們使用長(zhǎng)厚木板作為旋轉(zhuǎn)臂C

5、，對(duì)電機(jī)的輸出要求較大。經(jīng)比較采用了方案二，能夠提供較大轉(zhuǎn)速和扭力的直流減速電機(jī)ZGB42FM直流減速電機(jī)。 1.23電機(jī)驅(qū)動(dòng)模塊 方案一、使用人工焊接的H橋，可以使電機(jī)正常工作，但是自主焊接電路板可能會(huì)出現(xiàn)硬件問(wèn)題。 方案二、用BTN7971B驅(qū)動(dòng)模塊，相當(dāng)于一個(gè)半橋，也可以滿足電機(jī)的驅(qū)動(dòng)要求，安裝空間也比較小。 在使用焊接的H橋時(shí)，MOS管升溫很快，硬件部分存在隱患，所一最終我們選用了與H橋功能相同的方案二BTN電機(jī)驅(qū)動(dòng)模塊。 1.24傳感器模塊 本題中只需使用到角度傳感器 方案一、使用MPU-6050模塊，作為角度傳感器，其集合程度高，免除了組合陀螺儀與加速器時(shí)之軸間差的問(wèn)題，減少了大量

6、的包裝空間，工作效果好。但程序復(fù)雜，成本較高。 方案二、使用可變電阻，耐磨損，壽命長(zhǎng)，線性精度高。可變電阻同時(shí)具有與角度傳感器相同的功能，成本也相對(duì)較低。 所以經(jīng)過(guò)比較，選擇方案二，一個(gè)可變電阻即可。1.3設(shè)計(jì)實(shí)現(xiàn)： 硬件的設(shè)計(jì)與制作 除了以上論證后選用的模塊，對(duì)于機(jī)械方面的部分的設(shè)計(jì)， 將電機(jī)套在一塊用樹(shù)脂膠做成的模具上，裝上用以固定，用一根厚長(zhǎng)條形的木板做旋轉(zhuǎn)臂C，擺桿E也以軸接的方式連接在旋轉(zhuǎn)臂邊緣的WDD35D4的轉(zhuǎn)軸上，不影響E的靈活性。擺桿E同樣是金屬質(zhì)地，為的是保證E的質(zhì)量，使其能夠有較大的慣性和向心力。在方案的實(shí)施過(guò)程中，首先使用普通的直流電機(jī)進(jìn)行測(cè)試，與作為旋轉(zhuǎn)臂的厚長(zhǎng)木條連

7、接后電機(jī)轉(zhuǎn)動(dòng)很慢，未提供較大的扭力和，最終選用了ZGB42FM直流減速電機(jī)，解決動(dòng)力提供不足的問(wèn)題。木條與電機(jī)轉(zhuǎn)軸的一端不容易接上，最終使用樹(shù)脂膠將木條與電機(jī)轉(zhuǎn)軸固定在一起，以達(dá)到運(yùn)動(dòng)狀態(tài)一致。在測(cè)試電機(jī)時(shí)，嘗試使用自己焊接的H橋。電機(jī)短暫工作后，發(fā)現(xiàn)MOS管發(fā)燙，在用新的MOS管焊接成H橋后測(cè)試時(shí)依然溫度過(guò)高，為避免出現(xiàn)硬件問(wèn)題，選擇裝上BTN7971B電機(jī)驅(qū)動(dòng)模塊。最終將電機(jī)固定在一塊平面木板上，將木板釘在板凳的四只并且在運(yùn)轉(zhuǎn)時(shí)保持穩(wěn)定的狀態(tài)2.理論分析與計(jì)算 2.1電機(jī)的選擇 我們選擇了ZGB42FM直流減速電機(jī)，額定電壓12V，轉(zhuǎn)速300d/分，該電機(jī)本身轉(zhuǎn)速和扭力都比較大，驅(qū)動(dòng)重物也

8、可以輕松運(yùn)轉(zhuǎn)。2.2擺桿狀態(tài)檢測(cè) 是由空心金屬棒制成，與軸承緊密連接，當(dāng)擺桿被轉(zhuǎn)到九十度后松手，擺桿可在自身重力的作用下做單擺運(yùn)動(dòng)直至停下來(lái)回?cái)[動(dòng)超過(guò)三個(gè)往復(fù)運(yùn)動(dòng)，最后保持著豎直向下的狀態(tài)。2.3驅(qū)動(dòng)在該題中，我們采用了BTN7971B電機(jī)驅(qū)動(dòng)，在接收到單片機(jī)輸出的高低電平后，驅(qū)動(dòng)會(huì)驅(qū)使電機(jī)做出相應(yīng)的運(yùn)轉(zhuǎn)動(dòng)作。2.4控制算法 控制算法是單片機(jī)控制的重要組成部分，合適的算法能夠簡(jiǎn)單，高效的解決問(wèn)題。本題中，由于需要不斷的調(diào)整旋轉(zhuǎn)臂的轉(zhuǎn)角，使用PID算法比較合適。PID控制器本身是一種基于對(duì)“過(guò)去”、“現(xiàn)在”和“未來(lái)”信息估計(jì)的簡(jiǎn)單控制算法。u1(t) u(t) -+r(t) 積分比例微分被控對(duì)象

9、c(t) +y(t) v(t) PID控制系統(tǒng)原理框圖該系統(tǒng)主要由PID控制器和被控對(duì)象組成。作為一種線性控制器，它根據(jù)設(shè)定值r(t)和實(shí)際輸出值y(t)構(gòu)成控制偏差e(t)，將偏差按比例、積分和微分通過(guò)線性組合構(gòu)成控制量u(t)，對(duì)被控對(duì)象進(jìn)行控制?？刂破鞯妮斎胼敵鲫P(guān)系可描述為: 式中:e(t)=r(t)-u(t), K為比例系數(shù)T,為積分時(shí)間常數(shù)、T為微分時(shí)間常數(shù)。使用位置式PID控制算法。如果基于用求和代替積分，用差分代替微分來(lái)進(jìn)行離散化，那么，離散化之后的數(shù)字量的PID控制算式為經(jīng)z變換后，數(shù)字PID校正器的脈沖傳遞函數(shù)的一般形式為：3.電路圖及原理圖見(jiàn)附錄1 主程序流程圖及PID子程

10、序流程圖見(jiàn)附錄2 程序見(jiàn)附錄4 54.測(cè)量方案及測(cè)試結(jié)果 測(cè)量?jī)x器：電腦，MATLAB 軟件。 測(cè)量方案： 利用MATLAB仿真，通過(guò)建模將非線性關(guān)系轉(zhuǎn)化成線性關(guān)系。分別測(cè)出旋轉(zhuǎn)臂的角度和擺桿角度的仿真曲線。測(cè)量結(jié)果見(jiàn)附錄35.討論與總結(jié) 5.1 簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置的完成情況 題目完成情況 未完成原因基礎(chǔ)一。 擺桿從處于自然下垂?fàn)顟B(tài)（擺角 0°）開(kāi)始，驅(qū)動(dòng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂作往復(fù)旋轉(zhuǎn)使擺桿擺動(dòng)，并盡快使擺角達(dá)到或超過(guò)-60° +60°完成基礎(chǔ)二。 從擺桿處于自然下垂?fàn)顟B(tài)開(kāi)始，盡快增大擺桿的擺動(dòng)幅度，直至完成圓周運(yùn)動(dòng)完成基礎(chǔ)三。 在擺桿處于自然下垂?fàn)顟B(tài)下，外力拉起

11、擺桿至接近 165°位置，外力撤除同時(shí)，啟動(dòng)控制旋轉(zhuǎn)臂使擺桿保持倒立狀態(tài)時(shí)間不少于 5s；期間旋轉(zhuǎn)臂的轉(zhuǎn)動(dòng)角度不大于 90°完成發(fā)揮一。 從擺桿處于自然下垂?fàn)顟B(tài)開(kāi)始，控制旋轉(zhuǎn)臂作往復(fù)旋轉(zhuǎn)運(yùn)動(dòng)，盡快使擺桿擺起倒立，保持倒立狀態(tài)時(shí)間不少于 10s未完成硬件制作粗糙，穩(wěn)定性差，程序難以調(diào)整出合適的數(shù)據(jù)去維持穩(wěn)定狀態(tài)。發(fā)揮二。 在擺桿保持倒立狀態(tài)下，施加干擾后擺桿能繼續(xù)保持倒立或 2s 內(nèi)恢復(fù)倒立狀態(tài)；未完成發(fā)揮三。 在擺桿保持倒立狀態(tài)的前提下，旋轉(zhuǎn)臂作圓周運(yùn)動(dòng)，并盡快使單方向轉(zhuǎn)過(guò)角度達(dá)到或超過(guò) 360°未完成5.2心得體會(huì)： 比賽完后，我們各組都總結(jié)了一下，認(rèn)為，賽前準(zhǔn)

12、備很不充分，要用的元器件多有短缺，有些器件只有一兩件。在調(diào)試過(guò)程中產(chǎn)生損耗后，還要去網(wǎng)購(gòu)，浪費(fèi)時(shí)間。同時(shí)，組員間的配合不緊密，工作起來(lái)七手八腳，亂糟糟的。工作態(tài)度不嚴(yán)謹(jǐn)，硬件制作粗糙，工具隨意亂丟，用起來(lái)卻找不著。 出現(xiàn)的這些問(wèn)題就是我們今后改進(jìn)的方向，在賽前要充分準(zhǔn)備，多準(zhǔn)備元器件用以備用，工具設(shè)備也要準(zhǔn)備充足以方便工作。同時(shí)加強(qiáng)訓(xùn)練，讓參賽隊(duì)員嚴(yán)肅，認(rèn)真地對(duì)待比賽，熟悉隊(duì)友，做到默契分工合作。6. 參考文獻(xiàn)： 黃南晨旋轉(zhuǎn)式倒立擺系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)碩士學(xué)位論文】，中國(guó)科學(xué)技術(shù)大學(xué)控制理論與控制工程專業(yè)，2002 管鳳旭倒立擺系統(tǒng)的可拓控制策略及實(shí)驗(yàn)研究【碩士學(xué)位論文】，哈爾濱工程大學(xué)控制理論與

13、控制工程專業(yè)，2006 黃輝基于可拓控制的仿真研究碩士學(xué)位論文】，南京理工大學(xué)控制理論與控制工程專業(yè)，2003 王俊基于倒立擺的三種控制策略的研究碩士學(xué)位論文】，湖北工業(yè)大學(xué)電力電子與電力傳動(dòng)專業(yè)，2008 李曉丹模糊PID控制器的設(shè)計(jì)研究陬士學(xué)位論文】，天津大學(xué)控制理論與控制工程專業(yè)，2005王曉明電動(dòng)機(jī)的單片機(jī)控制北京航空航天大學(xué)出版社(第2版)，2007 江晨，王富東旋轉(zhuǎn)式倒立擺系統(tǒng)的算法研究及仿真工業(yè)控制計(jì)算機(jī)2010附錄1：C8051F040單片機(jī) BTN7971B驅(qū)動(dòng)模塊 開(kāi)始PID子程序附錄2 開(kāi)始 計(jì)算偏差 E(n)=R(n)-Y(n) 程序初始化 計(jì)算比例項(xiàng) P-term=K

14、p*E(n)讀入Kp,Ki,Kd 計(jì)算積分項(xiàng)并限幅 I-term=Ki*Ts*sumE判斷PID-flag 是否為 1 NO YES 計(jì)算積分項(xiàng)并限幅 D-term=Kd*(E(n)-E(n-2)/(2*Ts)調(diào)用數(shù)據(jù)采集處理子程序-調(diào)用PID子程序 計(jì)算輸出控制量C(n) C(n)=P_term+I_term+D_term將PID_flag位清0調(diào)用電機(jī)控制子程序主程序流程圖PID子程序結(jié)束 PID子程序流程圖 附錄3旋轉(zhuǎn)臂轉(zhuǎn)動(dòng)角度和擺桿轉(zhuǎn)動(dòng)角度的仿真曲線 附錄4#include <c8051F040.h> / SFR declarations#include<math.h

15、>/-/ 16-bit SFR Definitions for 'F04x/-sfr16 ADC0 = 0xbe; / ADC0 datasfr16 RCAP2 = 0xca; / Timer2 capture/reloadsfr16 RCAP3 = 0xca; / Timer3 capture/reloadsfr16 TMR2 = 0xcc; / Timer2sfr16 TMR3 = 0xcc; / Timer3sbit PWM = P00; / LED='1' means ONsbit SW1 = P37; / SW1='0' means sw

16、itch pressedsbit P4_0 = P40; / LED='1' means ONsbit P4_1 = P41; / SW1='0' means switch pressed/-/ Global Constants/-#define BAUDRATE 115200 / Baud rate of UART in bps#define SYSCLK 24500000 / System Clock#define SAMPLE_RATE 64 / Sample frequency in Hz#define INT_DEC 256 / Integrate a

17、nd decimate ratio#define SAR_CLK 2500000 / Desired SAR clock speed#define TIMER0_RELOAD_HIGH (65535-40)/256#define TIMER0_RELOAD_LOW (65535-40)%256#define a_moto_go P4_1=0; /左邊兩個(gè)電機(jī)向前走#define a_moto_back P4_1=1; /左邊兩個(gè)電機(jī)向后轉(zhuǎn)/#define a_moto_Stop P4_0=0,P4_1=0; /左邊兩個(gè)電機(jī)停轉(zhuǎn) #define a_moto_pwm P4_0 /PWM信號(hào)端bi

18、t a_moto_stop =1;bit flag=1;int en0,en1,en2, DATA;int SumE,d_term,i_term;long Cn;unsigned char K,Ki,Kd;unsigned char PID_flag,motor_flag;/*typedef struct PID uint16 SetPoint ;/設(shè)定目標(biāo)uint16 Proportion ;/比例常數(shù)uint16 Integral ; /積分常數(shù)uint16 Derivative ;/微分常數(shù)uint16 LastError ; /Error-1uint16 PrevError ; /Er

19、ror-2 PID ;*/-/ Function Prototypes/-void OSCILLATOR_Init (void); void PORT_Init (void);void UART1_Init (void);void ADC0_Init (void);void TIMER3_Init (int counts);void ADC0_ISR (void);void Wait_MS (unsigned int ms);void PCA0_Init (void);void Timer0_Init (void);void pwm_out_a_moto();void motor();void

20、 PID();/-/ Global Variables/-unsigned short Result; / ADC0 decimated valuelong delay_count = 0;unsigned char pwm_val_a =0;/變量定義unsigned char push_val_a =0;/ 左電機(jī)占空比N/10/-/ main() Routine/- void run(void) / push_val_a=3; /速度調(diào)節(jié)變量 0-9。9最小，0最大 P4_1=0; /左電機(jī)往前走 void backrun(void) / push_val_a=3; /速度調(diào)節(jié)變量 0-

21、9。9最小，0最大 P4_1=1 ; /左電機(jī)往前走/*/延時(shí)函數(shù) void delay(unsigned int k) unsigned int x,y; for(x=0;x<k;x+) for(y=0;y<2000;y+);void main (void) WDTCN = 0xde; / Disable watchdog timer WDTCN = 0xad; OSCILLATOR_Init (); / Initialize oscillator PORT_Init (); / Initialize crossbar and GPIO Timer0_Init(); TIMER3

22、_Init (SYSCLK/SAMPLE_RATE); / Initialize Timer3 to overflow at / sample rate ADC0_Init (); / Init ADC PCA0_Init (); P4 = 0x00; SFRPAGE = ADC0_PAGE; AD0EN = 1; / Enable ADCResult = 0; EA = 1; / Enable global interruptsdelay(50);SFRPAGE = CONFIG_PAGE; K=75;Ki=70;Kd=2;en0=en1=en2=0;Cn=i_term=d_term=0;S

23、umE=0;motor_flag=0; while (1) run();delay(300);backrun();delay(320);/*PID();*/ /-/ Initialization Subroutines/-/-/ OSCILLATOR_Init/-/ Return Value : None/ Parameters : None/ This function initializes the system clock to use the internal oscillator/ at 24.5 MHz./-void OSCILLATOR_Init (void) char SFRP

24、AGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = CONFIG_PAGE; / Set SFR page OSCICN = 0x83; / Set internal oscillator to run / at its maximum frequency CLKSEL = 0x00; / Select the internal osc. as / the SYSCLK source SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ PORT_Init/-/ Return Value : None

25、/ Parameters : None/ This function configures the crossbar and GPIO ports./ P0.4 digital push-pull UART TX/ P0.5 digital open-drain UART RX/ P1.6 digital push-pull LED/ AIN0.1 analog Analog input /-void PORT_Init (void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = CONFIG_PAGE; / se

26、t SFR page XBR0 = 0x08; /XBR0 = 0x00; XBR1 = 0x00; XBR2 = 0x40; / Enable crossbar and weak pull-up / Enable UART1 P0MDOUT |= 0x01; / Set CEX0 (P0.0) to push-pull P4MDOUT |= 0x03; P7MDOUT |= 0xff; SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ ADC0_Init/-/ Return Value : None/ Parameters : None/ Confi

27、gure ADC0 to use Timer3 overflows as conversion source, to/ generate an interrupt on conversion complete, and to use right-justified/ output mode. Enables ADC end of conversion interrupt. Leaves ADC disabled./-void ADC0_Init (void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = ADC0_

28、PAGE; ADC0CN = 0x04; / ADC0 disabled; normal tracking / mode; ADC0 conversions are initiated / on overflow of Timer3; ADC0 data is / right-justified REF0CN = 0x03; / Enable on-chip VREF, / and VREF output buffer AMX0CF = 0x00; / AIN inputs are single-ended (default) AMX0SL = 0x01; / Select AIN0.1 pi

29、n as ADC mux input ADC0CF = (SYSCLK/SAR_CLK) << 3; / ADC conversion clock = 2.5MHz, Gain=1 EIE2 |= 0x02; / enable ADC interrupts SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ TIMER3_Init/-/ Return Value : None/ Parameters :/ 1) int counts - calculated Timer overflow rate/ range is postive rang

30、e of integer: 0 to 32767/ Configure Timer3 to auto-reload at interval specified by <counts> (no/ interrupt generated) using SYSCLK as its time base./-void TIMER3_Init (int counts) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = TMR3_PAGE; TMR3CN = 0x00; / Stop Timer3; Clear TF3;

31、 TMR3CF = 0x08; / use SYSCLK as timebase RCAP3 = -counts; / Init reload values TMR3 = RCAP3; / Set to reload immediately EIE2 &= 0x01; / Disable Timer3 interrupts TR3 = 1; / start Timer3 SFRPAGE = SFRPAGE_SAVE; / Restore SFR pagevoid PCA0_Init (void) char SFRPAGE_save = SFRPAGE; / Save current S

32、FR Page SFRPAGE = PCA0_PAGE; / configure PCA time base; overflow interrupt disabled PCA0CN = 0x00; / Stop counter; clear all flags PCA0MD = 0x04; / Use SYSCLK as time base PCA0CPM0 = 0x42; / Module 0 = 8-bit PWM mode / Configure initial PWM duty cycle = 50% PCA0CPH0 = 256 - (256 * 0.3); / Start PCA

33、counter CR = 1; SFRPAGE = SFRPAGE_save;void Timer0_Init(void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = TIMER01_PAGE; / Set SFR page TH0 = TIMER0_RELOAD_HIGH; / Init Timer0 High register TL0 = TIMER0_RELOAD_LOW ; / Init Timer0 Low register TMOD = 0x01; / Timer0 in 16-bit mode CK

34、CON = 0x02; / Timer0 uses a 1:48 prescaler ET0 = 1; / Timer0 interrupt enabled TCON = 0x10; / Timer0 ON SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/*/* PWM調(diào)制電機(jī)轉(zhuǎn)速 */*/* 左電機(jī)調(diào)速 */*調(diào)節(jié)push_val_a的值改變電機(jī)轉(zhuǎn)速,占空比 */void pwm_out_a_moto(void) if(a_moto_stop) if(pwm_val_a<=push_val_a) a_moto_pwm=1; else a_moto_

35、pwm=0; if(pwm_val_a>=10) pwm_val_a=0; else a_moto_pwm=0; void PID() i_term=d_term=0; en0=1990-Result; SumE=SumE+en0; /計(jì)算誤差項(xiàng)之和 if(SumE>10000) /檢測(cè)sumE是否太大 SumE=10000;if(SumE<-10000) /檢測(cè)sumE是否太小 SumE=-10000;i_term=SumE>>6;/計(jì)算sumE/Tf Tf為64HZi_term*=Ki; /計(jì)算積分項(xiàng) d_term=en0-en2;if(d_term>60) /檢測(cè)是否太大 d_term=60;if(d_term<-60) /檢測(cè)是否太小 d_term=-60;d_term*=Kd;d_term*=32;/檢測(cè)微分項(xiàng)if(d_term>2000) d_term=2000;if(d_term<-2000) d_term=-2000; /比例