第5章-智能小車設(shè)計(jì)

第5章智能小車設(shè)計(jì)目錄5.1制作智能小車5.2采用驅(qū)動(dòng)模塊進(jìn)行控制5.3材料清單5.4機(jī)械零件設(shè)計(jì)5.5實(shí)物拼裝5.6成品實(shí)物圖5.7項(xiàng)目拓展-智能巡線避障小車

5.1制作智能小車

大部分同學(xué)小的時(shí)候都?jí)粝胗幸慌_(tái)屬于自己的四驅(qū)車，并且好奇為什么通電四驅(qū)車就會(huì)跑起來(lái)，接下來(lái)的章節(jié)將介紹四驅(qū)車的工作原理以及帶領(lǐng)同學(xué)們了解電機(jī)的類型并且讓同學(xué)們制作出屬于自己的智能小車。5.1.1直流電機(jī)直流電機(jī)（directcurrentmachine）是指能將直流電能轉(zhuǎn)換成機(jī)械能（直流電動(dòng)機(jī)）或?qū)C(jī)械能轉(zhuǎn)換成直流電能（直流發(fā)電機(jī)）的旋轉(zhuǎn)電機(jī)。它是能實(shí)現(xiàn)直流電能和機(jī)械能互相轉(zhuǎn)換的電機(jī)。當(dāng)它作電動(dòng)機(jī)運(yùn)行時(shí)是直流電動(dòng)機(jī)，將電能轉(zhuǎn)換為機(jī)械能；作發(fā)電機(jī)運(yùn)行時(shí)是直流發(fā)電機(jī)，將機(jī)械能轉(zhuǎn)換為電能。直流電機(jī)示意圖如圖5-1所示。圖5-1常見(jiàn)的直流電機(jī)5.1.2直流無(wú)刷電機(jī)的控制原理直流無(wú)刷電機(jī)的控制原理，要讓電機(jī)轉(zhuǎn)動(dòng)起來(lái)，首先控制部就必須根據(jù)hall-sensor感應(yīng)到的電機(jī)轉(zhuǎn)子所在位置，然后依照定子繞線決定開(kāi)啟(或關(guān)閉)換流器(inverter)中功率晶體管的順序，inverter中之AH、BH、CH(這些稱為上臂功率晶體管)及AL、BL、CL(這些稱為下臂功率晶體管)，使電流依序流經(jīng)電機(jī)線圈產(chǎn)生順向(或逆向)旋轉(zhuǎn)磁場(chǎng)，并與轉(zhuǎn)子的磁鐵相互作用，如此就能使電機(jī)順時(shí)/逆時(shí)轉(zhuǎn)動(dòng)。當(dāng)電機(jī)轉(zhuǎn)子轉(zhuǎn)動(dòng)到hall-sensor感應(yīng)出另一組信號(hào)的位置時(shí)，控制部又再開(kāi)啟下一組功率晶體管，如此循環(huán)電機(jī)就可以依同一方向繼續(xù)轉(zhuǎn)動(dòng)直到控制部決定要電機(jī)轉(zhuǎn)子停止則關(guān)閉功率晶體管(或只開(kāi)下臂功率晶體管)；要電機(jī)轉(zhuǎn)子反向則功率晶體管開(kāi)啟順序相反。

基本上功率晶體管的開(kāi)法可舉例如下：AH、BL一組→AH、CL一組→BH、CL一組→BH、AL一組→CH、AL一組→CH、BL一組，但絕不能開(kāi)成AH、AL或BH、BL或CH、CL。此外因?yàn)殡娮恿慵傆虚_(kāi)關(guān)的響應(yīng)時(shí)間，所以功率晶體管在關(guān)與開(kāi)的交錯(cuò)時(shí)間要將零件的響應(yīng)時(shí)間考慮進(jìn)去，否則當(dāng)上臂(或下臂)尚未完全關(guān)閉，下臂(或上臂)就已開(kāi)啟，結(jié)果就造成上、下臂短路而使功率晶體管燒毀。

當(dāng)電機(jī)轉(zhuǎn)動(dòng)起來(lái)，控制部會(huì)再根據(jù)驅(qū)動(dòng)器設(shè)定的速度及加/減速率所組成的命令(Command)與hall-sensor信號(hào)變化的速度加以比對(duì)(或由軟件運(yùn)算)再來(lái)決定由下一組(AH、BL或AH、CL或BH、CL或……)開(kāi)關(guān)導(dǎo)通，以及導(dǎo)通時(shí)間長(zhǎng)短。速度不夠則開(kāi)長(zhǎng)，速度過(guò)頭則減短，此部份工作就由PWM來(lái)完成。PWM是決定電機(jī)轉(zhuǎn)速快或慢的方式，如何產(chǎn)生這樣的PWM才是要達(dá)到較精準(zhǔn)速度控制的核心。

