3 Ultraschallsensoren HC-SR04 einbinden-> finde den Fehler nicht
Hallo zusammen,
nach langer Zeit habe ich mal wieder ein Projekt geplant.
Es soll ein Auto Mower werden, vorerst aber erst über Ultras steuerbar.
Jetzt komme ich zu meinem Prob.
Ich kann jeden Ultra für sich alleine ansteuern, aber wenn ich versuche den zweiten einzubinden, passiert nichts mehr oder er fährt nur rückwärts.
ich stehe echt auf dem Schlauch.
Code:
#include <NewPing.h>
int trigPinF = 11;
int echoPinF = 12;
int trigPinR = 32;
int echoPinR = 33;
int enA = 2;
int revright = 3;
int fwdright= 4;
int revleft= 5;
int fwdleft = 6;
int enB = 7;
int c = 0;
void setup() {
//Serial.begin(9600);
pinMode(enA, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(trigPinF, OUTPUT);
pinMode(echoPinF, INPUT);
pinMode(trigPinR, OUTPUT);
pinMode(echoPinR, INPUT);
}
void loop() {
long duration,distanceF,distanceR;
digitalWrite(trigPinF,HIGH);
digitalWrite(trigPinR,HIGH);
delayMicroseconds(1000);
digitalWrite(trigPinF,LOW);
digitalWrite(trigPinR,LOW);
duration=pulseIn(echoPinF,HIGH);
duration=pulseIn(echoPinR,HIGH);
distanceF,distanceR=(duration/2)/29.1;
//Serial.print(distance);
//Serial.println("CM");
delay(10);
if((distanceF>=20&distanceR>=20))
{
moveforward();
}
if(distanceF<20&distanceR>=20)
{
movestop();
delay(400);
movereverse();
delay(3000);
movestop();
}
else if(distanceF>=20&distanceR<20)
{
movestop();
delay(400);
movereverse();
delay(3000);
movestop();
delay(400);
moveleft();
}
}
void moveforward()
{
digitalWrite(4,LOW);
digitalWrite(3,HIGH);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
analogWrite(enA, 160);
analogWrite(enB, 160);//
}
void moveleft()
{
digitalWrite(4,HIGH);
digitalWrite(3,LOW);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
analogWrite(enA, 140);
analogWrite(enB, 180);
}
void moveright()
{
digitalWrite(4,LOW);
digitalWrite(3,HIGH);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
analogWrite(enA, 180);
analogWrite(enB, 140);
}
void movereverse()
{
digitalWrite(4,HIGH);
digitalWrite(3,LOW);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
analogWrite(enA, 120);
analogWrite(enB, 120);
}
void movestop()
{
digitalWrite(4,LOW);
digitalWrite(3,LOW);
digitalWrite(6,LOW);
digitalWrite(5,LOW);
analogWrite(enA, 0);
analogWrite(enB, 0);
}
Vielleicht kann jemand mal drüber schauen und mir sagen wo der Fehler liegt.
Das wäre mir eine große Hilfe.
Danke schön im voraus.
Gruß Danilo
Liste der Anhänge anzeigen (Anzahl: 2)
Danke für die Antworten.
Ich habe den Sketch noch komplett überarbeitet.
Der Ablauf stimmt auch fast, leider nur fast.
Als erstes mal der grobe Aufbau. Ist erst mal nur für das programmieren.
Die Kabel bleiben natürlich nicht so;)
Anhang 34332
Und hier der Sketch:
Code:
int frontEchoPin = 12;
int frontTriggerPin = 11;
int leftEchoPin = 30;
int leftTriggerPin = 31;
int rightEchoPin = 33;
int rightTriggerPin = 32;
int enA = 2;
int motorL1 = 5;
int motorL2 = 6;
int motorR1 = 3;
int motorR2 = 4;
int enB = 7;
volatile float maxFrontDistance = 30.00;
volatile float frontDuration, frontDistanceCm, leftDuration, leftDistanceCm, rightDuration, rightDistanceCm;
volatile float maxLeftDistance, maxRightDistance = 30.00;
void setup() {
// serial
Serial.begin(9600);
// ultrasonic
pinMode(frontTriggerPin, OUTPUT);
pinMode(frontEchoPin, INPUT);
pinMode(leftTriggerPin, OUTPUT);
pinMode(leftEchoPin, INPUT);
pinMode(rightTriggerPin, OUTPUT);
pinMode(rightEchoPin, INPUT);
// motors
pinMode(motorL1, OUTPUT);
pinMode(motorL2, OUTPUT);
pinMode(motorR1, OUTPUT);
pinMode(motorR2, OUTPUT);
}
void loop() {
// front distance check
checkFrontDistance();
if (frontDistanceCm < maxFrontDistance) {
Serial.println("Too close");
checkLeftDistance();
delay(100);
checkRightDistance();
delay(100);
if (leftDistanceCm < rightDistanceCm)
{
moveBackward;
delay(100);
moveRight();
}
else if (leftDistanceCm > rightDistanceCm)
{
moveBackward;
delay(100);
moveLeft();
}
}
else {
Serial.println("OK");
moveForward();
}
// left distance check
checkLeftDistance();
if (leftDistanceCm < maxLeftDistance) {
Serial.println("Left too close");
delay(100);
checkFrontDistance();
delay(100);
checkRightDistance();
delay(100);
if (frontDistanceCm > rightDistanceCm)
{
moveForward();
}
else if (frontDistanceCm < rightDistanceCm)
{
moveBackward;
delay(100);
moveRight();
}
}
// right distance check
checkRightDistance();
if (rightDistanceCm < maxRightDistance) {
Serial.println("Right too close");
delay(100);
checkFrontDistance();
delay(100);
checkLeftDistance();
delay(100);
if (frontDistanceCm > leftDistanceCm)
moveForward();
else if (frontDistanceCm < leftDistanceCm)
{
moveBackward;
delay(100);
moveLeft();
}
}
}
void checkFrontDistance() {
digitalWrite(frontTriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(frontTriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(frontTriggerPin, LOW);
frontDuration = pulseIn(frontEchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos
frontDistanceCm = frontDuration * 10 / 292 / 2; //convertimos a distancia, en cm
Serial.print("Distance: ");
Serial.print(frontDistanceCm);
Serial.println(" cm");
}
void checkLeftDistance() {
digitalWrite(leftTriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(leftTriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(leftTriggerPin, LOW);
leftDuration = pulseIn(leftEchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos
leftDistanceCm = leftDuration * 10 / 292 / 2; //convertimos a distancia, en cm
Serial.print("Left distance: ");
Serial.print(leftDistanceCm);
Serial.println(" cm");
}
void checkRightDistance() {
digitalWrite(rightTriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(rightTriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(rightTriggerPin, LOW);
rightDuration = pulseIn(rightEchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos
rightDistanceCm = rightDuration * 10 / 292 / 2; //convertimos a distancia, en cm
Serial.print("Right distance: ");
Serial.print(rightDistanceCm);
Serial.println(" cm");
}
void moveBackward() {
Serial.println("Backward.");
digitalWrite(motorL1, LOW);
digitalWrite(motorL2, HIGH);
digitalWrite(motorR1, LOW);
digitalWrite(motorR2, HIGH);
analogWrite(enA, 100);
analogWrite(enB, 100);
}
void moveForward() {
Serial.println("Forward.");
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, LOW);
digitalWrite(motorR1, HIGH);
digitalWrite(motorR2, LOW);
analogWrite(enA, 130);
analogWrite(enB, 130);
}
void moveLeft() {
Serial.println("Left.");
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, LOW);
digitalWrite(motorR1, LOW);
digitalWrite(motorR2, HIGH);
analogWrite(enA, 90);
analogWrite(enB, 100);
}
void moveRight() {
Serial.println("Right.");
digitalWrite(motorL1, LOW);
digitalWrite(motorL2, HIGH);
digitalWrite(motorR1, HIGH);
digitalWrite(motorR2, LOW);
analogWrite(enA, 90);
analogWrite(enB, 100);
}
void moveStop()
{
digitalWrite(4,LOW);
digitalWrite(3,LOW);
digitalWrite(6,LOW);
digitalWrite(5,LOW);
analogWrite(enA, 0);
analogWrite(enB, 0);
}
Mein Problem ist jetzt das der "leftDistance" rum Zickt.
Ich finde der Fehler leider nicht, den Ultra habe ich schon getauscht.
Aber es ist keine Veränderung zu sehen.
Anhang 34333
Woran könnte es liegen?
An der Verkabelung?
Könnten die Ultras durch die Motoren gestört werden?
Vielleicht kann mir jemand auf die Sprünge helfen.
Gruß
Danilo
