建造一個(gè)由鋰電池供電的小型機(jī)器人

　　機(jī)器人已經(jīng)慢慢開始爬進(jìn)我們的社會(huì)，讓我們的生活變得更簡(jiǎn)單。我們已經(jīng)可以在英國(guó)的道路上找到來(lái)自 Starship 的六輪送餐機(jī)器人，它們巧妙地在平民中導(dǎo)航到達(dá)目的地。每個(gè)導(dǎo)航到環(huán)境中的移動(dòng)機(jī)器人都應(yīng)該始終了解其相對(duì)于現(xiàn)實(shí)世界的位置和方向。有很多方法可以通過(guò)使用不同的技術(shù)來(lái)實(shí)現(xiàn)這一點(diǎn)，如 GPS、RF 三角測(cè)量、加速度計(jì)、陀螺儀等。每種技術(shù)都有其自身的優(yōu)勢(shì)，并且是獨(dú)一無(wú)二的。在這個(gè)Arduino LM393 速度傳感器教程中，我們將使用簡(jiǎn)單易用的LM393 速度傳感器模塊使用 Arduino 測(cè)量一些重要參數(shù)，例如速度、行進(jìn)距離和機(jī)器人角度。通過(guò)這些參數(shù)，機(jī)器人將能夠了解其真實(shí)世界的狀態(tài)，并可以使用它進(jìn)行安全導(dǎo)航。

　　Arduino 是構(gòu)建機(jī)器人的愛(ài)好者中最受歡迎的選擇，從簡(jiǎn)單的線跟隨器到更復(fù)雜的自平衡或地板清潔機(jī)器人。您可以在機(jī)器人部分查看所有類型的機(jī)器人。

　　我們將建造一個(gè)由鋰電池供電的小型機(jī)器人，并使用操縱桿驅(qū)動(dòng)它。在運(yùn)行期間，我們可以測(cè)量機(jī)器人的速度、距離和角度，并將其實(shí)時(shí)顯示在連接到 Arduino 的 LCD 顯示器上。這個(gè)項(xiàng)目只是幫助你測(cè)量這些參數(shù)，一旦你完成了這個(gè)，你就可以根據(jù)需要使用這些參數(shù)來(lái)自主操作你的機(jī)器人。聽(tīng)起來(lái)很有趣吧？所以讓我們開始吧。

　　LM393 速度傳感器模塊 （H206）

　　在我們進(jìn)入項(xiàng)目的電路圖和代碼之前，讓我們了解LM393 速度傳感器模塊，因?yàn)樗陧?xiàng)目中起著至關(guān)重要的作用。H206 速度傳感器模塊由集成了 LM393 電壓比較器 IC 的紅外光傳感器組成，因此名稱為 LM393 速度傳感器。該模塊還包括一個(gè)網(wǎng)格板，該網(wǎng)格板必須安裝在電機(jī)的旋轉(zhuǎn)軸上。所有組件都標(biāo)記在下圖中。

　　紅外光傳感器由一個(gè) IR LED 和一個(gè)由一個(gè)小間隙隔開的光電晶體管組成。整個(gè)傳感器裝置放置在黑色外殼中，如上圖所示。網(wǎng)格板由槽組成，板布置在紅外光傳感器的間隙之間，傳感器可以感應(yīng)到網(wǎng)格板中的間隙。柵板中的每個(gè)縫隙在通過(guò)縫隙時(shí)都會(huì)觸發(fā)IR傳感器；然后使用比較器將這些觸發(fā)器轉(zhuǎn)換為電壓信號(hào)。比較器不過(guò)是 ON 半導(dǎo)體的 LM393 IC。該模塊共有三個(gè)引腳，其中兩個(gè)用于為模塊供電，一個(gè)輸出引腳用于計(jì)數(shù)觸發(fā)次數(shù)。

　　H206 傳感器安裝布置

　　安裝這些類型的傳感器有點(diǎn)棘手。它只能安裝在兩側(cè)都有軸突出的電機(jī)上。軸的一側(cè)連接到車輪，而另一側(cè)用于安裝網(wǎng)格板，如上圖所示。

