兩足機(jī)器人課程設(shè)計(jì)報(bào)告

1、目錄目錄1第一章 緒論2第二章：硬件設(shè)計(jì)32.1硬件介紹32.1.1傳感器介紹32.1.2舵機(jī)介紹52.2結(jié)構(gòu)設(shè)計(jì)62.2.1 機(jī)器人結(jié)構(gòu)示意圖62.3 驅(qū)動(dòng)器的選擇72.3.1驅(qū)動(dòng)方式72.3.2電動(dòng)機(jī)驅(qū)動(dòng)8第三章 軟件設(shè)計(jì)93.1步態(tài)設(shè)計(jì)103.2控制策略16第四章 設(shè)計(jì)總結(jié)17參考文獻(xiàn)：18第一章 緒論隨著社會(huì)的不斷發(fā)展，各行各業(yè)的分工越來(lái)越明細(xì)，尤其是在現(xiàn)代化的大產(chǎn)業(yè)中，有的人每天就只管擰一批產(chǎn)品的同一個(gè)部位上的一個(gè)螺母，有的人整天就是接一個(gè)線頭，就像電影摩登時(shí)代中演示的那樣，人們感到自己在不斷異化，各種職業(yè)病逐漸產(chǎn)生，于是人們強(qiáng)烈希望用某種機(jī)器代替自己工作，因此人們研制出了機(jī)器人，用

2、以代替人們?nèi)ネ瓿赡切﹩握{(diào)、枯燥或是危險(xiǎn)的工作。由于機(jī)器人的問(wèn)世，使一部分工人失去了原來(lái)的工作，于是有人對(duì)機(jī)器人產(chǎn)生了敵意?！皺C(jī)器人上崗，人將下崗?！辈粌H在我國(guó)，即使在一些發(fā)達(dá)國(guó)家如美國(guó)，也有人持這種觀念。其實(shí)這種擔(dān)心是多余的，任何先進(jìn)的機(jī)器設(shè)備，都會(huì)提高勞動(dòng)生產(chǎn)率和產(chǎn)品質(zhì)量，創(chuàng)造出更多的社會(huì)財(cái)富，也就必然提供更多的就業(yè)機(jī)會(huì)，這已被人類生產(chǎn)發(fā)展史所證明。任何新事物的出現(xiàn)都有利有弊，只不過(guò)利大于弊，很快就得到了人們的認(rèn)可。比如汽車的出現(xiàn)，它不僅奪了一部分人力車夫、挑夫的生意，還常常出車禍，給人類生命財(cái)產(chǎn)帶來(lái)威脅。雖然人們都看到了汽車的這些弊端，但它還是成了人們?nèi)粘Ｉ钪斜夭豢缮俚慕煌üぞ?。英?guó)一位

3、著名的政治家針對(duì)關(guān)于工業(yè)機(jī)器人的這一問(wèn)題說(shuō)過(guò)這樣一段話：“日本機(jī)器人的數(shù)量居世界首位，而失業(yè)人口最少，英國(guó)機(jī)器人數(shù)量在發(fā)達(dá)國(guó)家中最少，而失業(yè)人口居高不下”，這也從另一個(gè)側(cè)面說(shuō)明了機(jī)器人是不會(huì)搶人飯碗的。機(jī)器人技術(shù)建立在多學(xué)科發(fā)展的基礎(chǔ)上，具有應(yīng)用領(lǐng)域廣，技術(shù)新，學(xué)科綜合與交叉性強(qiáng)等特點(diǎn)。傳統(tǒng)的機(jī)器人技術(shù)涉及機(jī)械，電子，自動(dòng)控制等學(xué)科；現(xiàn)代機(jī)器人技術(shù)則綜合了更加廣泛的學(xué)科和技術(shù)領(lǐng)域，如計(jì)算機(jī)技術(shù)，仿生學(xué)，生物工程，人工智能，材料，結(jié)構(gòu)，微機(jī)械，信息工程，遙感等。各種各樣的機(jī)器人不但已經(jīng)成為現(xiàn)代高科技的應(yīng)用載體，而且自身也發(fā)展成為一個(gè)相對(duì)獨(dú)立的研究與交叉應(yīng)用領(lǐng)域，形成了特有的理論研究和學(xué)術(shù)發(fā)展方向

