#include <Servo.h> //servo library
Servo myservo; // create servo object to control servo
int Echo = A4;
int Trig = A5;
#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define carSpeed 150
int rightDistance = 0, leftDistance = 0, middleDistance = 0;
void forward(){
analogWrite(ENA, 150);
analogWrite(ENB, 150);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
Serial.println("Forward");
}
void back() {
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
Serial.println("Back");
}
void left() {
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
//analogWrite(ENA, 300);
//analogWrite(ENB, 300);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
Serial.println("Left");
}
void right() {
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
//analogWrite(ENA, 300);
// analogWrite(ENB, 300);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
Serial.println("Right");
}
void stop() {
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
Serial.println("Stop!");
}
void sright(){
analogWrite(ENA, 250);
analogWrite(ENB, 250);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
Serial.println("go right!");
}
void sleft(){
analogWrite(ENA, 250);
analogWrite(ENB, 250);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
Serial.println("go right!");
}
//Ultrasonic distance measurement Sub function
int Distance_test() {
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(20);
digitalWrite(Trig, LOW);
float Fdistance = pulseIn(Echo, HIGH);
Fdistance= Fdistance / 58;
return (int)Fdistance;
}
void longtracking()
{
for(int i=0;i<125;i++)
{
forward();
delay(5);
if(LT_L)
{
sleft();
while(LT_L);
}
else if(LT_R)
{
sright();
while(LT_R);
}
if(LT_L&<_R&<_M)
{
forward();
delay(40);
break;
}
}
}
void turnleft()
{
}
void longtracking2()
{
for(int i=0;i<130;i++)
{
forward();
delay(5);
if(LT_L)
{
sleft();
while(LT_L);
}
else if(LT_R)
{
sright();
while(LT_R);
}
if(LT_L&<_R&<_M)
{
forward();
delay(40);
break;
}
}
}
void shorttracking()
{
for(int i=0;i<83;i++)
{
forward();
delay(5);
if(LT_L)
{
sleft();
while(LT_L);
}
else if(LT_R)
{
sright();
while(LT_R);
}
if(LT_R&<_L&<_M)
{
forward();
delay(40);
break;
}
}
}
void setup() {
myservo.attach(3); // attach servo on pin 3 to servo object
Serial.begin(9600);
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
stop();
}
int x=2;
int y=0;
int dx=1;
int dy=3;
int fuck=0;
void loop()
{
if(fuck==0)
{
forward();
while(!LT_L||!LT_R||!LT_M)
{
if(LT_R&<_L&<_M)
{
break;
}
}
delay(60);
stop();
fuck++;
}
if(x==1&&y!=3) //中間點
{
delay(100);
middleDistance = Distance_test();
if(middleDistance<25)
{
stop();
delay(300);
right();
delay(100);
while(!LT_R);
stop();
delay(300);
if(y==0)
{
shorttracking();
stop();
delay(300);
left();
delay(100);
while(!LT_L);
stop();
delay(300);
x++;
delay(100);
middleDistance = Distance_test();
if(middleDistance<25)
{
left();
delay(100);
while(!LT_L);
stop();
delay(300);
stop();
delay(200);
shorttracking();
stop();
delay(200);
shorttracking();
stop();
delay(200);
right();
delay(100);
while(!LT_R);
stop();
delay(300);
x--;
x--;
}
/* else
{
stop();
delay(200);
longtracking();
stop();
delay(200);
}*/
}
else
{
shorttracking(); //此處短途迅即
stop();
delay(300);
left();
delay(100);
while(!LT_L);
stop();
delay(300);
x++;
delay(100);
middleDistance = Distance_test();
if(middleDistance<25)
{
left();
delay(100);
while(!LT_L);
stop();
delay(200);
shorttracking();
stop();
delay(200);
shorttracking();
stop();
delay(200);
x--;
x--;
right();
delay(100);
while(!LT_R);
stop();
delay(300);
}
}
}
else
{
stop();
delay(200);
longtracking2();
stop();
delay(300);
y++;
}
}
else if(x==0&&y!=3)
{
delay(100);
middleDistance = Distance_test();
if(middleDistance<25)
{
stop();
delay(300);
right();
delay(100);
while(!LT_R);
stop();
delay(300);
if(y==0)
{
forward();
delay(520);
stop();
delay(300);
left();
delay(100);
while(!LT_L);
stop();
delay(300);
x++;
}
else
{
shorttracking(); //此處短途迅即
stop();
delay(300);
left();
delay(100);
while(!LT_L);
stop();
delay(300);
stop();
delay(300);
x++;
}
}
else
{
stop();
delay(200);
longtracking(); //zhongjianxunji
stop();
delay(300);
y++;
}
}
else if(x==2&&y!=3)
{
delay(300);
middleDistance = Distance_test();
if(middleDistance<25)
{
stop();
delay(300);
if(y==0)
{
left();
delay(100);
while(!LT_L);
stop();
delay(300);
forward();
delay(520);
right();
delay(100);
while(!LT_R);
stop();
delay(300);
x--;
}
else
{
left();
delay(100);
while(!LT_L);
stop();
delay(300);
shorttracking();
right();
delay(100);
while(!LT_R);
stop();
delay(300);
x--;
}
}
else
{
stop();
delay(200);
longtracking();
stop();
delay(300);
y++;
}
}
if(y==3)
{
int d;
d=dx-x;
if(d==0)
{
stop();
delay(300);
forward();
delay(400);
stop();
delay(1000000);
}
else if(d<0)
{
stop();
delay(300);
left();
delay(350);
forward();
delay(600);
stop();
delay(300);
if(d==-2)
{
forward();
delay(600);
stop();
delay(300);
}
right();
delay(350);
forward();
delay(400);
stop();
delay(100000);
}
else if(d>0)
{
stop();
delay(300);
right();
delay(353);
forward();
delay(600);
stop();
delay(300);
if(d==2)
{
forward();
delay(600);
stop();
delay(300);
}
left();
delay(350);
stop();
delay(300);
forward();
delay(400);
stop();
delay(10000000);
}
}
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