　　由于輪子和盤子連接到同一根軸上，兩者都以相同的速度旋轉(zhuǎn)，因此通過(guò)測(cè)量盤子的速度，我們可以測(cè)量輪子的速度。確保網(wǎng)格板中的間隙通過(guò) IR 傳感器，只有這樣傳感器才能計(jì)算已通過(guò)的間隙數(shù)。只要滿足指定條件，您也可以提出自己的機(jī)械布置來(lái)安裝傳感器。紅外傳感器通常用于許多機(jī)器人項(xiàng)目中，以引導(dǎo)機(jī)器人繞過(guò)障礙物。

　　上面顯示的網(wǎng)格板有 20 個(gè)槽（網(wǎng)格）。這意味著傳感器會(huì)為車輪一整圈找到 20 個(gè)間隙。通過(guò)計(jì)算傳感器檢測(cè)到的間隙數(shù)量，我們可以計(jì)算車輪行進(jìn)的距離，類似地，通過(guò)測(cè)量傳感器找到間隙的速度，我們可以檢測(cè)到車輪的速度。在我們的機(jī)器人中，我們將這個(gè)傳感器安裝在兩個(gè)輪子上，因此我們也可以找到機(jī)器人的角度。然而，使用加速度計(jì)或陀螺儀可以更明智地計(jì)算旋轉(zhuǎn)角度，在此處學(xué)習(xí)將加速度計(jì)和陀螺儀與 Arduino連接，并嘗試使用它們來(lái)測(cè)量旋轉(zhuǎn)角度。

　　DIY Arduino LM393 速度傳感器機(jī)器人電路圖

　　該速度和距離感應(yīng)機(jī)器人的完整電路圖如下所示。Bot 由Arduino Nano作為其大腦組成，兩個(gè)用于車輪的直流電機(jī)由 L298N H 橋電機(jī)驅(qū)動(dòng)模塊驅(qū)動(dòng)。操縱桿用于控制機(jī)器人的速度和方向，兩個(gè)速度傳感器 H206 用于測(cè)量機(jī)器人的速度、距離和角度。然后測(cè)量值顯示在16x2 LCD 模塊中。連接到 LCD 的電位器可用于調(diào)節(jié) LCD 的對(duì)比度，電阻器用于限制流向 LCD 背光的電流。

　　整個(gè)電路由一個(gè) 7.4V 鋰電池供電。這個(gè) 7.4V 提供給電機(jī)驅(qū)動(dòng)模塊的 12V 引腳。然后，電機(jī)驅(qū)動(dòng)模塊上的穩(wěn)壓器將 7.4V 轉(zhuǎn)換為穩(wěn)定的 +5V，用于為 Arduino、LCD、傳感器和操縱桿供電。

　　電機(jī)由 Arduino 的數(shù)字引腳 8、9、10 和 11 控制。由于還必須控制電機(jī)的速度，我們應(yīng)該向電機(jī)的正極端子提供PWM 信號(hào)。因此，我們有引腳 9 和 10，它們都是支持 PWM 的引腳。操縱桿的 X 和 Y 值分別使用模擬引腳 A2 和 A3 讀取。

　　正如我們所知，H206 傳感器在檢測(cè)到網(wǎng)格板中的間隙時(shí)會(huì)生成觸發(fā)器。由于不應(yīng)始終準(zhǔn)確讀取這些觸發(fā)器以計(jì)算正確的速度和距離，因此兩個(gè)觸發(fā)器（輸出）引腳都連接到 Arduino 板的外部中斷引腳 2 和 3。將整個(gè)電路組裝在底盤上并按照說(shuō)明安裝速度傳感器，連接完成后，我的機(jī)器人如下所示。您還可以觀看本頁(yè)末尾的視頻，了解傳感器的安裝方式。

　　現(xiàn)在硬件部分已經(jīng)完成，讓我們進(jìn)入如何測(cè)量機(jī)器人的速度、距離和單程的邏輯，然后進(jìn)入編程部分。