4、，具有鮮明的學(xué)科特色?？梢灶A(yù)見(jiàn)，機(jī)器人技術(shù)將會(huì)滲透到未來(lái)生活的方方面面，而且從瞬息萬(wàn)變的社會(huì)發(fā)展中已經(jīng)可以切身的感受到-機(jī)器人的時(shí)代已經(jīng)悄悄來(lái)臨。第二章：硬件設(shè)計(jì)2.1硬件介紹2.1.1傳感器介紹根據(jù)檢測(cè)對(duì)象的不同可分為內(nèi)部傳感器和外部傳感器。 a. 內(nèi)部傳感器：用來(lái)檢測(cè)機(jī)器人本身狀態(tài)（如手臂間角度）的傳感器。多為檢測(cè)位置和角度的傳感器。 b.外部傳感器：用來(lái)檢測(cè)機(jī)器人所處環(huán)境（如是什么物體，離物體的距離有多遠(yuǎn)等）及狀況（如抓取的物體是否滑落）的傳感器。具體有物體識(shí)別傳感器、物體探傷傳感器、接近覺(jué)傳感器、距離傳感器、力覺(jué)傳感器，聽(tīng)覺(jué)傳感器等。 本課題外部?jī)H應(yīng)用到了紅外接近傳感器，所以這里只對(duì)它

5、進(jìn)行介紹。紅外接近傳感器俗稱光電開(kāi)關(guān)。它是利用被檢測(cè)無(wú)對(duì)光線的遮擋或反射有同步回路選通電路，從而選擇物體有無(wú)的。光電開(kāi)關(guān)將輸入電流在發(fā)射器上轉(zhuǎn)換為光信號(hào)射出，接受其再根據(jù)接收到的光線的強(qiáng)弱或有無(wú)目標(biāo)物體進(jìn)行探測(cè)。工作原理如下圖所示。多數(shù)光電開(kāi)關(guān)選用的是波長(zhǎng)接近可見(jiàn)光的紅外線光波型，因此也稱為紅外開(kāi)關(guān)。由于紅外線是不可見(jiàn)光，紅外探頭體積小巧，隱蔽性非常高，所以各種規(guī)格的紅外開(kāi)關(guān)，紅外測(cè)距傳感器廠常用于安放保衛(wèi)領(lǐng)域。在好萊塢電影里，我們常?？吹浇饚?kù)，博物館里有一條條紅色的光線，達(dá)到門應(yīng)用各種手段避開(kāi)這些光線，最終盜得各種財(cái)寶。實(shí)際上紅外線是不可見(jiàn)的，電影里采用了普通紅色光線冒充紅外線，以烘托緊張的

6、故事情節(jié)，我們對(duì)此大可以笑而過(guò)，不可收到她的誤導(dǎo)。圖2.1.1（a）紅外接近傳感器工作原理光電開(kāi)關(guān)可以分類如下：1.漫反射是光電開(kāi)關(guān)：它是一種集發(fā)射器和接收器于一體的傳感器，當(dāng)有被檢測(cè)物體被檢測(cè)物體經(jīng)過(guò)時(shí)，物體將光電開(kāi)關(guān)發(fā)射器發(fā)射的足夠量的光線反射到接收器，于是光電開(kāi)關(guān)便產(chǎn)生了開(kāi)關(guān)信號(hào)。當(dāng)被檢測(cè)物體的表面光亮或其反光率極高時(shí)，漫反射式的光電開(kāi)關(guān)是首選的檢測(cè)模式。2.經(jīng)反射式的光電開(kāi)關(guān)：它集發(fā)射器與接收器于一體，光電開(kāi)關(guān)發(fā)射器發(fā)出的光線經(jīng)過(guò)反射鏡反射回接收器，當(dāng)被檢測(cè)物體經(jīng)過(guò)且完全阻斷光線時(shí)，光電開(kāi)關(guān)就產(chǎn)生了檢測(cè)開(kāi)關(guān)信號(hào)。3.對(duì)射式光電開(kāi)關(guān)：它包含了在結(jié)構(gòu)上相互分離且光軸相對(duì)放置的接收器和發(fā)射器