高轉(zhuǎn)速的速度控制必須考慮到系統(tǒng)的CLOCK分辨率是否足以掌握處理軟件指令的時(shí)間，另外對(duì)于hall-sensor信號(hào)變化的資料存取方式也影響到處理器效能與判定正確性、實(shí)時(shí)性。至于低轉(zhuǎn)速的速度控制尤其是低速起動(dòng)則因?yàn)榛貍鞯膆all-sensor信號(hào)變化變得更慢，怎樣擷取信號(hào)方式、處理時(shí)機(jī)以及根據(jù)電機(jī)特性適當(dāng)配置控制參數(shù)值就顯得非常重要?；蛘咚俣然貍鞲淖円詄ncoder變化為參考，使信號(hào)分辨率增加以期得到更佳的控制。電機(jī)能夠運(yùn)轉(zhuǎn)順暢而且響應(yīng)良好，P.I.D.控制的恰當(dāng)與否也無(wú)法忽視。之前提到直流無(wú)刷電機(jī)是閉回路控制，因此回授信號(hào)就等于是告訴控制部電機(jī)轉(zhuǎn)速距離目標(biāo)速度還差多少，這就是誤差(Error)。知道了誤差自然就要補(bǔ)償，方式有傳統(tǒng)的工程控制如P.I.D.控制。但控制的狀態(tài)及環(huán)境其實(shí)是復(fù)雜多變的，若要控制的堅(jiān)固耐用則要考慮的因素恐怕不是傳統(tǒng)的工程控制能完全掌握，所以模糊控制、專家系統(tǒng)及神經(jīng)網(wǎng)絡(luò)也將被納入成為智能型P.I.D.控制的重要理論。5.1.3直流電機(jī)的控制直流電機(jī)控制方法較為簡(jiǎn)單，可以采用Arduinouno直接控制，具體控制連接原理圖如圖5-2所示。控制代碼也相對(duì)簡(jiǎn)單，具體如下文所示。圖5-2直流電機(jī)控制連接圖簡(jiǎn)單控制的示例：voidsetup(){pinMode(3,OUTPUT);}voidloop(){digitalWrite(3,HIGH);delay(2000);pinMode(3,OUTPUT);}intmotorpin=3;voidsetup(){}voidloop(){for(intfadeValue=0;fadeValue<=255;fadeValue+=50){analogWrite(motorpin,fadeValue);delay(2000);}for(intfadeValue=255;fadeValue>=0;fadeValue-=50){analogWrite(motorpin,fadeValue);delay(2000);}}5.2采用驅(qū)動(dòng)模塊進(jìn)行控制驅(qū)動(dòng)模塊根據(jù)功能、性能分為很多種類，本節(jié)根據(jù)實(shí)際需求將介紹L298N驅(qū)動(dòng)模塊。L298N是一種高電壓、大電流電機(jī)驅(qū)動(dòng)芯片。該芯片采用15腳封裝。內(nèi)含兩個(gè)H橋的高電壓大電流全橋式驅(qū)動(dòng)器，可以用來(lái)驅(qū)動(dòng)直流電動(dòng)機(jī)和步進(jìn)電動(dòng)機(jī)、繼電器線圈等感性負(fù)載；采用標(biāo)準(zhǔn)邏輯電平信號(hào)控制；具有兩個(gè)使能控制端，在不受輸入信號(hào)影響的情況下允許或禁止器件工作有一個(gè)邏輯電源輸入端，使內(nèi)部邏輯電路部分在低電壓下工作；可以外接檢測(cè)電阻，將變化量反饋給控制電路。使用L298N芯片驅(qū)動(dòng)電機(jī)，該芯片可以驅(qū)動(dòng)一臺(tái)兩相步進(jìn)電機(jī)或四相步進(jìn)電機(jī)，也可以驅(qū)動(dòng)兩臺(tái)直流電機(jī)。接口定義如圖5-3所示，Arduino+L298N驅(qū)動(dòng)直流電機(jī)接線方式如圖5-4所示，接線方法如圖5-5所示。圖5-3

接口定義（1）簡(jiǎn)單示例圖5-4Arduino+L298N驅(qū)動(dòng)直流電機(jī)接線方式*當(dāng)ENA使能IN1IN2控制OUT1OUT2