　　使用 LM393 速度傳感器模塊測(cè)量速度背后的邏輯

　　從傳感器安裝設(shè)置中，您應(yīng)該知道 LM393 速度傳感器模塊 （H206） 僅測(cè)量網(wǎng)格板中存在的間隙。安裝時(shí)應(yīng)確保車輪（應(yīng)測(cè)量其速度）和網(wǎng)格板以相同的速度旋轉(zhuǎn)。就像這里一樣，由于我們將輪子和盤子都安裝在同一軸上，因此它們顯然將以相同的速度旋轉(zhuǎn)。

　　在我們的設(shè)置中，我們?yōu)槊總€(gè)車輪安裝了兩個(gè)傳感器來(lái)測(cè)量機(jī)器人的角度。但是，如果您的目標(biāo)只是測(cè)量速度和距離，我們可以將傳感器安裝在任何一個(gè)車輪上。傳感器的輸出（觸發(fā)信號(hào)）最常連接到微控制器的外部中斷引腳。每次檢測(cè)到網(wǎng)格板中的間隙時(shí)，都會(huì)觸發(fā)中斷，并執(zhí)行 ISR（中斷服務(wù)程序）中的代碼。如果我們能夠計(jì)算出兩個(gè)這樣的觸發(fā)器之間的時(shí)間間隔，我們就可以計(jì)算出車輪的速度。

　　在 Arduino 中，我們可以使用millis（）函數(shù)輕松計(jì)算此時(shí)間間隔。從設(shè)備上電開始，該毫秒函數(shù)將每毫秒遞增 1。因此，當(dāng)?shù)谝粋€(gè)中斷發(fā)生時(shí)，我們可以將 millis（） 的值保存在一個(gè)虛擬變量中（如代碼中的pevtime），然后當(dāng)?shù)诙€(gè)中斷發(fā)生時(shí)，我們可以通過(guò)從millis（）中減去pevtime值來(lái)計(jì)算所花費(fèi)的時(shí)間。

　　所用時(shí)間 = 當(dāng)前時(shí)間 - 上一個(gè)時(shí)間所

　　用時(shí)間= millis （）- pevtime ; //以毫秒為單位的時(shí)間

　　一旦我們計(jì)算出所花費(fèi)的時(shí)間，我們可以使用以下公式簡(jiǎn)單地計(jì)算 rpm 的值，其中 （1000/timetaken） 給出 RPS（每秒轉(zhuǎn)數(shù)），然后乘以 60 將 RPS 轉(zhuǎn)換為 RPM（每分鐘轉(zhuǎn)數(shù)） 。

　　rpm=（1000/timetaken）*60；

　　在計(jì)算 rpm 后，我們可以使用以下公式計(jì)算車輛的速度，前提是我們知道車輪的半徑。

　　速度 = 2π × RPS × 車輪半徑。

　　v = radius_of_wheel * rpm * 0.104

　　注意，上面的公式是以 m/s 為單位計(jì)算速度的，如果你想以 km/hr 為單位計(jì)算，請(qǐng)將 0.0104 替換為 0.376。如果您想知道值 0.104 是如何獲得的，請(qǐng)嘗試簡(jiǎn)化公式 V = 2π × RPS × 車輪半徑。

　　即使使用霍爾傳感器來(lái)測(cè)量旋轉(zhuǎn)物體的速度，也使用相同的技術(shù)。但是對(duì)于 H206 傳感器有一個(gè)問(wèn)題，網(wǎng)格板有 20 個(gè)插槽，因此測(cè)量?jī)蓚€(gè)插槽間隙之間的時(shí)間會(huì)使微控制器過(guò)載。因此，我們僅在車輪完全旋轉(zhuǎn)時(shí)測(cè)量速度。由于每個(gè)間隙都會(huì)產(chǎn)生兩個(gè)中斷（一個(gè)在間隙開始，另一個(gè)在間隙結(jié)束），我們將總共獲得 40 個(gè)中斷，以便輪子完成一個(gè)完整的旋轉(zhuǎn)。因此，我們?cè)趯?shí)際計(jì)算車輪速度之前等待 40 次中斷。相同的代碼如下所示