7、，發(fā)射器發(fā)出的光線直接進(jìn)入接收器，當(dāng)被檢測(cè)物體進(jìn)入發(fā)射器與接收器之間且阻斷光線時(shí)，光電開(kāi)關(guān)就產(chǎn)生可開(kāi)關(guān)信號(hào)。當(dāng)檢測(cè)物體為不透明時(shí)，對(duì)射式開(kāi)關(guān)是最可靠的檢測(cè)裝置。4.槽式光電開(kāi)關(guān)：它通常采用標(biāo)準(zhǔn)的u字型結(jié)構(gòu)，其發(fā)射器和接收器分別位于u型槽的兩邊，并形成一光軸，當(dāng)被檢測(cè)物體經(jīng)過(guò)u型槽且阻斷光軸時(shí)，光電開(kāi)關(guān)就產(chǎn)生了開(kāi)關(guān)量信號(hào)。槽式光電開(kāi)關(guān)比較適合檢測(cè)高速運(yùn)動(dòng)的物體，并且它能分辨透明與半透明物體，使用安全可靠。5.光纖式光電開(kāi)關(guān)：它采用塑料或玻璃光線傳感器來(lái)引導(dǎo)光線，可以對(duì)距離遠(yuǎn)的被檢測(cè)物體進(jìn)行檢測(cè)，通常光線傳感器分為對(duì)射式和漫反射式。本設(shè)計(jì)所使用的紅外光電開(kāi)關(guān)型號(hào)為e18-b0，規(guī)格數(shù)據(jù)為：vcc：

8、5v。工作電流：小于100ma。輸出形式：npn三極管oc輸出。封裝形式：工程塑料。圖2.1.1（b）紅外接近傳感器紅外接近傳感器是開(kāi)關(guān)量傳感器，接io0io11的任意一個(gè)接口都可以通過(guò)northstar進(jìn)行疏枝讀取和編程。由于輸出是開(kāi)關(guān)量，只能判斷在測(cè)量距離內(nèi)有無(wú)障礙物，不能給出障礙物的實(shí)際距離。但是該傳感器帶有一個(gè)靈敏度調(diào)節(jié)旋鈕，可以調(diào)節(jié)傳感觸發(fā)的距離。2.1.2舵機(jī)介紹舵機(jī)，顧名思義，是控制多面的電機(jī)。舵機(jī)的出現(xiàn)最早是作為遙控模型控制多面、油門等機(jī)構(gòu)的動(dòng)力來(lái)源，但是由于舵機(jī)具有很多優(yōu)秀的特性，在制作機(jī)器人時(shí)也時(shí)常能看到它的應(yīng)用。舵機(jī)最早出現(xiàn)在航模運(yùn)動(dòng)中。在航空模型中，飛行機(jī)的飛行姿態(tài)是通

9、過(guò)調(diào)節(jié)發(fā)動(dòng)機(jī)和各個(gè)控制舵面來(lái)實(shí)現(xiàn)的。不僅在航模飛機(jī)中，在其他的模型運(yùn)動(dòng)中都可以看到它的應(yīng)用，如船模上用它來(lái)控制舵，車磨重用他來(lái)轉(zhuǎn)向等。一般來(lái)講，多極主要由舵盤、減速齒輪組、位置反饋點(diǎn)危機(jī)、直流電機(jī)、控制電路板等幾部分組成。 舵機(jī)是一種位置伺服的驅(qū)動(dòng)器，轉(zhuǎn)動(dòng)范圍一般不能超過(guò)1800，適用于那些角度不斷變化并可以保持的驅(qū)動(dòng)當(dāng)中，如機(jī)器人的關(guān)節(jié)、飛機(jī)的舵面等。不過(guò)也有一些特殊的舵機(jī)，轉(zhuǎn)動(dòng)范圍可達(dá)到5圈之多，主要用于模型帆船的收帆，俗稱帆舵。2.2結(jié)構(gòu)設(shè)計(jì) 機(jī)器人的機(jī)械、電氣和控制結(jié)構(gòu)千差萬(wàn)別。這次的雙足追光機(jī)器人有眼利用的是紅外接近傳感器，手臂利用兩個(gè)舵機(jī)來(lái)驅(qū)動(dòng)，雙足用四個(gè)舵機(jī)驅(qū)動(dòng)。把手臂、雙足、和

