arduino超聲波舵機(jī)避障程序

ID:1021685 · 發(fā)表于 2022-4-26 22:06

//#include <Servo.h>
#include <LiquidCrystal.h> //申明1602液晶的函數(shù)庫
//LiquidCrystal lcd(12,11,10,9,8,7,6,5,4,3,2); //8數(shù)據(jù)口模式連線聲明
LiquidCrystal lcd(13,12,7,6,5,4,3); //4數(shù)據(jù)口模式連線聲明 P13--LCD 4腳  P12--LCD 5腳
            //P7--LCD 6腳 P6--LCD 11腳  P5--LCD 12腳  P4--LCD 13腳  P3--LCD 14腳

int Echo = A1;  // Echo回聲腳(P2.0)
int Trig =A0;  //  Trig 觸發(fā)腳(P2.1)

int Front_Distance = 0;//
int Left_Distance = 0;
int Right_Distance = 0;

int Left_motor=8;    //左電機(jī)(IN3) 輸出0  前進(jìn) 輸出1 后退
int Left_motor_pwm=9;    //左電機(jī)PWM調(diào)速

int Right_motor_pwm=10; // 右電機(jī)PWM調(diào)速
int Right_motor=11; // 右電機(jī)后退(IN1)  輸出0  前進(jìn) 輸出1 后退

int key=A2;//定義按鍵 A2 接口
int beep=A3;//定義蜂鳴器 A3 接口

//const int SensorRight = 3;          //右循跡紅外傳感器(P3.2 OUT1)
//const int SensorLeft = 4;          //左循跡紅外傳感器(P3.3 OUT2)

//const int SensorRight_2 = 6;          //右紅外傳感器(P3.5 OUT4)
//const int SensorLeft_2 = 5;    //左紅外傳感器(P3.4 OUT3)

//int SL; //左循跡紅外傳感器狀態(tài)
//int SR; //右循跡紅外傳感器狀態(tài)
//int SL_2; //左紅外傳感器狀態(tài)
//int SR_2; //右紅外傳感器狀態(tài)

int servopin=2;//設(shè)置舵機(jī)驅(qū)動腳到數(shù)字口2
int myangle;//定義角度變量
int pulsewidth;//定義脈寬變量
int val;

void setup()
{
  Serial.begin(9600);    // 初始化串口
  //初始化電機(jī)驅(qū)動IO為輸出方式
pinMode(Left_motor,OUTPUT); // PIN 8 8腳無PWM功能
  pinMode(Left_motor_pwm,OUTPUT); // PIN 9 (PWM)
  pinMode(Right_motor_pwm,OUTPUT);// PIN 10 (PWM)
  pinMode(Right_motor,OUTPUT);// PIN 11 (PWM)
  pinMode(key,INPUT);//定義按鍵接口為輸入接口
  pinMode(beep,OUTPUT);
  // pinMode(SensorRight, INPUT); //定義右循跡紅外傳感器為輸入
  // pinMode(SensorLeft, INPUT); //定義左循跡紅外傳感器為輸入
  //pinMode(SensorRight_2, INPUT); //定義右紅外傳感器為輸入
  //pinMode(SensorLeft_2, INPUT); //定義左紅外傳感器為輸入
  //初始化超聲波引腳
  pinMode(Echo, INPUT); // 定義超聲波輸入腳
  pinMode(Trig, OUTPUT); // 定義超聲波輸出腳
  lcd.begin(16,2);    //初始化1602液晶工作                      模式
  //定義1602液晶顯示范圍為2行16列字符
  pinMode(servopin,OUTPUT);//設(shè)定舵機(jī)接口為輸出接口
}
//void run(int time)    // 前進(jìn)
void run()    // 前進(jìn)
{
  digitalWrite(Right_motor,LOW);  // 右電機(jī)前進(jìn)
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機(jī)前進(jìn)
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,LOW);  // 左電機(jī)前進(jìn)
  digitalWrite(Left_motor_pwm,HIGH);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減
  //delay(time * 100); //執(zhí)行時間，可以調(diào)整
}

void brake(int time)  //剎車，停車
{
  digitalWrite(Right_motor_pwm,LOW);  // 右電機(jī)PWM 調(diào)速輸出0
  analogWrite(Right_motor_pwm,0);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor_pwm,LOW);  //左電機(jī)PWM 調(diào)速輸出0
  analogWrite(Left_motor_pwm,0);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100);//執(zhí)行時間，可以調(diào)整
}