*當(dāng)ENB使能IN3IN4控制OUT3OUT4

由于本模塊是2路的H橋驅(qū)動(dòng)，所以可以同時(shí)驅(qū)動(dòng)兩個(gè)電機(jī)，使能ENAENB之后，

*可以分別從IN1IN2輸入PWM信號(hào)驅(qū)動(dòng)電機(jī)1的轉(zhuǎn)速和方向

*可以分別從IN3IN4輸入PWM信號(hào)驅(qū)動(dòng)電機(jī)2的轉(zhuǎn)速和方向圖5-5接線方法（2）接線方式IN1-4分別接Arduinomega2560的PIN4-7，將L298N上的ENA、ENB插口上的插銷拔掉，隨后，將ENA與Arduinomega2560的PIN11腳相連，ENB與Arduinomega2560的PIN8相連。連線示意圖如圖5-6所示。

IN1接PIN4;IN2接PIN5;IN3接PIN6;IN4接PIN7;

ENA接PIN11；ENB接PIN8；圖5-6連線示意圖示例：#definemotor1pin14//定義IN1引腳#definemotor1pin25//定義IN2引腳#definemotor1pwm11//定義ENA引腳(我的是Mega2560的板子)#definemotor2pin16//定義IN3引腳#definemotor2pin27//定義IN4引腳#definemotor2pwm8//定義ENB引腳voidmotor(intmotorpin1,intmotorpin2,intmotorpwm,intval){pinMode(motorpin1,OUTPUT);//輸出第一個(gè)引腳pinMode(motorpin2,OUTPUT);//輸出第二個(gè)引腳digitalWrite(motorpin2,0);//將第二個(gè)引腳置低digitalWrite(motorpin1,1);//將第一個(gè)引腳抬高analogWrite(motorpwm,val);//給EN引腳設(shè)定模擬值,設(shè)定轉(zhuǎn)速}voidsetup(){}voidloop(){inti;for(i=100;i<=255;i++)//讓電機(jī)的轉(zhuǎn)速?gòu)?00到255轉(zhuǎn)動(dòng),A,B轉(zhuǎn)速不一樣,完成轉(zhuǎn)向{motor(motor2pin1,motor2pin2,motor2pwm,255);//電機(jī)B保持255勻速轉(zhuǎn)動(dòng)motor(motor1pin1,motor1pin2,motor1pwm,i);//電機(jī)A從100到255轉(zhuǎn)動(dòng)delay(500);//間隔500ms

}for(i=100;i<=255;i++)//讓電機(jī)從100到255轉(zhuǎn)動(dòng),功能同上{motor(motor1pin1,motor1pin2,motor1pwm,255);motor(motor2pin1,motor2pin2,motor2pwm,i);delay(500);}motor(motor1pin1,motor1pin2,motor1pwm,255);//電機(jī)A以最大轉(zhuǎn)速轉(zhuǎn)動(dòng)delay(5000);motor(motor1pin2,motor1pin1,motor1pwm,255);//電機(jī)A反向轉(zhuǎn)動(dòng)delay(5000);}程序效果：直流電機(jī)B電機(jī)勻速轉(zhuǎn)動(dòng)，A電機(jī)從100加速到255轉(zhuǎn)動(dòng).5.3材料清單材料清單序號(hào)元器件名稱型號(hào)參數(shù)規(guī)格數(shù)量參考實(shí)物圖1ArduinoMega25601個(gè)2萬(wàn)向輪1寸全局從動(dòng)轉(zhuǎn)向輪1個(gè)3語(yǔ)言模塊非特定人聲語(yǔ)音識(shí)別模塊1個(gè)4銅柱M3*64個(gè)5螺絲M3*8126螺母M3127杜邦線20cm公對(duì)母18電池盒四節(jié)電池盒19小車底盤網(wǎng)購(gòu)（有條件可自己3d打印，或者使用洞洞板代替）110橡膠車輪直徑65mm軸孔長(zhǎng)5.3mm211電機(jī)DC3V-6V直流減速電機(jī)212開(kāi)關(guān)小型開(kāi)關(guān)113驅(qū)動(dòng)板L298N114藍(lán)牙串口模塊HC-051個(gè)2.上述清單中的材料中有幾處需要特別注意：（1）第11項(xiàng)直流減速電機(jī)，一定要按照要求購(gòu)買減速電機(jī)，否則將會(huì)像市面上的玩具賽車一樣一經(jīng)上電就具有很高的速度。這樣及其容易燒毀電路。（2）第3項(xiàng)是本項(xiàng)目的語(yǔ)音識(shí)別、對(duì)話功能實(shí)現(xiàn)的模塊。之所以選擇這個(gè)型號(hào)是因?yàn)槠鋬?nèi)置了較大的存儲(chǔ)空間，用戶可以自行定義2000句的對(duì)話?？梢詫?shí)現(xiàn)人和機(jī)器人的對(duì)話交流，也能實(shí)現(xiàn)用戶對(duì)機(jī)器人的語(yǔ)音控制功能。該語(yǔ)音模塊有三個(gè)版本，分別是核心板、升級(jí)版和完全版。核心板的價(jià)錢相對(duì)較低，本項(xiàng)目建議使用該版本。如果有同學(xué)想額外增加語(yǔ)音方面的功能和有這方面需求，可以選擇后面兩個(gè)版本。（3）項(xiàng)目2中萬(wàn)向輪亦可以用牛眼輪來(lái)替代。（4）項(xiàng)目14中需購(gòu)買HC-05種類的藍(lán)牙串口模塊，HC-05主從端一體，可以進(jìn)行信號(hào)的轉(zhuǎn)化。比較出色的完成遙控任務(wù)5.4機(jī)械零件設(shè)計(jì)下車底板零件圖如圖5-7所示，立體圖如圖5-8所示。圖5-7小車底板零件圖圖5-8小車地板立體圖本節(jié)所提供的零件都是3D建模，自行打印完成的，在打印時(shí)需要注意小車地板上的螺絲口以及細(xì)小的口，以免后期因?yàn)楫?dāng)時(shí)的馬虎而造成不必要的損失。5.5實(shí)物拼裝底板拼裝實(shí)物圖如圖5-9所示。圖5-9底板拼裝實(shí)物圖5.5.1電路設(shè)計(jì)智能聲控小車主要由轉(zhuǎn)向機(jī)構(gòu)、驅(qū)動(dòng)機(jī)構(gòu)、驅(qū)動(dòng)控制模塊（Mega2560）、語(yǔ)音控制模塊四大部分組成小車為輪式結(jié)構(gòu)，機(jī)械部分分為轉(zhuǎn)向機(jī)構(gòu)和驅(qū)動(dòng)機(jī)構(gòu)（Mega2560）轉(zhuǎn)向機(jī)構(gòu)主要由轉(zhuǎn)向電機(jī)、轉(zhuǎn)向架和萬(wàn)向輪輪組成驅(qū)動(dòng)機(jī)構(gòu)采用玩具小車常用的雙電機(jī)驅(qū)動(dòng)方案，包括兩個(gè)減速電機(jī)和Mega2560，其原理為：轉(zhuǎn)向時(shí)由控制語(yǔ)音者向小車發(fā)出轉(zhuǎn)向信號(hào)，語(yǔ)音控制模塊向Mega2560發(fā)送指令信息，Mega2560再發(fā)送信號(hào)給轉(zhuǎn)向電機(jī)，轉(zhuǎn)向電機(jī)根據(jù)轉(zhuǎn)向信號(hào)正向或反向旋轉(zhuǎn)一定角度，電機(jī)通過(guò)齒輪、齒條系統(tǒng)帶動(dòng)轉(zhuǎn)向架擺動(dòng)一定角度，最終帶動(dòng)與轉(zhuǎn)向架固定在一起的前輪偏擺一定角度小車在轉(zhuǎn)向時(shí)由于內(nèi)、外側(cè)的車輪的轉(zhuǎn)彎半徑不同，所以內(nèi)外側(cè)車輪的轉(zhuǎn)速也不相同前輪為從動(dòng)輪，會(huì)根據(jù)轉(zhuǎn)彎角度的大小自動(dòng)調(diào)節(jié)內(nèi)、外側(cè)車輪的轉(zhuǎn)速；而后輪為主動(dòng)輪，其轉(zhuǎn)速分別由兩個(gè)電機(jī)獨(dú)立驅(qū)動(dòng)，不會(huì)根據(jù)轉(zhuǎn)彎半徑自動(dòng)調(diào)節(jié)轉(zhuǎn)速因此小車轉(zhuǎn)彎時(shí)，控制系統(tǒng)在控制轉(zhuǎn)向電機(jī)的同時(shí)還需要根據(jù)轉(zhuǎn)向角度的大小向兩個(gè)驅(qū)動(dòng)電機(jī)發(fā)出控制信號(hào)。連接圖如圖5-10所示。圖5-10連接圖5.5.2語(yǔ)音識(shí)別模塊連接1.本項(xiàng)目語(yǔ)言識(shí)別模塊使用了WEGASUN-M6核心板。該模塊一款集語(yǔ)音識(shí)別、語(yǔ)音合成、語(yǔ)音（MP3）點(diǎn)播、RF（射頻）功能、紅外功能于一體的多功能模塊。目前主要應(yīng)用在智能家居、對(duì)話機(jī)器人、車載調(diào)度終端、高端智能語(yǔ)音交互玩具、樓宇智能化、教育機(jī)器人等方面。主打傻瓜式的簡(jiǎn)易操作、優(yōu)越的語(yǔ)音識(shí)別和語(yǔ)音合成性能，應(yīng)用領(lǐng)域十分廣泛。2.模塊連接方法語(yǔ)音識(shí)別模塊與Arduino2560接線方法：語(yǔ)音模塊TXD---2560RX1語(yǔ)音模塊RXD---2560TX1語(yǔ)音模塊GND---2560GND語(yǔ)音模塊3V3---25603.3V

