Arduino自走車程式設計

A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?

接線完成後,首先來處理自走車的馬達程式

利用  digitalWrite(腳位 , HIGH or LOW ) 可以控制高低電位,自走車前進時程式寫法如下:

 //右輪馬達

digitalWrite(In1,LOW);
 digitalWrite(In2,HIGH);
  // 左輪馬達
 digitalWrite(In3,LOW);
 digitalWrite(In4,HIGH);

此外自走車轉彎的方式有幾種方式,例如當右輪往前進,左輪後退時可以達到快速左轉

 //右輪馬達 往前

digitalWrite(In1,LOW);
 digitalWrite(In2,HIGH);
  // 左輪馬達  往後
 digitalWrite(In3,HIGH);
 digitalWrite(In4,LOW);

也可以透過改變左右兩輪行進的速度,控制自走車逐漸改變行進方向,比如自走車在前進狀態時,左輪轉速 小於右輪時,自走車會逐漸左轉彎.

analogWrite(ENA,rightSpeed) ;
analogWrite(ENB,leftSpeed) ;

避障自走車的運作邏輯說明如下,自走車往前進,超音波感測器隨時偵測前方路況,當偵測到前方30cm前有障礙物時,自走車停止,伺服馬達向右轉,偵測右方再向左轉偵測左方.

若是右方障礙物較遠,自走車往右轉,若左方障礙物較遠,自走車往左轉,如果前方,右方,左方30cm內都有障礙物,自走車會先後退,再右轉.

/* motoTest04

* 馬達測試程式 第四版  測試 OK
 * 功能:  前進  後退 右轉彎  左轉彎  停止 避障功能,
 * 前方30cm有障礙物,車子停止,偵測兩方路況,決定左轉彎或是右轉彎
 * *
 * 改進:轉彎弧度與馬力輸出設定調整到兩輪能直線前進
 */
#include <Servo.h>

#define In1 7  //右馬達
#define In2 8
#define ENA 6
#define In3 12  //左馬達
#define In4 13
#define ENB 11
#define trigPin 4 // 超音波Pin
#define echoPin 5
#define servoPin 9  //  伺服馬達 Pin

unsigned long d    // 前方障礙物距離
const int leftSpeed = 115 ;  //左輪轉速,經過實際測試,若左右輪轉速相同時,自走車無法走直線. P
const int rightSpeed =135;   //右輪轉速

Servo servo ;
void setup() {
  // put your setup code here, to run once:
  //馬達腳位設定
  pinMode(In1,OUTPUT); //右馬達
  pinMode(In2,OUTPUT);
  pinMode(In3,OUTPUT); //左馬達
  pinMode(In4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
 // 超音波腳位設定
  pinMode(trigPin,OUTPUT) ;
  pinMode(echoPin,INPUT) ;
 //超音波初始化
  servo.attach(servoPin) ;
  servo.write(85) ;  //因為組裝的緣故,經測試,伺服馬達在85度時方能在正前方

  delay(1000) ;
  forward();  //自走車開始前進
}

// 超音波距離偵測,傳回 cm
unsigned long ping_d() {
    digitalWrite(trigPin,HIGH) ;
    delayMicroseconds(1000) ;
    digitalWrite(trigPin,LOW);
    return ( pulseIn(echoPin,HIGH)/58) ;
}

// 後退函數   ok
void backward() {
   // 右馬達
    digitalWrite(In1,HIGH);
    digitalWrite(In2,LOW);
    analogWrite(ENA,rightSpeed) ;
    // 左馬達
    digitalWrite(In3,HIGH);
    digitalWrite(In4,LOW);
    analogWrite(ENB,leftSpeed) ;
  }

// 前進函數 OK
 void forward() {
    // 右馬達
    digitalWrite(In1,LOW);
    digitalWrite(In2,HIGH);
    analogWrite(ENA,rightSpeed) ;
    // 左馬達
    digitalWrite(In3,LOW);
    digitalWrite(In4,HIGH);
    analogWrite(ENB,leftSpeed) ;
  }

 // 左轉彎函數  turnLeft ok
 void turnLeft(){
   // 右馬達 前進
    digitalWrite(In1,LOW);
    digitalWrite(In2,HIGH);
    analogWrite(ENA,rightSpeed) ;
    // 左馬達 後退
    digitalWrite(In3,HIGH);
    digitalWrite(In4,LOW);
    analogWrite(ENB,leftSpeed) ;
  }

  // 右轉彎函數  turnRight ok
 void turnRight(){
   // 右馬達 前進
    digitalWrite(In1,HIGH);
    digitalWrite(In2,LOW);
    analogWrite(ENA,rightSpeed) ;
    // 左馬達 後退
    digitalWrite(In3,LOW);
    digitalWrite(In4,HIGH);
    analogWrite(ENB,leftSpeed) ;
  }

  //停止函數 OK
  void motoStop(){
     // 右馬達 前進
    digitalWrite(In1,LOW);
    digitalWrite(In2,LOW);
    analogWrite(ENA,rightSpeed) ;
    // 左馬達 後退
    digitalWrite(In3,LOW);
    digitalWrite(In4,LOW);
    analogWrite(ENB,leftSpeed) ;
   }

  void loop() {
  // put your main code here, to run repeatedly:
 int left_d, right_d ; //紀錄左,右邊障礙物距離

 d = ping_d() ;  //偵測前方障礙物距離

 //  如果前方30cm處有障礙物,自走車需要進入判斷模式,決定行進方式
 if( d<=30) {
    motoStop() ; //自走車停止前進
    servo.write(20) ; // 伺服馬達轉向右邊
    delay(500) ;
    right_d = ping_d() ; // 取得右邊障礙物距離
    delay(20) ;
    servo.write(150) ; // 伺服馬達轉向左邊
    delay(500) ;
    left_d = ping_d() ; // 取得左邊障礙物的距離
    servo.write(85) ;  // 轉動伺服馬達,使超音波回到正前方

    // 如果左邊空間大 且 障礙物距離超過30cm以上 ---> 左轉彎後繼續前進
    if( (left_d>right_d) && (left_d>30)) { //左邊有空間
        turnLeft() ;
        delay(350) ;
        forward() ;
     } else if( (right_d>=left_d) && (right_d>30)) { // 右邊空間大且右邊障礙物距離大於30cm以上 -->右轉彎後前進
        turnRight() ;
         delay(350) ;
         forward() ;
      } else {  // 前,左,右障礙物距離都小於30公分 --->後退->轉彎->前進
         backward() ;
         delay(1500) ;
         turnRight() ;
         delay(350) ;
         forward() ; 
       }   
  } // end if d<=30
   delay(30) ;
}