|
|
本帖最后由 周成瑞 于 2019-12-5 15:01 編輯
#include <Servo.h> //調(diào)用舵機(jī)函數(shù)庫(kù)
#define enable_L 13 //左輪
#define INA1 3
#define INA2 5
#define enable_R A4 //右輪
#define INTB1 6
#define INTB2 9
int myangle;//定義角度變量
int pulsewidth;//定義脈寬變量
int val;
int Front_Distance = 0;//
int Left_Distance = 0;
int Right_Distance = 0;
const int Trig = 7; //超聲波
const int Echo = 8;
int myservo=10;//設(shè)置舵機(jī)驅(qū)動(dòng)腳到數(shù)字口2
void setup()
{
Serial.begin(9600);
/*引腳初始化*/
pinMode(enable_L,OUTPUT);
pinMode(INA1,OUTPUT);
pinMode(INA2,OUTPUT);
pinMode(enable_R,OUTPUT);
pinMode(INTB1,OUTPUT);
pinMode(INTB2,OUTPUT);
pinMode(myservo,OUTPUT);
/*電機(jī)驅(qū)動(dòng)使能*/
digitalWrite(enable_L,HIGH); // 左電機(jī)使能
digitalWrite(enable_R,HIGH); // 右電機(jī)使能
pinMode(Trig, OUTPUT); //超聲波
pinMode(Echo, INPUT);
}
/*===========================小車(chē)基本動(dòng)作===================
小車(chē)運(yùn)動(dòng)函數(shù)
后退Back
前進(jìn)Forward
左轉(zhuǎn)Left
右轉(zhuǎn)Right
剎車(chē)Brake
*/
void Back(int time) //小車(chē)后轉(zhuǎn)
{
analogWrite(INA1,95);
analogWrite(INA2,LOW); //左PWM 調(diào)速
analogWrite(INTB1,100);
analogWrite(INTB2,LOW); //右PWM 調(diào)速
delay(time * 100); //執(zhí)行時(shí)間，可以調(diào)整
}
void Right(int time) //小車(chē)左轉(zhuǎn)(右輪不動(dòng)，左輪前進(jìn))
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 調(diào)速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 調(diào)速
delay(time * 100); //執(zhí)行時(shí)間，可以調(diào)整
}
void Forward() //小車(chē)前進(jìn)
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調(diào)速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 調(diào)速
}
void Left(int time) //小車(chē)右轉(zhuǎn)(左輪不動(dòng)，右輪前進(jìn))
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調(diào)速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 調(diào)速
delay(time * 100); //執(zhí)行時(shí)間，可以調(diào)整
}
void spin_left(int time) //左轉(zhuǎn)(左輪后退，右輪前進(jìn))
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調(diào)速
analogWrite(INTB1,100);
analogWrite(INTB2,HIGH); //右PWM 調(diào)速
delay(time * 100); //執(zhí)行時(shí)間，可以調(diào)整
}
void Brake(int time) //剎車(chē)
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 調(diào)速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 調(diào)速
delay(time * 100); //執(zhí)行時(shí)間，可以調(diào)整
}
//==================超聲波===============
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); // 讀取高電平時(shí)間(單位：微秒)
Fdistance= Fdistance/58; //為什么除以58等于厘米， Y米=（X秒*344）/2
return Fdistance;
}
void servopulse(int myservo,int myangle)/*定義一個(gè)脈沖函數(shù)，用來(lái)模擬方式產(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度的時(shí)候基本就是1.5MS
digitalWrite(myservo,HIGH);//將舵機(jī)接口電平置高 90*11+50=1490uS 就是1.5ms
delayMicroseconds(pulsewidth);//延時(shí)脈寬值的微秒數(shù) 這里調(diào)用的是微秒延時(shí)函數(shù)
digitalWrite(myservo,LOW);//將舵機(jī)接口電平置低
// delay(20-pulsewidth/1000);//延時(shí)周期內(nèi)剩余時(shí)間 這里調(diào)用的是ms延時(shí)函數(shù)
delay(20-(pulsewidth*0.001));//延時(shí)周期內(nèi)剩余時(shí)間 這里調(diào)用的是ms延時(shí)函數(shù)
}
void front_detection()
{
//此處循環(huán)次數(shù)減少，為了增加小車(chē)遇到障礙物的反應(yīng)速度
for(int i=0;i<=5;i++) //產(chǎn)生PWM個(gè)數(shù)，等效延時(shí)以保證能轉(zhuǎn)到響應(yīng)角度
{
servopulse(myservo,90);//模擬產(chǎn)生PWM
}
Front_Distance = Distance_test();
}
void left_detection()
{
for(int i=0;i<=15;i++) //產(chǎn)生PWM個(gè)數(shù)，等效延時(shí)以保證能轉(zhuǎn)到響應(yīng)角度
{
servopulse(myservo,175);//模擬產(chǎn)生PWM
}
Left_Distance = Distance_test();
}
void right_detection()
{
for(int i=0;i<=15;i++) //產(chǎn)生PWM個(gè)數(shù)，等效延時(shí)以保證能轉(zhuǎn)到響應(yīng)角度
{
servopulse(myservo,5);//模擬產(chǎn)生PWM
}
Right_Distance = Distance_test();
}
void loop()
{
while(1)
{
front_detection();//測(cè)量前方距離
if(Front_Distance < 30)//當(dāng)遇到障礙物時(shí)
{
Back(2);//后退減速
Brake(2);//停下來(lái)做測(cè)距
left_detection();//測(cè)量左邊距障礙物距離
right_detection();//測(cè)量右邊距障礙物距離
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);//剎車(chē)，穩(wěn)定方向
}
else//右邊比左邊空曠
{
Right(3);//右轉(zhuǎn)
Brake(1);//剎車(chē)，穩(wěn)定方向
}
}
else
{
Forward(); //無(wú)障礙物，直行
}
}
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
