久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

<delect id="aueky"></delect>

<abbr id="aueky"><source id="aueky"></source></abbr><li id="aueky"></li>

標題: arduino超聲波舵機避障程序 [打印本頁]

作者: liyyii 時間: 2022-4-26 22:06
標題: arduino超聲波舵機避障程序
//#include <Servo.h>
#include <LiquidCrystal.h> //申明1602液晶的函數庫
//LiquidCrystal lcd(12,11,10,9,8,7,6,5,4,3,2); //8數據口模式連線聲明
LiquidCrystal lcd(13,12,7,6,5,4,3); //4數據口模式連線聲明 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 觸發腳(P2.1)

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

int Left_motor=8;    //左電機(IN3) 輸出0  前進輸出1 后退
int Left_motor_pwm=9;    //左電機PWM調速

int Right_motor_pwm=10; // 右電機PWM調速
int Right_motor=11; // 右電機后退(IN1)  輸出0  前進輸出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; //左循跡紅外傳感器狀態
//int SR; //右循跡紅外傳感器狀態
//int SL_2; //左紅外傳感器狀態
//int SR_2; //右紅外傳感器狀態

int servopin=2;//設置舵機驅動腳到數字口2
int myangle;//定義角度變量
int pulsewidth;//定義脈寬變量
int val;

void setup()
{
  Serial.begin(9600);    // 初始化串口
  //初始化電機驅動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);//設定舵機接口為輸出接口
}
//void run(int time)    // 前進
void run()    // 前進
{
  digitalWrite(Right_motor,LOW);  // 右電機前進
  digitalWrite(Right_motor_pwm,HIGH);  // 右電機前進
  analogWrite(Right_motor_pwm,150);//PWM比例0~255調速，左右輪差異略增減

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

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

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

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

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

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

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

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

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

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

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

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

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

float Distance_test() // 量出前方距離
{
  digitalWrite(Trig, LOW); // 給觸發腳低電平2μs
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  // 給觸發腳高電平10μs，這里至少是10μs
  delayMicroseconds(10);
  digitalWrite(Trig, LOW); // 持續給觸發腳低電
  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();       //把光標移回左上角，即從頭開始輸出
lcd.print(" Distance: ");    //顯示
lcd.setCursor(6,2); //把光標定位在第2行，第6列
lcd.print(Distance);    //顯示距離
lcd.print("cm");       //顯示
  }
  else
  {
lcd.home();       //把光標移回左上角，即從頭開始輸出
lcd.print("!!! Out of range");    //顯示
  }
  delay(250);
  lcd.clear();
}

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

void front_detection()
{
  //此處循環次數減少，為了增加小車遇到障礙物的反應速度
  for(int i=0;i<=5;i++) //產生PWM個數，等效延時以保證能轉到響應角度
  {
servopulse(servopin,90);//模擬產生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++) //產生PWM個數，等效延時以保證能轉到響應角度
  {
servopulse(servopin,175);//模擬產生PWM
  }
  Left_Distance = Distance_test();
  //Serial.print("Left_Distance:");    //輸出距離（單位：厘米）
  //Serial.println(Left_Distance);       //顯示距離
}

void right_detection()
{
  for(int i=0;i<=15;i++) //產生PWM個數，等效延時以保證能轉到響應角度
  {
servopulse(servopin,5);//模擬產生PWM
  }
  Right_Distance = Distance_test();
  //Serial.print("Right_Distance:");    //輸出距離（單位：厘米）
  //Serial.println(Right_Distance);       //顯示距離
}
void loop()
{
  keysacn();          //調用按鍵掃描函數
  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 ))//當左右兩側均有障礙物靠得比較近
      spin_left(0.7);//旋轉掉頭
   else if(Left_Distance > Right_Distance)//左邊比右邊空曠
   {
      left(3);//左轉
      brake(1);//剎車，穩定方向
   }
   else//右邊比左邊空曠
   {
      right(3);//右轉
      brake(1);//剎車，穩定方向
   }
}
else
{
   run(); //無障礙物，直行
}
  }
}

歡迎光臨 (http://m.zg4o1577.cn/bbs/)