10、眼睛固定在控制器上。2.2.1 機(jī)器人結(jié)構(gòu)示意圖圖2.2.1 機(jī)器人結(jié)構(gòu)示意圖 2.2.2 完成的機(jī)器人圖2.2.2 完整機(jī)器人2.3 驅(qū)動(dòng)器的選擇2.3.1驅(qū)動(dòng)方式 驅(qū)動(dòng)方式有三種：液壓式、氣動(dòng)式、電動(dòng)式。 液壓驅(qū)動(dòng)方式的輸出力和功率更大，能構(gòu)成伺服機(jī)構(gòu)，常用于大型機(jī)器人關(guān)節(jié)的驅(qū)動(dòng)。氣動(dòng)式驅(qū)動(dòng)多用于開(kāi)關(guān)控制和順序控制的機(jī)器人。本次課程設(shè)計(jì)采用的是電動(dòng)機(jī)驅(qū)動(dòng)。2.3.2電動(dòng)機(jī)驅(qū)動(dòng) 電動(dòng)機(jī)驅(qū)動(dòng)可分為普通交流電動(dòng)機(jī)驅(qū)動(dòng)，交直流伺服電動(dòng)機(jī)驅(qū)動(dòng)和步進(jìn)電動(dòng)機(jī)驅(qū)動(dòng)。中型或重型機(jī)器人。伺服電動(dòng)機(jī)和步進(jìn)電動(dòng)機(jī)輸出力矩相對(duì)小，控制性能好，可實(shí)現(xiàn)速度和位置的精確控制，適用于中小型機(jī)器人。交、直流伺服電動(dòng)機(jī)一般用于

11、閉環(huán)控制系統(tǒng)，而步進(jìn)電動(dòng)機(jī)則主要用于開(kāi)環(huán)控制系統(tǒng)，一般用于速度和位置精度要求不高的場(chǎng)合。直流伺服電動(dòng)機(jī)結(jié)構(gòu)和原理與普通直流電動(dòng)機(jī)的結(jié)構(gòu)和原理沒(méi)有根本區(qū)別。按照勵(lì)磁方式的不同，直流伺服電動(dòng)機(jī)分為永磁式直流伺服電動(dòng)機(jī)和電磁式直流伺服電動(dòng)機(jī)。永磁式直流伺服電動(dòng)機(jī)的磁極由永久磁鐵制成，不需要?jiǎng)?lì)磁繞組和勵(lì)磁電源。電磁式直流伺服電動(dòng)機(jī)一般采用他勵(lì)結(jié)構(gòu)，磁極由勵(lì)磁繞組構(gòu)成，通過(guò)單獨(dú)的勵(lì)磁電源供電。 按照轉(zhuǎn)子結(jié)構(gòu)的不同，直流伺服電動(dòng)機(jī)分為空心杯形轉(zhuǎn)子直流伺服電動(dòng)機(jī)和無(wú)槽電樞直流伺服電動(dòng)機(jī)?？招谋无D(zhuǎn)子直流伺服電動(dòng)機(jī)由于其力能指標(biāo)較低，現(xiàn)在已很少采用。無(wú)槽電樞直流伺服電動(dòng)機(jī)的轉(zhuǎn)子是直徑較小的細(xì)長(zhǎng)型圓柱鐵芯，通過(guò)

12、耐熱樹(shù)脂將電樞繞組固定在鐵芯上，具有散熱好、力能指標(biāo)高、快速性好的特點(diǎn)?？刂品绞剑?直流電動(dòng)機(jī)的控制方式有兩種：一種稱為電樞控制，在電動(dòng)機(jī)的勵(lì)磁繞組上加上恒壓勵(lì)磁，將控制電壓作用于電樞繞組來(lái)進(jìn)行控制；一種稱為磁場(chǎng)控制，在電動(dòng)機(jī)的電樞繞組上施加恒壓，將控制電壓作用于勵(lì)磁繞組來(lái)進(jìn)行控制。交流伺服電機(jī)的結(jié)構(gòu)交流伺服的電動(dòng)機(jī)結(jié)構(gòu)主要可分為兩部分，即定子部分和轉(zhuǎn)子部分。其中定子的結(jié)構(gòu)與旋轉(zhuǎn)變壓器的定子基本相同，在定子鐵心中也安放著空間互成90度電角度的兩相繞組。其中一組為激磁繞組，另一組為控制繞組，交流伺服電動(dòng)機(jī)一種兩相的交流電動(dòng)機(jī)。 交流伺服電動(dòng)機(jī)使用時(shí)，激磁繞組兩端施加恒定的激磁電壓uf，控制繞組兩