　　if（rotation》=40）

　　{

　　timetaken = millis（）-pevtime; //以毫秒為單位的時(shí)間

　　rpm=（1000/timetaken）*60; //計(jì)算轉(zhuǎn)速的公式

　　pevtime = millis（）;

　　旋轉(zhuǎn)=0；

　　}

　　這種方法的另一個(gè)缺點(diǎn)是，速度值不會(huì)下降到零，因?yàn)橹袛鄬⑹冀K等待車輪完成一個(gè)旋轉(zhuǎn)來(lái)計(jì)算 rpm 值。這個(gè)缺點(diǎn)可以通過(guò)添加一個(gè)簡(jiǎn)單的代碼來(lái)監(jiān)控兩個(gè)中斷之間的時(shí)間間隔來(lái)輕松克服，如果它超過(guò)正常值，那么我們可以強(qiáng)制 rpm 和速度的值為零。鏈接在下面的代碼中，我們使用變量dtime來(lái)檢查時(shí)間差異，如果超過(guò) 500 毫秒，則速度和 rpm 的值被強(qiáng)制為零。

　　/*如果車輛停止則降為零*/

　　if（millis（）-dtime》500） //500ms 內(nèi)沒(méi)有發(fā)現(xiàn) inetrrupt

　　{

　　rpm= v = 0; // 使 rpm 和速度為零

　　dtime=millis（）;

　　}

　　測(cè)量車輪行駛距離背后的邏輯

　　我們已經(jīng)知道，當(dāng)車輪轉(zhuǎn)一圈時(shí)，Arduino 會(huì)感應(yīng)到 40 次中斷。因此，對(duì)于輪子每轉(zhuǎn)一圈，輪子行進(jìn)的距離顯然等于輪子的周長(zhǎng)。由于我們已經(jīng)知道車輪的半徑，我們可以使用以下公式輕松計(jì)算所覆蓋的距離

　　距離 = 2πr * 轉(zhuǎn)數(shù)

　　距離 = （2*3.141*radius_of_wheel） * （left_intr/40）

　　使用公式 2πr 計(jì)算車輪的周長(zhǎng)，然后乘以車輪的轉(zhuǎn)數(shù)。

　　測(cè)量機(jī)器人角度背后的邏輯

　　確定機(jī)器人角度的方法有很多種。加速度計(jì)和陀螺儀通常用于確定這些值。但另一種便宜的方法是在兩個(gè)車輪上都使用 H206 傳感器。這樣我們就可以知道每個(gè)輪子轉(zhuǎn)了多少圈。下圖說(shuō)明了如何計(jì)算角度。

　　當(dāng)機(jī)器人初始化時(shí)，它所面對(duì)的角度被認(rèn)為是 0°。從那里它向左旋轉(zhuǎn)，角度增加為負(fù)，如果它向右旋轉(zhuǎn)，角度增加為正。為了便于理解，讓我們考慮如圖所示的 -90 到 +90 的范圍。在這樣的布置中，由于兩個(gè)輪子具有相同的直徑，如果任何一個(gè)輪子完全旋轉(zhuǎn)，我們就會(huì)以 90° 的角度轉(zhuǎn)動(dòng)機(jī)器人。

　　例如，如果左輪完成一整圈（80 次中斷），那么機(jī)器人將向左轉(zhuǎn) 90°，類似地，如果右輪完成一整圈（80 次中斷），那么機(jī)器人將向右轉(zhuǎn) -90°?，F(xiàn)在我們知道，如果 Arduino 在一個(gè)輪子上檢測(cè)到 80 個(gè)中斷，那么機(jī)器人已經(jīng)轉(zhuǎn)動(dòng)了 90°，我們可以根據(jù)哪個(gè)輪子判斷機(jī)器人是正轉(zhuǎn)（右）還是負(fù)（左）轉(zhuǎn)。所以左右角可以用下面的公式計(jì)算