void left(int time)       //左轉(zhuǎn)(左輪不動，右輪前進(jìn))
//void left()       //左轉(zhuǎn)(左輪不動，右輪前進(jìn))
{
  digitalWrite(Right_motor,LOW);  // 右電機(jī)前進(jìn)
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機(jī)前進(jìn)
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,LOW);  // 左電機(jī)前進(jìn)
  digitalWrite(Left_motor_pwm,LOW);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,0);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100);       //執(zhí)行時間，可以調(diào)整
}

void spin_left(int time)       //左轉(zhuǎn)(左輪后退，右輪前進(jìn))
{
  digitalWrite(Right_motor,LOW);  // 右電機(jī)前進(jìn)
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機(jī)前進(jìn)
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,HIGH);  // 左電機(jī)后退
  digitalWrite(Left_motor_pwm,HIGH);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100);       //執(zhí)行時間，可以調(diào)整
}

void right(int time)
//void right()       //右轉(zhuǎn)(右輪不動，左輪前進(jìn))
{
  digitalWrite(Right_motor,LOW);  // 右電機(jī)不轉(zhuǎn)
  digitalWrite(Right_motor_pwm,LOW);  // 右電機(jī)PWM輸出0
  analogWrite(Right_motor_pwm,0);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,LOW);  // 左電機(jī)前進(jìn)
  digitalWrite(Left_motor_pwm,HIGH);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100);       //執(zhí)行時間，可以調(diào)整
}

void spin_right(int time)       //右轉(zhuǎn)(右輪后退，左輪前進(jìn))
{
digitalWrite(Right_motor,HIGH);  // 右電機(jī)后退
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機(jī)PWM輸出1
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,LOW);  // 左電機(jī)前進(jìn)
  digitalWrite(Left_motor_pwm,HIGH);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100);       //執(zhí)行時間，可以調(diào)整
}

void back(int time)       //后退
{
  digitalWrite(Right_motor,HIGH);  // 右電機(jī)后退
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機(jī)前進(jìn)
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減

  digitalWrite(Left_motor,HIGH);  // 左電機(jī)后退
  digitalWrite(Left_motor_pwm,HIGH);  //左電機(jī)PWM
  analogWrite(Left_motor_pwm,150);//PWM比例0~255調(diào)速，左右輪差異略增減
  delay(time * 100); //執(zhí)行時間，可以調(diào)整
}
void keysacn()//按鍵掃描
{
  int val;
  val=digitalRead(key);//讀取數(shù)字7 口電平值賦給val
  while(!digitalRead(key))//當(dāng)按鍵沒被按下時，一直循環(huán)
  {
val=digitalRead(key);//此句可省略，可讓循環(huán)跑空
  }
  while(digitalRead(key))//當(dāng)按鍵被按下時
  {
delay(10);       //延時10ms
val=digitalRead(key);//讀取數(shù)字7 口電平值賦給val
if(val==HIGH)  //第二次判斷按鍵是否被按下
{
   digitalWrite(beep,HIGH);             //蜂鳴器響
   while(!digitalRead(key))       //判斷按鍵是否被松開
      digitalWrite(beep,LOW);             //蜂鳴器停止
}
else
   digitalWrite(beep,LOW);       //蜂鳴器停止
  }
}

float Distance_test() // 量出前方距離
{
  digitalWrite(Trig, LOW); // 給觸發(fā)腳低電平2μs
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  // 給觸發(fā)腳高電平10μs，這里至少是10μs
  delayMicroseconds(10);
  digitalWrite(Trig, LOW); // 持續(xù)給觸發(fā)腳低電
  float Fdistance = pulseIn(Echo, HIGH);  // 讀取高電平時間(單位：微秒)
  Fdistance= Fdistance/58;    //為什么除以58等于厘米，  Y米=（X秒*344）/2
  // X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
  //Serial.print("Distance:");    //輸出距離（單位：厘米）
  //Serial.println(Fdistance);       //顯示距離
  //Distance = Fdistance;
  return Fdistance;
}

