#include <MsTimer2.h>
#include <SoftwareSerial.h>
#include <Servo.h>
Servo myservo;// 定義舵機(jī)對(duì)
int pos = 90;//定義舵機(jī)轉(zhuǎn)動(dòng)位置
SoftwareSerial mySerial(8, 9); // RX, TX
const int TrigPin = 2;
const int EchoPin = 3;
const int L298nIn1 = 7;
const int L298nIn2 = 6;
const int L298nIn3 = 5;
const int L298nIn4 = 4;
float distance,a,b,a1=360.0,b1=360.0;
unsigned char flag=0;
void setup()
{ // 初始化串口通信及連接SR04的引腳
mySerial.begin(9600);
myservo.attach(10); // 設(shè)置舵機(jī)控制針腳
myservo.write(pos);
pinMode(TrigPin, OUTPUT);// 要檢測(cè)引腳上輸入的脈沖寬度，需要先設(shè)置為輸入狀態(tài)
pinMode(EchoPin, INPUT);
pinMode(L298nIn1, OUTPUT);
pinMode(L298nIn2, OUTPUT);
pinMode(L298nIn3, OUTPUT);
pinMode(L298nIn4, OUTPUT);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
goStop();
// mySerial.println("Ultrasonic sensor:");
// MsTimer2::set(500, flash); // 中斷設(shè)置函數(shù)，每 500ms 進(jìn)入一次中斷
// MsTimer2::start(); //開始計(jì)時(shí)
}
void flash() //中斷處理函數(shù)，改變燈的狀態(tài)
{
if(flag==1) {
mySerial.print(distance);
mySerial.print("cm");
mySerial.println("");}
}
void goForward() {//小車前進(jìn)
digitalWrite(L298nIn1, LOW);
analogWrite(L298nIn2, 250);//右輪正轉(zhuǎn)
analogWrite(L298nIn3, 0);
digitalWrite(L298nIn4, HIGH);//左輪直走
}
void goBack() {//小車后退
digitalWrite(L298nIn1, HIGH);
analogWrite(L298nIn2, 0);
analogWrite(L298nIn3, 255);
digitalWrite(L298nIn4, LOW);
}
void goStop() {//小車停止
digitalWrite(L298nIn1, LOW);
digitalWrite(L298nIn2, LOW);
digitalWrite(L298nIn3, LOW);
digitalWrite(L298nIn4, LOW);
}
void turnRight() {//小車右轉(zhuǎn)
digitalWrite(L298nIn1, HIGH);
digitalWrite(L298nIn2, LOW);
digitalWrite(L298nIn3, LOW);
digitalWrite(L298nIn4, HIGH);
}
void turnLeft() {//小車左轉(zhuǎn)
digitalWrite(L298nIn1, LOW);
digitalWrite(L298nIn2, HIGH);
digitalWrite(L298nIn3, HIGH);
digitalWrite(L298nIn4, LOW);
}
void PWM_Right() {//小車右轉(zhuǎn)
digitalWrite(L298nIn1, LOW);
analogWrite(L298nIn2, 170);
digitalWrite(L298nIn3, LOW);
digitalWrite(L298nIn4, HIGH);
}
void PWM_Left() {//小車左轉(zhuǎn)
digitalWrite(L298nIn1, LOW);
digitalWrite(L298nIn2, HIGH);
analogWrite(L298nIn3, 75);
digitalWrite(L298nIn4, HIGH);
}
void PWM_back_L(){
digitalWrite(L298nIn1, HIGH);
digitalWrite(L298nIn2, LOW);
analogWrite(L298nIn3, 200);
digitalWrite(L298nIn4, LOW);
}
void PWM_back_R(){
digitalWrite(L298nIn1, HIGH);
analogWrite(L298nIn2, 60);
digitalWrite(L298nIn3, HIGH);
digitalWrite(L298nIn4, LOW);
}
void flag_1(){
if(flag==1)
flag=0;
else
flag=1;
}
void readTtl() {
while (mySerial.available())
{
char s=mySerial.read();
delay(2);
if (s == 'W') goForward();//如果按GO鍵，小車前進(jìn)，直到按STOP鍵停止
else if (s == 'A' ) PWM_Left();
else if (s == 'Q' ) {goStop();flag=0;}
else if (s == 'D' ) PWM_Right();
else if (s == 'S' ) goBack();
else if (s == 'L' ) turnLeft();
else if (s == 'R' ) turnRight();
else if (s == 'Z' ) PWM_back_L();
else if (s == 'Y' ) PWM_back_R();
else if (s == 'F' ) flag_1();
}
}
void Ultr(){
distance=0.0;
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10); // 產(chǎn)生一個(gè)10us的高脈沖去觸發(fā)TrigPin
digitalWrite(TrigPin, LOW);
distance = pulseIn(EchoPin, HIGH) / 58.00; // 檢測(cè)脈沖寬度，并計(jì)算出距離
delay(2);
}
void Duoji(){
for(pos = 90; pos <= 180; pos += 1){
myservo.write(pos);
delay(5);
readTtl();
}
for(pos = 180; pos >= 90; pos-= 1){
myservo.write(pos);
delay(5);
readTtl();
Ultr();
if(pos>120){
if(distance<a1)a1=distance;}
a=a1;
}
for(pos = 90; pos >= 0; pos -= 1){
myservo.write(pos);
delay(5);
readTtl();
}
for(pos = 0; pos <= 90; pos += 1){
myservo.write(pos);
delay(5);
readTtl();
Ultr();
if(pos<60){
if(distance<b1)b1=distance;}
b=b1;
}
}
void loop()
{
readTtl();
if(flag==1)
{
goForward();
Ultr();
if(distance<=20.0)
{
goStop();
Duoji();
if(a>15.0 || b>15.0){
if(a>b){turnLeft(); delay(300);}
else if(a<b){turnRight();delay(300);}
else if(a>20.0&&b>20.0){turnRight();delay(300);}
}
else
{goBack();delay(300);turnLeft();delay(580);}
goForward();
a1=50.0;
b1=50.0;
}
}
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