13、端施加控制電壓uk。當(dāng)定子繞組加上電壓后，伺服電動(dòng)機(jī)很快就會(huì)轉(zhuǎn)動(dòng)起來(lái)。 通入勵(lì)磁繞組及控制繞組的電流在電機(jī)內(nèi)產(chǎn)生一個(gè)旋轉(zhuǎn)磁場(chǎng)，旋轉(zhuǎn)磁場(chǎng)的轉(zhuǎn)向決定了電機(jī)的轉(zhuǎn)向，當(dāng)任意一個(gè)繞組上所加的電壓反相時(shí)，旋轉(zhuǎn)磁場(chǎng)的方向就發(fā)生改變，電機(jī)的方向也發(fā)生改變。 為了在電機(jī)內(nèi)形成一個(gè)圓形旋轉(zhuǎn)磁場(chǎng)，要求激磁電壓uj和控制電壓uk之間應(yīng)有90度的相位差，常用的方法有： 1） 利用三相電源的相電壓和線電壓構(gòu)成90度的移相 2） 利用三相電源的任意線電壓3） 采用移相網(wǎng)絡(luò) 4） 在激磁相中串聯(lián)電容器（二）交流伺服電機(jī)的優(yōu)良特性1 控制精度高 步進(jìn)電機(jī) 步進(jìn)的步距角一般為18。(兩相)或072。(五相)，而交流伺服電機(jī)的精度

14、取決于電機(jī)編碼器的精度。以伺服電機(jī)為例，其編碼器為l6位，驅(qū)動(dòng)器每接收2 =65 536個(gè)脈沖，電機(jī)轉(zhuǎn)一圈，其脈沖當(dāng)量為360/65 536=0，0055 ；并實(shí)現(xiàn)了位置的閉環(huán)控制從根本上克服了步進(jìn)電機(jī)的失步問(wèn)題。 2 矩頻特性好 步進(jìn)電機(jī)的輸出力矩隨轉(zhuǎn)速的升高而下降，且在較高轉(zhuǎn)速時(shí)會(huì)急劇下降，其工作轉(zhuǎn)速一般在每分鐘幾十轉(zhuǎn)到幾百轉(zhuǎn)。而交流伺服電機(jī)在其額定轉(zhuǎn)速(一般為2000r/min或3000r/rain)以內(nèi)為恒轉(zhuǎn)矩輸出，在額定轉(zhuǎn)速以e為恒功率輸出。 3 加速性能好 步進(jìn)電機(jī)空載時(shí)從靜止加速到每分鐘幾百轉(zhuǎn)，需要200400ms：交流伺服電機(jī)的加速性能較好由于電樞控制的特性好，電樞控制中回路電

15、感小，響應(yīng)快，在自動(dòng)控制系統(tǒng)中多采用電樞控制。在會(huì)追光的雙足機(jī)器人中，我們選用的是，電動(dòng)機(jī)驅(qū)動(dòng)。 第三章 軟件設(shè)計(jì)3.1步態(tài)設(shè)計(jì)雙足機(jī)器人前進(jìn)分為五步，第一步，左足前進(jìn)，右手臂朝后；第二步，左足恢復(fù)中位狀態(tài)，手臂也恢復(fù)中位狀態(tài)；第三步，右足朝前，左手臂朝后；第四步，右足和左手臂恢復(fù)中位；第五步，重復(fù)第一步，實(shí)現(xiàn)循環(huán)。追光的步態(tài)設(shè)計(jì)：在獲取了兩側(cè)的光強(qiáng)數(shù)值后，便可以開(kāi)始設(shè)計(jì)程序的邏輯框架。將情況分為3種：狀況一，左側(cè)光比右側(cè)光強(qiáng)；狀況二，左側(cè)光比右側(cè)弱，狀況三，左右側(cè)光強(qiáng)近似相等。這三種情況在邏輯上較好區(qū)分，并且覆蓋了所有可能出現(xiàn)的狀態(tài)。在程序的主程序里，用三個(gè)條件來(lái)區(qū)分這三種情況。作為左右光強(qiáng)