同時(shí)，注意語(yǔ)音識(shí)別模塊的供電。如果發(fā)現(xiàn)語(yǔ)音識(shí)別模塊的3.3V電壓從2560的3.3V引腳取電無(wú)法進(jìn)行語(yǔ)音識(shí)別或者播放語(yǔ)音，那請(qǐng)將語(yǔ)音識(shí)別模塊的5V電源輸入端接到UNO的VIN引腳端，并且選擇外部9V的電源適配器給2560供電，這樣就有充足的電流和電壓了。實(shí)物連接圖如圖5-11所示。圖5-11實(shí)物連接圖3.程序代碼1.核心板代碼@KeyWordBuf02#前進(jìn)001|后退002|$@KeyWordBuf04#左轉(zhuǎn)003|右轉(zhuǎn)004|$

@KeyWordBuf04#停止005|$@WriteKeyWordBuf#$@WriteFlashText#|001！|002！|003！|004！|005！|$2.ArduinoMega2560代碼voidsk(){val=Serial2.read();if(-1!=val){if(1==val){digitalWrite(4,HIGH);digitalWrite(7,HIGH);analogWrite(5,250);analogWrite(6,250);}if(2==val){digitalWrite(4,LOW);digitalWrite(7,LOW);analogWrite(5,250);analogWrite(6,250);}if(3==val){digitalWrite(4,LOW);digitalWrite(7,HIGH);analogWrite(5,240);analogWrite(6,240);}if(4==val){digitalWrite(4,HIGH);digitalWrite(7,LOW);

