Arduino Roboter - Servo reagiert nicht korrekt
Hallo,
momentan bin ich dabei mit einem Arduino Uno, eimem Motorshield, einem Servo, einem Ultraschall-Sensor und einer RP5-Chassis einen Roboter zu konstruieren.
Wenn er mein Sketch durchläuft und aur ein 10cm oder weniger entferntes Hindernis trifft, soll er stehen bleiben, den Sensor um 45° nach links und dann nach rechts, ebenfalls um 45° drehen. Dabei soll überprüft werden, in welcher Richtung kein Hindernis in 10cm auftaucht. In die Richtung drehen und weiterfahren. Also eigentlich "Standard". Der Sensor wird nach links gedreht, aber nicht nach rechts. Anbei mein Sketch, ich sehe den Fehler nicht.
Code:
#include <Servo.h>
#define echoPin 7 // Echo Pin
#define trigPin 6 // Trigger Pin
Servo myservo;
int richtungA = 12; // Motor A
int richtungB = 13; // Motor B
int geschwindigkeitA = 3; // Geschwindigkeit Motor A
int geschwindigkeitB = 11; // Geschwindigkeit Motor B
int bremseA = 9; // Bremse Motor A
int bremseB = 8; // Bremse Motor B
int geschwindigkeit = 255 ; // Geschwindigkeit der Motoren. 255 ist der Maximalwert!
int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long a, b, duration, distance; // Duration used to calculate distance
void kopflinks(){
//Kopf links
myservo.write(125);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
b = duration/58.2;
}
void kopfrechts(){
//Kopf rechts
myservo.write(35);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
a = duration/58.2;
}
void setup()
{
myservo.attach(5);
// Pins für Motorenrichtungen als Ausgang
pinMode(richtungA, OUTPUT);
pinMode(richtungB, OUTPUT);
// Pins für Bremsen als Ausgang
pinMode(bremseA, OUTPUT);
pinMode(bremseB, OUTPUT);
// Geschwindigkeiten setzen
analogWrite(geschwindigkeitA, geschwindigkeit);
analogWrite(geschwindigkeitB, geschwindigkeit);
// Bremse an (Motoren aus)
digitalWrite(bremseA, HIGH);
digitalWrite(bremseB, HIGH);
// 2 Sekunden warten nach einschalten
delay(2000);
//US-Sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
//LEDs
pinMode(2, OUTPUT);
pinMode(4, OUTPUT);
//Servo 0-Position (etwas schief, daher 80°)
myservo.write(80);
}
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
//Distanzberechnung des Ultraschall-Signals.
distance = duration/58.2;
//Distanz größer 10cm, fährt vorwärts
if (distance > 10){
digitalWrite(richtungA, HIGH);
digitalWrite(richtungB, HIGH);
digitalWrite(bremseA, LOW);
digitalWrite(bremseB, LOW);
digitalWrite(2, HIGH);
digitalWrite(4, LOW);
}
//Distanz kleiner gleich 10cm, Bremse ein, LED wechsel und Kopf drehen.
else if (distance <=10){
digitalWrite(bremseA, HIGH);
digitalWrite(bremseB, HIGH);
digitalWrite(2, LOW);
digitalWrite(4, HIGH);
delay(1000);
kopflinks();
delay(500);
kopfrechts();
myservo.write(80);
if (a > 10 && b <=10){
digitalWrite(richtungA, HIGH);
digitalWrite(richtungB, LOW);
digitalWrite(bremseA, LOW);
digitalWrite(bremseB, LOW);
delay(500);
digitalWrite(bremseA, HIGH);
digitalWrite(bremseB, HIGH);
digitalWrite(richtungA, HIGH);
digitalWrite(richtungB, HIGH);
digitalWrite(bremseA, LOW);
digitalWrite(bremseB, LOW);
}
else if (a<=10 && b > 10){
digitalWrite(richtungA, LOW);
digitalWrite(richtungB, HIGH);
digitalWrite(bremseA, LOW);
digitalWrite(bremseB, LOW);
delay(500);
digitalWrite(bremseA, HIGH);
digitalWrite(bremseB, HIGH);
digitalWrite(richtungA, HIGH);
digitalWrite(richtungB, HIGH);
digitalWrite(bremseA, LOW);
digitalWrite(bremseB, LOW);
}
}
}
Ohne Servofunktion läuft der Roboter normal.
Liste der Anhänge anzeigen (Anzahl: 1)
Hallo,
erstmal danke ich euch für die Hilfe. Es ist mein erster Roboter den ich momentan am Basteln bin, daher ist mein Wissen natürlich auch nicht besonders groß.
Ich glaube Sisor hatte mir auch mal Links zu den Interrupt-Programmierungen gepostet, als ich nicht klar kam mit meiner Schaltung Alarmanlage/Temeperaturmesser.
Ich möchte erstmal mit den zugegebenermaßen beschränkten Fertigkeiten ein kleines Ziel erreichen. Ich bin auch am überlegen, wie ich es einrichten kann, wenn er denn endlich so läuft, wie ich es möchte, dass er gleichzeitig zwei Abläufe ausführt, nämlich wendet und den Kopf wieder in "0"-Position bringt. Ist das möglich durch Interrupts?
Hier ein Bild meines kleinen:
Anhang 32206