　　int angle_left = （left_intr % 360） * （90/80） ;

　　int angle_right = （right_intr % 360） * （90/80） ;

　　其中 90 是進(jìn)行 80 度中斷時(shí)所覆蓋的角度。結(jié)果值是中斷次數(shù)的乘積。我們還使用了 360 的模數(shù)，因此結(jié)果值永遠(yuǎn)不會(huì)超過(guò) 36。一旦我們計(jì)算了左角和右角，機(jī)器人面對(duì)的有效角度可以簡(jiǎn)單地通過(guò)從右角中減去左角來(lái)獲得。

　　角度 = 角度右 - 角度左；

Arduino機(jī)器人代碼
此速度和角度測(cè)量機(jī)器人的完整 Arduino 代碼可在本頁(yè)末尾找到。該程序的目的是使用上述邏輯計(jì)算機(jī)器人的速度、距離和角度，并將其顯示在 LCD 屏幕上。除此之外，它應(yīng)該提供一個(gè)使用操縱桿控制機(jī)器人的選項(xiàng)。

我們通過(guò)定義兩個(gè)電機(jī)的數(shù)字 I/O 引腳來(lái)啟動(dòng)程序。請(qǐng)注意，我們還必須控制電機(jī)的速度，因此我們必須使用 Arduino 上的 PWM 引腳來(lái)控制電機(jī)。在這里，我們使用了引腳 8、9、10 和 11。

#define LM_pos 9 // 左電機(jī)
#define LM_neg 8 // 左電機(jī)
#define RM_pos 10 // 右電機(jī)
#define RM_neg 11 // 右電機(jī)
#define joyX A2
#define joyY A3
要測(cè)量所覆蓋的速度和距離，我們需要知道車輪的半徑，測(cè)量值并以米為單位輸入，如下所示。對(duì)于我的機(jī)器人，半徑為 0.033 米，但根據(jù)你的機(jī)器人，它可能會(huì)有所不同。

浮動(dòng)radius_of_wheel = 0.033；//測(cè)量你的輪子的半徑并在這里輸入厘米
在setup函數(shù)中，我們將所有值初始化為零，然后在 LCD 上顯示 Intro Text。我們還初始化了串行監(jiān)視器以進(jìn)行調(diào)試。然后我們提到速度傳感器 H206 連接到引腳 2 和 3 作為外部中斷。這就是檢測(cè)到中斷的地方，ISR 函數(shù)Left_ISR和Right_ISR將相應(yīng)地執(zhí)行。

void setup()
{
旋轉(zhuǎn) = rpm = pevtime = 0; //將所有變量初始化為零
Serial.begin(9600);
lcd.begin(16, 2); //初始化 16*2 LCD
lcd.print("Bot Monitor"); //介紹消息行 1
lcd.setCursor(0, 1);
lcd.print("-CircuitDigest"); //介紹消息第 2 行
延遲（2000）；
lcd.clear();
lcd.print("Lt: Rt:");
lcd.setCursor(0, 1);
lcd.print("S:D:A:");

pinMode（LM_pos，輸出）；
pinMode（LM_neg，輸出）；

pinMode（RM_pos，輸出）；
pinMode（RM_neg，輸出）；

數(shù)字寫入（LM_neg，低）；
數(shù)字寫入（RM_neg，低）；

attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); //左輪傳感器觸發(fā)時(shí)
調(diào)用Left_ISR attachInterrupt(digitalPinToInterrupt(3), Right_ISR, CHANGE);//右輪傳感器觸發(fā)時(shí)調(diào)用Right_ISR
}
在 Left_ISR 例程中，我們簡(jiǎn)單地增加一個(gè)名為 left_intr 的變量，稍后將用于測(cè)量機(jī)器人的角度。在 Right_ISR 中，我們做同樣的事情，但另外我們也在此處計(jì)算速度。每次中斷都會(huì)增加變量旋轉(zhuǎn)，然后使用上述邏輯來(lái)計(jì)算速度。

