arduino超聲波避障小車代碼

#include <Servo.h>
 
#define STOP      0
#define FORWARD   1
#define BACKWARD  2
#define TURNLEFT  3
#define TURNRIGHT 4
 
int leftMotor1 = 10;
int leftMotor2 = 11;
int rightMotor1 = 12;
int rightMotor2 = 13;
 
int leftPWM = 5;
int rightPWM = 6;
 
Servo myServo;  //舵機
 
int inputPin=7;   // 定義超聲波信號接收接口
int outputPin=8;  // 定義超聲波信號發出接口
 
void setup() {
  // put your setup code here, to run once:
  //串口初始化
  Serial.begin(9600);
  //舵機引腳初始化
  myServo.attach(9);
  //測速引腳初始化
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(leftPWM, OUTPUT);
  pinMode(rightPWM, OUTPUT);
  //超聲波控制引腳初始化
  pinMode(inputPin, INPUT);
  pinMode(outputPin, OUTPUT);
}
 
void loop() {
  // put your main code here, to run repeatedly:
  avoidance();
}
void motorRun(int cmd,int value)
{
  analogWrite(leftPWM, value);  //設置PWM輸出，即設置速度
  analogWrite(rightPWM, value);
  switch(cmd){
    case FORWARD:
      Serial.println("FORWARD"); //輸出狀態
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     case BACKWARD:
      Serial.println("BACKWARD"); //輸出狀態
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNLEFT:
      Serial.println("TURN  LEFT"); //輸出狀態
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNRIGHT:
      Serial.println("TURN  RIGHT"); //輸出狀態
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     default:
      Serial.println("STOP"); //輸出狀態
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
  }
}
void avoidance()
{
  int pos;
  int dis[3];//距離
  motorRun(FORWARD,200);
  myServo.write(90);
  dis[1]=getDistance(); //中間
 
  if(dis[1]<30)
  {
    motorRun(STOP,0);
    for (pos = 90; pos <= 150; pos += 1)
    {
      myServo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    dis[2]=getDistance(); //左邊
    for (pos = 150; pos >= 30; pos -= 1)
    {
      myServo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
      if(pos==90)
        dis[1]=getDistance(); //中間
    }
    dis[0]=getDistance();  //右邊
    for (pos = 30; pos <= 90; pos += 1)
    {
      myServo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    if(dis[0]<dis[2]) //右邊距離障礙的距離比左邊近
    {
      //左轉
      motorRun(TURNLEFT,250);
      delay(500);
    }
    else  //右邊距離障礙的距離比左邊遠
    {
      //右轉
      motorRun(TURNRIGHT,250);
      delay(500);
    }
  }
}
int getDistance()
{
  digitalWrite(outputPin, LOW); // 使發出發出超聲波信號接口低電平2μs
  delayMicroseconds(2);
  digitalWrite(outputPin, HIGH); // 使發出發出超聲波信號接口高電平10μs，這裏是至少10μs
  delayMicroseconds(10);
  digitalWrite(outputPin, LOW); // 保持發出超聲波信號接口低電平
  int distance = pulseIn(inputPin, HIGH); // 讀出脈衝時間
  distance= distance/58; // 將脈衝時間轉化爲距離（單位：釐米）
  Serial.println(distance); //輸出距離值
 
  if (distance >=50)
  {
    //如果距離小於50釐米返回數據
    return 50;
  }//如果距離小於50釐米小燈熄滅
  else
    return distance;
}