analogWrite(5,240);analogWrite(6,240);}if(5==val){analogWrite(5,0);analogWrite(6,0);}}}4.藍(lán)牙控制藍(lán)牙小車原理：

app設(shè)定編碼-->通過(guò)手機(jī)藍(lán)牙發(fā)送編碼-->HC05收到編碼，發(fā)送到Arduino2560板子-->板子解析編碼-->控制電機(jī)。下圖5-12為藍(lán)牙控制手機(jī)控制端截圖。圖5-12藍(lán)牙控制手機(jī)控制端截圖點(diǎn)擊打開(kāi)app之后，首先會(huì)自動(dòng)彈出提示要求打開(kāi)手機(jī)藍(lán)牙，可以看到四個(gè)方向箭頭以及中間停止，用來(lái)控制小車前進(jìn)后退左轉(zhuǎn)右轉(zhuǎn)，做這些操作前提是要連接上藍(lán)牙小車的藍(lán)牙模塊HC05.所以需要首先點(diǎn)擊右下角很像螺母的圖標(biāo)彈出下圖5-13藍(lán)牙設(shè)備連接示意圖。圖5-13藍(lán)牙設(shè)備連接示意圖如果沒(méi)有圖5-13所示，請(qǐng)點(diǎn)擊搜索藍(lán)牙設(shè)備，不出意外應(yīng)該會(huì)看到一個(gè)HC06，沒(méi)有的話請(qǐng)確保你的藍(lán)牙模塊電源連接正確?？吹紿C06，點(diǎn)擊一下，如果是第一次配對(duì)，是需要輸入密碼：默認(rèn)是0000或者1234.如果配對(duì)過(guò)，點(diǎn)擊就自動(dòng)連接了。藍(lán)牙小車前進(jìn)后退左轉(zhuǎn)右轉(zhuǎn)停止，都是一種狀態(tài)，所以我們用編碼去區(qū)分。在我的app里，我默認(rèn)設(shè)置為：前進(jìn)3