void Left_ISR()
{
left_intr++;delay(10);
}


無(wú)效 Right_ISR()
{
right_intr++; 延遲（10）；

旋轉(zhuǎn)++；
dtime=毫秒（）；
if(rotation>=40)
{
timetaken = millis()-pevtime; //以毫秒為單位的時(shí)間
rpm=(1000/timetaken)*60; //計(jì)算轉(zhuǎn)速的公式
pevtime = millis();
旋轉(zhuǎn)=0；
}
}
在主無(wú)限循環(huán)函數(shù)中，我們通過(guò)操縱桿監(jiān)控 X 和 Y 的值。如果操縱桿移動(dòng)，我們會(huì)根據(jù)該值控制機(jī)器人。機(jī)器人的速度取決于操縱桿被推多遠(yuǎn)。

int xValue = 模擬讀?。╦oyX ）；
int yValue = 模擬讀?。╦oyY ）；

int 加速度 = 地圖 (xValue, 500, 0, 0, 200);

如果（xValue<500）
{
模擬寫入（LM_pos，加速度）；
模擬寫入（RM_pos，加速度）；
}

否則
{
類比寫入（LM_pos，0）；
模擬寫入（RM_pos，0）；
}

如果（yValue>550）
模擬寫入（RM_pos，80）；

如果（yValue<500）
analogWrite（LM_pos，100）；
這將幫助用戶移動(dòng)機(jī)器人并檢查獲得的值是否符合預(yù)期。最后，我們可以使用上述邏輯計(jì)算機(jī)器人的速度、距離和角度，并使用以下代碼將其顯示在 LCD 上。

v = radius_of_wheel * rpm * 0.104；//0.033 是以米為單位的車輪半徑
= (2*3.141*radius_of_wheel) * (left_intr/40);
int angle_left = (left_intr % 360) * (90/80) ;
int angle_right = (right_intr % 360) * (90/80) ;
角度 = 角度右 - 角度左；

lcd.setCursor(3, 0); lcd.print(""); lcd.setCursor(3, 0); lcd.print(left_intr);
lcd.setCursor(11, 0); lcd.print(""); lcd.setCursor(11, 0);lcd.print(right_intr);
lcd.setCursor(2, 1); lcd.print(""); lcd.setCursor(2, 1);lcd.print(v);
lcd.setCursor(9, 1); lcd.print(""); lcd.setCursor(9, 1);lcd.print(距離);
lcd.setCursor(13, 1); lcd.print(""); lcd.setCursor(13, 1);lcd.print(角度);

　　測(cè)試 Arduino 機(jī)器人以測(cè)量距離、速度和角度

　　準(zhǔn)備好硬件后，將代碼上傳到 Arduino 并使用操縱桿移動(dòng)機(jī)器人。機(jī)器人的速度、距離和角度將顯示在 LCD 中，如下所示。

　　在 LCD 上，術(shù)語(yǔ) Lt 和 Rt 分別代表左中斷計(jì)數(shù)和右中斷計(jì)數(shù)。您會(huì)發(fā)現(xiàn)這些值隨著傳感器檢測(cè)到的每個(gè)間隙而增加。項(xiàng)S 表示機(jī)器人的速度，以米/秒為單位，項(xiàng) D 表示以米為單位的距離。機(jī)器人的角度顯示在末端，其中 0° 表示直線，逆時(shí)針旋轉(zhuǎn)為負(fù)，順時(shí)針旋轉(zhuǎn)為正。

/*

* Arduino 車速、距離和角度計(jì)算器

*/


/*-------定義引腳------*/

#define LM_pos 9 // 左電機(jī)

#define LM_neg 8 // 左電機(jī)

#define RM_pos 10 // 右電機(jī)

#define RM_neg 11 // 右電機(jī)

#define joyX A2

#define joyY A3


#include