void Distance_display(int Distance)//顯示距離
{
  if((2<Distance)&(Distance<400))
  {
lcd.home();       //把光標(biāo)移回左上角，即從頭開始輸出
lcd.print(" Distance: ");    //顯示
lcd.setCursor(6,2); //把光標(biāo)定位在第2行，第6列
lcd.print(Distance);    //顯示距離
lcd.print("cm");       //顯示
  }
  else
  {
lcd.home();       //把光標(biāo)移回左上角，即從頭開始輸出
lcd.print("!!! Out of range");    //顯示
  }
  delay(250);
  lcd.clear();
}

void servopulse(int servopin,int myangle)/*定義一個脈沖函數(shù)，用來模擬方式產(chǎn)生PWM值舵機(jī)的范圍是0.5MS到2.5MS 1.5MS 占空比是居中周期是20MS*/
{
  pulsewidth=(myangle*11)+500;//將角度轉(zhuǎn)化為500-2480 的脈寬值這里的myangle就是0-180度  所以180*11+50=2480  11是為了換成90度的時候基本就是1.5MS
  digitalWrite(servopin,HIGH);//將舵機(jī)接口電平置高                                     90*11+50=1490uS  就是1.5ms
  delayMicroseconds(pulsewidth);//延時脈寬值的微秒數(shù)  這里調(diào)用的是微秒延時函數(shù)
  digitalWrite(servopin,LOW);//將舵機(jī)接口電平置低
// delay(20-pulsewidth/1000);//延時周期內(nèi)剩余時間  這里調(diào)用的是ms延時函數(shù)
  delay(20-(pulsewidth*0.001));//延時周期內(nèi)剩余時間  這里調(diào)用的是ms延時函數(shù)
}

void front_detection()
{
  //此處循環(huán)次數(shù)減少，為了增加小車遇到障礙物的反應(yīng)速度
  for(int i=0;i<=5;i++) //產(chǎn)生PWM個數(shù)，等效延時以保證能轉(zhuǎn)到響應(yīng)角度
  {
servopulse(servopin,90);//模擬產(chǎn)生PWM
  }
  Front_Distance = Distance_test();
  //Serial.print("Front_Distance:");    //輸出距離（單位：厘米）
// Serial.println(Front_Distance);       //顯示距離
//Distance_display(Front_Distance);
}

void left_detection()
{
  for(int i=0;i<=15;i++) //產(chǎn)生PWM個數(shù)，等效延時以保證能轉(zhuǎn)到響應(yīng)角度
  {
servopulse(servopin,175);//模擬產(chǎn)生PWM
  }
  Left_Distance = Distance_test();
  //Serial.print("Left_Distance:");    //輸出距離（單位：厘米）
  //Serial.println(Left_Distance);       //顯示距離
}

void right_detection()
{
  for(int i=0;i<=15;i++) //產(chǎn)生PWM個數(shù)，等效延時以保證能轉(zhuǎn)到響應(yīng)角度
  {
servopulse(servopin,5);//模擬產(chǎn)生PWM
  }
  Right_Distance = Distance_test();
  //Serial.print("Right_Distance:");    //輸出距離（單位：厘米）
  //Serial.println(Right_Distance);       //顯示距離
}
void loop()
{
  keysacn();          //調(diào)用按鍵掃描函數(shù)
  while(1)
  {
front_detection();//測量前方距離
{
   brake(2);//先剎車
   back(2);//后退減速
   brake(2);//停下來做測距
   left_detection();//測量左邊距障礙物距離
   Distance_display(Left_Distance);//液晶屏顯示距離
   right_detection();//測量右邊距障礙物距離
   Distance_display(Right_Distance);//液晶屏顯示距離
   if((Left_Distance < 30 ) &&( Right_Distance < 30 ))//當(dāng)左右兩側(cè)均有障礙物靠得比較近
      spin_left(0.7);//旋轉(zhuǎn)掉頭
   else if(Left_Distance > Right_Distance)//左邊比右邊空曠
   {
      left(3);//左轉(zhuǎn)
      brake(1);//剎車，穩(wěn)定方向
   }
   else//右邊比左邊空曠
   {
      right(3);//右轉(zhuǎn)
      brake(1);//剎車，穩(wěn)定方向
   }
}
else
{
   run(); //無障礙物，直行
}
  }
}

帳號		自動登錄	找回密碼
密碼			立即注冊

arduino超聲波舵機(jī)避障程序

評分