首页 › Arduino › 超声波避障小车问题求教

xiaocai10000 发表于 2015-11-12 10:11:53

超声波避障小车问题求教

我的小车主要问题就是停不下来，会直接撞上前面的障碍物，感觉超声波没在判断，程序如下，有高手帮我分析分析吗？
#include <Servo.h>
#define L1 7
#define L2 8
#define R1 12
#define R2 13
#define C0 5
#define C1 6
Servo myservo;
int rightDistance;
int leftDistance;
int middleDistance;
const int Trig=2;
const int Echo=3;

void forward()
{
      digitalWrite(L1,HIGH);
      digitalWrite(L2,LOW);
      analogWrite(C0,150);
      digitalWrite(R1,HIGH);
      digitalWrite(R2,LOW);
      analogWrite(C1,150);
}

void back()
{
      digitalWrite(L1,LOW);
      digitalWrite(L2,HIGH);
      analogWrite(C0,150);
      digitalWrite(R1,LOW);
      digitalWrite(R2,HIGH);
      analogWrite(C1,150);
}

void turnLeft()
{
      digitalWrite(L1,LOW);
      digitalWrite(L2,HIGH);
      analogWrite(C0,150);
      digitalWrite(R1,HIGH);
      digitalWrite(R2,LOW);
      analogWrite(C1,150);
}

void turnRight()
{
      digitalWrite(L1,HIGH);
      digitalWrite(L2,LOW);
      analogWrite(C0,150);
      digitalWrite(R1,LOW);
      digitalWrite(R2,HIGH);
      analogWrite(C1,150);
}

void gostop()
{
      digitalWrite(L1,LOW);
      digitalWrite(L2,LOW);
      analogWrite(C0,0);
      digitalWrite(R1,LOW);
      digitalWrite(R2,LOW);
      analogWrite(C1,0);
}

int Distance_test()
{
digitalWrite(Trig,LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
float cm=pulseIn(Echo, HIGH);
cm= cm/58.0;
cm = (int(cm * 100.0)) / 100.0;
return (int)cm;
}

void setup()
{
myservo.attach(9);
Serial.begin(9600);   // 初始化串口
pinMode(Echo,INPUT);
pinMode(Trig,OUTPUT);
pinMode(L1,OUTPUT);
pinMode(L2,OUTPUT);
pinMode(R1,OUTPUT);
pinMode(R2,OUTPUT);
pinMode(C0,OUTPUT);
pinMode(C1,OUTPUT);
gostop();
}
void loop()
{
myservo.write(90);
delay(1000);
middleDistance=Distance_test();
Serial.print(middleDistance);
Serial.println("cm");
delay(1000);
if(middleDistance<=20)
{
gostop();
delay(500);
myservo.write(30);
delay(1000);
rightDistance=Distance_test();
delay(1000);
myservo.write(90);
delay(1000);
myservo.write(150);
delay(1000);
leftDistance=Distance_test();
delay(1000);
myservo.write(90);
delay(1000);
middleDistance=Distance_test();
delay(1000);
if(rightDistance<=10&&leftDistance<=10)
{
    back();
    delay(50);
}
else if(rightDistance>leftDistance)
{
    turnRight();
    delay(50);
}
else if(rightDistance<leftDistance)
{
    turnLeft();
    delay(50);
}
else
{
    forward();
}
}
else
{
forward();
}
}

1415926 发表于 2015-11-12 10:56:14

myservo.write(90);
delay(1000);
middleDistance=Distance_test();
Serial.print(middleDistance);
Serial.println("cm");
delay(1000);
if(middleDistance<=20)


测距前后，你的程序各停了1秒钟，总计超过2秒，距离<20cm才转，是不是2秒已经跑的超过20cm了，
来不及停了？

564764406 发表于 2015-11-12 18:12:24

要停下来啊，不要一直走啊，我做的是三个超声波的避障，就完全没问题啊，你的时间太长，没有必要，而且，最好看着串口显示的数据来调小车，不要一味的找程序。
串口的数据，对比才能发现到底是哪里错了。再去解决问题。:)
加油吧 。

查看完整版本: 超声波避障小车问题求教