const int rs = 14, en = 15, d4 = 4, d5 = 5, d6 = 6, d7 = 7; //注明LCD連接的管腳號(hào)

LiquidCrystal lcd(rs, en, d4, d5, d6, d7);


int left_intr = 0;

int right_intr = 0;

int 角度 = 0;


/*硬件細(xì)節(jié)*/

float radius_of_wheel = 0.033; //測(cè)量你的輪子的半徑并在這里輸入厘米


易失字節(jié)輪換；// 中斷樂(lè)趣的變量必須是 volatile

float timetaken,rpm,dtime;

浮動(dòng) v;

整數(shù)距離；

無(wú)符號(hào)長(zhǎng) pevtime；


void setup()

{

旋轉(zhuǎn) = rpm = pevtime = 0; //將所有變量初始化為零

Serial.begin(9600);

lcd.begin(16, 2); //初始化16*2 LCD

lcd.print("Vechile Monitor"); //介紹消息行 1

lcd.setCursor(0, 1);

lcd.print("-CircuitDigest"); //介紹消息第 2 行

延遲（2000）；

lcd.clear();

lcd.print("Lt: Rt:");

lcd.setCursor(0, 1);

lcd.print("S:D:A:");


pinMode（LM_pos，輸出）；

pinMode（LM_neg，輸出）；

pinMode（RM_pos，輸出）；

pinMode（RM_neg，輸出）；


數(shù)字寫入（LM_neg，低）；

數(shù)字寫入（RM_neg，低）；



attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); //左輪傳感器觸發(fā)時(shí)

調(diào)用Left_ISR attachInterrupt(digitalPinToInterrupt(3), Right_ISR, CHANGE);//右輪傳感器觸發(fā)時(shí)調(diào)用Right_ISR

}



無(wú)效循環(huán)（）

{

int xValue = 模擬讀?。╦oyX）；

int yValue = 模擬讀?。╦oyY）；



int 加速度 = 地圖 (xValue, 500, 0, 0, 200);


如果（xValue<500）

{

模擬寫入（LM_pos，加速度）；

模擬寫入（RM_pos，加速度）；

}

其他

{

類比寫入（LM_pos，0）；

模擬寫入（RM_pos，0）；

}


如果（yValue>550）

模擬寫入（RM_pos，80）；

如果（yValue<500）

analogWrite（LM_pos，100）；


/*如果車輛停止則降為零*/

if(millis()-dtime>500) //500ms 內(nèi)沒(méi)有發(fā)現(xiàn) inetrrupt

{

rpm= v = 0; // 使 rpm 和速度為零

dtime=millis();

}

v = radius_of_wheel * rpm * 0.104；//0.033 是以米為單位的車輪半徑

= (2*3.141*radius_of_wheel) * (left_intr/40);

int angle_left = (left_intr % 360) * (90/80) ;

int angle_right = (right_intr % 360) * (90/80) ;

角度 = 角度右 - 角度左；



lcd.setCursor(3, 0); lcd.print(""); lcd.setCursor(3, 0); lcd.print(left_intr);

lcd.setCursor(11, 0); lcd.print(""); lcd.setCursor(11, 0);lcd.print(right_intr);

lcd.setCursor(2, 1); lcd.print(""); lcd.setCursor(2, 1);lcd.print(v);

lcd.setCursor(9, 1); lcd.print(""); lcd.setCursor(9, 1);lcd.print(距離);

lcd.setCursor(13, 1); lcd.print(""); lcd.setCursor(13, 1);lcd.print(角度);


延遲（100）；



}


void Left_ISR()

{

left_intr++;delay(10);

}


無(wú)效 Right_ISR()

{

right_intr++; 延遲（10）；



旋轉(zhuǎn)++；

dtime=毫秒（）；

if(rotation>=40)

{

timetaken = millis()-pevtime; //以毫秒為單位的時(shí)間

rpm=(1000/timetaken)*60; //計(jì)算轉(zhuǎn)速的公式

pevtime = millis();

旋轉(zhuǎn)=0；

}

}
?