后退4

左轉(zhuǎn)1

右轉(zhuǎn)2

停止0。當(dāng)然，也可以自定義編碼，點(diǎn)擊左下角進(jìn)入設(shè)置頁(yè)面、如圖5-14、5-15所示。圖5-14設(shè)置界面

編碼只能是0-9數(shù)字以及大小寫26個(gè)字母,足夠使用。圖5-15設(shè)置自定義說(shuō)完app端，要說(shuō)一下怎么測(cè)試編碼是否正確。把下面代碼燒入Arduino2560板子：

voidsetup(){

//putyoursetupcodehere,torunonce:

Serial.begin(9600);

}

voidloop(){

//putyourmaincodehere,torunrepeatedly:

if(Serial.available()>0){

charch=Serial.read();

if(ch=='1’){//前進(jìn)

Serial.println("up");

}elseif(ch=='2’){//后退

Serial.println("back");

}elseif(ch=='3’){//左轉(zhuǎn)

Serial.println("left");

}elseif(ch=='4’){

//右轉(zhuǎn)

Serial.println("right");

}elseif(ch=='0’){

//停車

Serial.println("stop");

}else{

//其他編碼

Serial.println(ch);

}

}

}

然后就可以用ArduinoIDE自帶的串口調(diào)試器來(lái)查看，不出意外應(yīng)該可以看到類似下面的打印，如圖5-16所示。圖5-16打印結(jié)果5.6成品實(shí)物圖電路細(xì)節(jié)圖如圖5-17所示，側(cè)視圖如圖5-18所示，主視圖如圖5-19所示圖5-17電路細(xì)節(jié)圖圖5-18側(cè)視圖圖5-19主視圖1.材料準(zhǔn)備材料清單如表5-2所示，底板實(shí)物圖如圖5-20所示，電路連接圖如圖5-21所示，車前實(shí)物圖如圖5-22所示，小車與遙控器的電路連接圖如圖5-23所示。5.7項(xiàng)目拓展-智能巡線避障小車元器件名稱參數(shù)規(guī)格數(shù)量參考實(shí)物圖巡線傳感器SEN1595-1D7超聲波SRF-041圖5-20實(shí)物圖圖5-21電路連接圖圖5-22實(shí)物圖圖5-23小車與超聲波的電路連接圖#include<Servo.h>//#definesendServomyservo;intEcho=A1;//Echo回聲腳(P2.0)intTrig=A0;//Trig觸發(fā)腳(P2.1)intin1=5;intin2=4;intin3=3;intin4=2;intrightDistance=0,leftDistance=0,middleDistance=0;voidforward(){digitalWrite(in1,HIGH);digitalWrite(in2,LOW);digitalWrite(in3,HIGH);digitalWrite(in4,LOW);}voidback(){digitalWrite(in1,LOW);digitalWrite(in2,HIGH);digitalWrite(in3,LOW);digitalWrite(in4,HIGH);}voidturnleft(){digitalWrite(in1,HIGH);digitalWrite(in2,LOW);digitalWrite(in3,LOW);digitalWrite(in4,HIGH);}voidturnright(){digitalWrite(in1,LOW);digitalWrite(in2,HIGH);digitalWrite(in3,HIGH);digitalWrite(in4,LOW);}voidstop(){digitalWrite(in1,LOW);digitalWrite(in2,LOW);digitalWrite(in3,LOW);digitalWrite(in4,LOW);}

intDistance_test()//量出前方距離{digitalWrite(Trig,LOW);//給觸發(fā)腳低電平2μsdelayMicroseconds(2);digitalWrite(Trig,HIGH);//給觸發(fā)腳高電平10μs，這里至少是10μsdelayMicroseconds(20);digitalWrite(Trig,LOW);//持續(xù)給觸發(fā)腳低電floatFdistance=pulseIn(Echo,HIGH);//讀取高電平時(shí)間(單位：微秒)Fdistance=Fdistance/58;//為什么除以58等于厘米，Y米=（X秒*344）/2//X秒=（2*Y米）/344==》X秒=0.0058*Y米==》厘米=微秒/58return(int)Fdistance;}voidsetup(){myservo.attach(9);Serial.begin(9600);//初始化串口

pinMode(Echo,INPUT);//定義超聲波輸入腳

pinMode(Trig,OUTPUT);//定義超聲波輸