Arduino筆記(97)：HC-SR04超音波測距避障小車

剛完成 HC-SR04 超音波測距與 VL53L0X 的測試比較，想運用 HC-SR04 在實際生活中，其中一個應用是作為小車的避障感測功能。如果小車要避開障礙，就需要偵測前方物體距離幾公分時，需停止或轉向右方或左方繼續前進。這個實作是先將車子倒退一點，再轉向左或向右繼續前進，如果遇到障礙繼續往左或往右，才能避開前方的障礙物。

這個小車的主體是在 thingiverse.com 看到的，我用 3D 印表機列印出來，鎖上銅製六角螺絲固定支架，搭配 N20 微型金屬減速馬達，這個 N20 馬達是我在淘X買的二手馬達，沒有說明轉速比，跑起來覺有點慢，但特點是體積小、扭力大。馬達驅動採用 L9110S 模組，藉由這個實作瞭解一下如何使用這個模組來控制馬達的轉向。

[L9110S直流電機驅動模組]

這個模組有 2 個 L9110 IC，可以控制 2 個直流電機或 1 個步進電機。

• 雙L9110S晶片的馬達驅動
• 模組供電電壓：2.5-12V
• 適合的馬達範圍：馬達工作電壓 2.5v - 12V 之間，最大工作電流 0.8A
• 可以同時驅動2個直流馬達，或者1個4線2相式步進馬達
• PCB板尺寸：2.8cm*2.1cm

L9110S 的 4 個引腳可以控制 2 個馬達的轉向，A-1A 和 A-1B 是一組，控制 1 個馬達，B-1A 和 B-1B 是另一組，控制第 2 個馬達。對引腳提供高低的電壓，就可以控制馬達的旋轉方向，可參考下表說明。引腳 VCC則可接外部電源，供電給馬達。

旋轉方向	A-1A 或 B-1A	A-1B或 B-1B
正轉	高電位	低電位
反轉	低電位	高電位
停止	高電位	高電位
停止	低電位	低電位

[材料]

• Arduino Nano x1
• L9110電機驅動模組 x1
• N20 馬達 x4
• 18650充電電池組(USB輸出) x1
• Arduino Nano 固定座 x1 
• 六角銅柱 n 個
• 排線 n 條

[線路連接與電路圖]

Arduino Nano	HC-SR04	L9110
GND	GND	GND
3.3V	VCC
D3	Trig
D4	Echo
D6	-	A-1A
D7	-	A-1B
D8	-	B-1A
D9	-	B-1B
5V	-	VCC

[程式]

以下程式有兩個副程式沒有用到，分別是 motorStop()和 motorLeft()，我仍列在程式中，方便作為其他情境中使用。

const int trigPin = 3;             //Trig Pin
const int echoPin = 4;             //Echo Pin
const int motorA1 =  6;            // A1A
const int motorA2  = 7;            // A1B
const int motorB1 =  8;            // B1A
const int motorB2 =  9;            // B1B

long duration, cm, inches;
int BackwardSpeed = 50;
int ForwardSpeed = 255;

void setup() {
  Serial.begin (9600);             // Serial Port begin
  pinMode(trigPin, OUTPUT);          // 定義輸入和輸出 
  pinMode(echoPin, INPUT);
    
  pinMode( motorA1 , OUTPUT);
  pinMode( motorA2 , OUTPUT);

  pinMode( motorB1 , OUTPUT);
  pinMode( motorB2 , OUTPUT);
}

void loop() {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);     // 給 Trig 高電位，持續 10微秒
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  pinMode(echoPin, INPUT);          // 讀取 echo 的電位
  duration = pulseIn(echoPin, HIGH);   // 收到高電位時的時間
 
  cm = (duration/2) / 29.1;         // 將時間換算成距離 cm 或 inch  
  Serial.println(cm);
  
  if ( cm < 20 ) {
      // 先倒退
      for(int i=0; i<5000 ; i++){
         motorBackward();
      }
      // 再右轉彎
      for(int i=0; i<5000 ; i++){
         motorRight();
      } 
  } else   
  {
      motorForward();
  }
}

void motorStop(){
  digitalWrite( motorA1 , LOW);
  digitalWrite( motorA2 , LOW);
  digitalWrite( motorB1 , LOW);
  digitalWrite( motorB2 , LOW);
}

// 往後
void motorBackward(){
//  digitalWrite( motorA1 , HIGH);
  analogWrite( motorA1 , BackwardSpeed);
  digitalWrite( motorA2 , LOW);
  digitalWrite( motorB1 , LOW);
  analogWrite( motorB2 , BackwardSpeed);

}

// 往前
void motorForward(){
  digitalWrite( motorA1 , LOW);
  analogWrite( motorA2 , ForwardSpeed);
  analogWrite( motorB1 , ForwardSpeed);
  digitalWrite( motorB2 , LOW);
}

// 右轉
void motorRight(){
  digitalWrite( motorA1 , LOW);
  analogWrite( motorA2 , ForwardSpeed);
  digitalWrite( motorB1 , LOW);
  analogWrite( motorB2 , ForwardSpeed);
}

// 左轉
void motorLeft(){
  digitalWrite( motorA1 , LOW);
  analogWrite( motorA2 , ForwardSpeed);  
  digitalWrite( motorB1 ,LOW);
  analogWrite( motorB2 ,ForwardSpeed);
}

[實作結果]

以下是幾張從不同角度拍攝的小車照片，並提供小車避障的測試結果。

[參考資料]

• L9110 H-bridge module
• https://www.thingiverse.com/thing:2592751