基于arduino2560的扫地机器人源程序 -

int FEcho = 11; // Echo回声脚(P2.0)
int FTrig =10; // Trig 触发脚(P2.1)
int LEcho = 9; // Echo回声脚(P2.0)
int LTrig =8; // Trig 触发脚(P2.1)
int REcho = 13; // Echo回声脚(P2.0)
int RTrig =12; // Trig 触发脚(P2.1)
int in1 = 4;
int in2 = 5;
int in3 = 22;
int in4 = 23;
int speedpin1=6;
int speedpin2=7;
int rightDistance = 0,leftDistance = 0,middleDistance = 0 ;
void forward()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
analogWrite(speedpin1,60);
analogWrite(speedpin2,60);
}
void back()
{
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
analogWrite(speedpin1,60);
analogWrite(speedpin2,60);
}
void turnright()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
analogWrite(speedpin1,70);
analogWrite(speedpin2,0);
delay(200);
}
void turnleft()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
analogWrite(speedpin1,0);
analogWrite(speedpin2,70);
delay(200);
}
void stop()
{
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,LOW);
}
int Fdistance_test() // 量出前方距离
{
digitalWrite(FTrig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(FTrig, HIGH); // 给触发脚高电平10μs，这里至少是10μs
delayMicroseconds(20);
digitalWrite(FTrig, LOW); // 持续给触发脚低电
float Fdistance = pulseIn(FEcho, HIGH); // 读取高电平时间(单位：微秒)
Fdistance= Fdistance/58; //为什么除以58等于厘米， Y米=（X秒*344）/2
// X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
return (int)Fdistance;
}
int Ldistance_test() // 量出left方距离
{
digitalWrite(LTrig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(LTrig, HIGH); // 给触发脚高电平10μs，这里至少是10μs
delayMicroseconds(20);
digitalWrite(LTrig, LOW); // 持续给触发脚低电
float Ldistance = pulseIn(LEcho, HIGH); // 读取高电平时间(单位：微秒)
Ldistance= Ldistance/58; //为什么除以58等于厘米， Y米=（X秒*344）/2
// X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
return (int)Ldistance;
}
int Rdistance_test() // 量出 right方距离
{
digitalWrite(RTrig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(RTrig, HIGH); // 给触发脚高电平10μs，这里至少是10μs
delayMicroseconds(20);
digitalWrite(RTrig, LOW); // 持续给触发脚低电
float Rdistance = pulseIn(REcho, HIGH); // 读取高电平时间(单位：微秒)
Rdistance= Rdistance/58; //为什么除以58等于厘米， Y米=（X秒*344）/2
// X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
return (int)Rdistance;
}
void setup()
{
Serial.begin(9600); // 初始化串口
pinMode(FEcho, INPUT); // 定义超声波输入脚
pinMode(FTrig, OUTPUT); // 定义超声波输出脚
pinMode(LEcho, INPUT); // 定义超声波输入脚
pinMode(LTrig, OUTPUT); // 定义超声波输出脚
pinMode(REcho, INPUT); // 定义超声波输入脚
pinMode(RTrig, OUTPUT); // 定义超声波输出脚
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
stop();
}
void loop()
{
forward();
delay(500);
//middleDistance = Fdistance_test();
//leftDistance = Ldistance_test();
//rightDistance = Rdistance_test();
//Serial.print(rightDistance);
//Serial.print("cm");
//Serial.println();
//delay(1000);
//stop();
//delay(500);
middleDistance = Fdistance_test();
leftDistance = Ldistance_test();
rightDistance = Rdistance_test();
if(middleDistance<=20)
{
if(leftDistance<=20&rightDistance>20)
{
back();
delay(400);
turnright();
delay(1000);
}
else if(rightDistance<=20&leftDistance>20)
{
back();
delay(400);
turnleft();
delay(1000);
}
else if(rightDistance<=20&leftDistance<=20)
{
back();
delay(600);
turnright();
delay(1000);
}
else
{
back();
delay(600);
turnright();
delay(1000);
}
}
else if(rightDistance<=20)
{
if( middleDistance<=20&leftDistance>20)
{
back();
delay(400);
turnleft();
delay(1000);
}
else if(leftDistance<=20&middleDistance>20)
{
back();
delay(600);
turnright();
delay(1000);
}
else if(leftDistance<=20&middleDistance<=20)
{
back();
delay(600);
turnright();
delay(1000);
}
else
{
back();
delay(400);
turnleft();
delay(1000);
}
}
else if(leftDistance<=20)
{
if( middleDistance<=20&rightDistance>20)
{
back();
delay(400);
turnright();
delay(1000);
}
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码