|
|
本帖最后由 周成瑞 于 2019-12-5 15:01 編輯
#include <Servo.h> //調用舵機函數(shù)庫
#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ù)字口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);
/*電機驅動使能*/
digitalWrite(enable_L,HIGH); // 左電機使能
digitalWrite(enable_R,HIGH); // 右電機使能
pinMode(Trig, OUTPUT); //超聲波
pinMode(Echo, INPUT);
}
/*===========================小車基本動作===================
小車運動函數(shù)
后退Back
前進Forward
左轉Left
右轉Right
剎車Brake
*/
void Back(int time) //小車后轉
{
analogWrite(INA1,95);
analogWrite(INA2,LOW); //左PWM 調速
analogWrite(INTB1,100);
analogWrite(INTB2,LOW); //右PWM 調速
delay(time * 100); //執(zhí)行時間，可以調整
}
void Right(int time) //小車左轉(右輪不動，左輪前進)
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 調速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 調速
delay(time * 100); //執(zhí)行時間，可以調整
}
void Forward() //小車前進
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 調速
}
void Left(int time) //小車右轉(左輪不動，右輪前進)
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 調速
delay(time * 100); //執(zhí)行時間，可以調整
}
void spin_left(int time) //左轉(左輪后退，右輪前進)
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 調速
analogWrite(INTB1,100);
analogWrite(INTB2,HIGH); //右PWM 調速
delay(time * 100); //執(zhí)行時間，可以調整
}
void Brake(int time) //剎車
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 調速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 調速
delay(time * 100); //執(zhí)行時間，可以調整
}
//==================超聲波===============
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
return Fdistance;
}
void servopulse(int myservo,int myangle)/*定義一個脈沖函數(shù)，用來模擬方式產生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(myservo,HIGH);//將舵機接口電平置高 90*11+50=1490uS 就是1.5ms
delayMicroseconds(pulsewidth);//延時脈寬值的微秒數(shù) 這里調用的是微秒延時函數(shù)
digitalWrite(myservo,LOW);//將舵機接口電平置低
// delay(20-pulsewidth/1000);//延時周期內剩余時間 這里調用的是ms延時函數(shù)
delay(20-(pulsewidth*0.001));//延時周期內剩余時間 這里調用的是ms延時函數(shù)
}
void front_detection()
{
//此處循環(huán)次數(shù)減少，為了增加小車遇到障礙物的反應速度
for(int i=0;i<=5;i++) //產生PWM個數(shù)，等效延時以保證能轉到響應角度
{
servopulse(myservo,90);//模擬產生PWM
}
Front_Distance = Distance_test();
}
void left_detection()
{
for(int i=0;i<=15;i++) //產生PWM個數(shù)，等效延時以保證能轉到響應角度
{
servopulse(myservo,175);//模擬產生PWM
}
Left_Distance = Distance_test();
}
void right_detection()
{
for(int i=0;i<=15;i++) //產生PWM個數(shù)，等效延時以保證能轉到響應角度
{
servopulse(myservo,5);//模擬產生PWM
}
Right_Distance = Distance_test();
}
void loop()
{
while(1)
{
front_detection();//測量前方距離
if(Front_Distance < 30)//當遇到障礙物時
{
Back(2);//后退減速
Brake(2);//停下來做測距
left_detection();//測量左邊距障礙物距離
right_detection();//測量右邊距障礙物距離
if((Left_Distance < 30 ) &&( Right_Distance < 30 ))//當左右兩側均有障礙物靠得比較近
spin_left(0.7);//旋轉掉頭
else if(Left_Distance > Right_Distance)//左邊比右邊空曠
{
Left(3);//左轉
Brake(1);//剎車，穩(wěn)定方向
}
else//右邊比左邊空曠
{
Right(3);//右轉
Brake(1);//剎車，穩(wěn)定方向
}
}
else
{
Forward(); //無障礙物，直行
}
}
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