16、比較的中間變量，對(duì)兩側(cè)光強(qiáng)的差值diff進(jìn)行計(jì)算：diff=left-right,三種狀況的區(qū)分將圍繞差值diff進(jìn)行：狀況一，左側(cè)光強(qiáng)比右側(cè)光強(qiáng)：diff50狀況二，左側(cè)光強(qiáng)比右側(cè)光弱：diff50狀況三，左側(cè)光強(qiáng)與右側(cè)光接近相等：diff=50在判斷條件里，使用50這個(gè)數(shù)值，是考慮到不同的光強(qiáng)傳感器采集同樣的光照強(qiáng)度數(shù)據(jù)可能存在著差異，因此給出一個(gè)區(qū)間而不是一個(gè)數(shù)據(jù)在控制器運(yùn)行的過(guò)程中，如果在短時(shí)間內(nèi)向多個(gè)舵機(jī)發(fā)送大量的控制指令，會(huì)造成總線的擁堵，導(dǎo)致舵機(jī)收到新指令的時(shí)間延后，最直觀的表現(xiàn)就是指令發(fā)送和舵機(jī)運(yùn)動(dòng)之間存在一個(gè)延時(shí)。為了保持舵機(jī)數(shù)據(jù)總線的流暢，讓舵機(jī)有時(shí)間執(zhí)行接收到的指令，同常

17、在兩次發(fā)送指令之間添加延時(shí)追光機(jī)器人的程序邏輯如下圖：while(1)獲取光強(qiáng)傳感ad值 執(zhí)行一個(gè)左轉(zhuǎn)完整動(dòng)作左邊比較亮執(zhí)行一個(gè)右轉(zhuǎn)完整動(dòng)作右邊比較亮執(zhí)行一個(gè)前進(jìn)完整動(dòng)作左右亮度接近圖3.1（a）程序邏輯由程序流程圖得出的程序如下： 圖3.1（b）程序流程圖程序代碼如下：#include apps/systemtask.huint8 servo_mapping10 = 1,2,3,4,5,6,7,8,9,10;int main() int diff = 0; int left = 0; int right = 0; mfinit(); mfinitservomapping(&servo_map

18、ping0,10); mfsetportdirect(0x00000fff); mfsetservomode(1,0); mfsetservomode(2,0); mfsetservomode(3,0); mfsetservomode(4,0); mfsetservomode(5,0); mfsetservomode(6,0); mfsetservomode(7,0); mfsetservomode(8,0); mfsetservomode(9,0); mfsetservomode(10,0); while (1) left = mfgetad(0); right = mfgetad(1);

19、diff = left - right; if (diff=-50)&(diff50) mfsetservopos(1,512,512); mfsetservopos(2,512,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,480,512); mfsetservopos(6,512,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,5

20、12); mfservoaction(); delayms(2000); mfsetservopos(1,512,512); mfsetservopos(2,480,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,520,512); mfsetservopos(6,650,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,512); mf

21、servoaction(); delayms(2000); /forward 1 mfsetservopos(1,512,512); mfsetservopos(2,480,512); mfsetservopos(3,600,512); mfsetservopos(4,512,512); mfsetservopos(5,520,512); mfsetservopos(6,650,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,512)

22、; mfservoaction(); delayms(2000); /2 mfsetservopos(1,512,512); mfsetservopos(2,512,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,480,512); mfsetservopos(6,512,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,512); mf

23、servoaction(); delayms(2000); if (diff-50) mfsetservopos(1,512,512); mfsetservopos(2,512,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,480,512); mfsetservopos(6,512,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,51

24、2); mfservoaction(); delayms(2000); mfsetservopos(1,512,512); mfsetservopos(2,480,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,520,512); mfsetservopos(6,650,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,512); mfs

25、ervoaction(); delayms(2000); /forward 1 mfsetservopos(1,512,512); mfsetservopos(2,480,512); mfsetservopos(3,600,512); mfsetservopos(4,512,512); mfsetservopos(5,520,512); mfsetservopos(6,650,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,512,512); mfservoaction(); delayms(2000); /2 mfsetservopos(1,512,512); mfsetservopos(2,512,512); mfsetservopos(3,512,512); mfsetservopos(4,512,512); mfsetservopos(5,480,512); mfsetservopos(6,512,512); mfsetservopos(7,512,512); mfsetservopos(8,512,512); mfsetservopos(9,512,512); mfsetservopos(10,5