Arduino Roboter - Servo reagiert nicht korrekt

**wendtdir** · 16.11.2016, 20:05

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.

**i_make_it** · 17.11.2016, 08:09

Versuchs mal mit

Code:

  kopflinks();
  delay(500);
  kopfrechts();
  delay(500);

Beim Überfliegen, scheint es mir so als ob Du dem RC-Servo gar keine Zeit lässt nach rechts zu fahren.
Allerdings wird Dir das nichts bringen, Soweit ich das sehe, dürfte Kopflinks nahezu das selbe Messergebniss liefern wie wenn der Kopf gradeaus schaut.
So ein RC-Servo hat ja eine Stellzeit z.B. 0,6s für 60°.
Soweit ich das sehe startest Du die Servobewegung und trigerst danach sofort den Sensor.
Die Messung müsste also erfolgen wärend sich der RC_Servo grade anfängt auf den Weg zur Zielposition zu machen.
Da RC-Servos intern regeln, hat man keine Rückmeldung wann ein Servo seine Zielposition erreicht hat.
Man schätzt entweder die Zeit die der RC_Servo braucht und arbeitet mit einem Delay und hofft das es keine Fehlersituation gibt in der der RC_Servo blokiert, oder man braucht ein Messystem daß einem das Erreichen der Zielposition anzeigt.
Im Zweifelsfall auf jeder Seite einen Taster.
Und wenn man rafiniert ist, spendiert man dem ganzen Betätigungsnocken, die bei Endlage 1 Taster 1 öffnen und bei Endlage 2 Taster 2 öffnen.
in Mittelstellung sind beide Taster dann offen.
Das heist wärend der Drehung sind beide Taster geschlossen.
Strohm fließt also nur wärend der Bewegung (Strom sparen) und man kann kabelbrüche erkennen, da ja nach dem Drehbefehl beide Taster auf 1 gehen müssen und in den Endlagen jeweils 1 Taster auf 1 bleibt.
kommen diese Bitmuster nicht zustande, dann ist irgendwo ein Fehler und der Roboter kann in einen Fehlermodus gehen. (z.B. blinken und piepen)

**wendtdir** · 17.11.2016, 19:22

Danke für den Hinweis mit dem Delay. Das hat ein Teil des Problems gelöst. Der Servo dreht sich nun von nach links, dann kommt der "ping", dann dreht er sich nach rechts "ping". Aber die Werte werden nicht in den Variable a und b gespeichert. Ich habe die Bedingungen angepasst auf if (a<b), dann drehe dich in die eine Richtung oder (a>b), dann in die andere Richtung. Wenn ich mir die Werte für a und b ausgeben lasse, steht immer 0 da, damit ist keine Bedingung erfüllt und der Kopf geht hin und her und hin und her.

**Sisor** · 17.11.2016, 21:06

Hallo wendtdir,
ich habe deinen Code mal ein wenig aufgeräumt (nicht vollständig). Vergleiche mal mit deinem Code.
Ich hoffe es fällt auf, dass der aufgeräumte Code deutlich lesbarer ist. Wenn es um Fehlersuche geht, ist Lesbarkeit ein sehr wichtiger Faktor. Da fallen dann Dinge ins Auge, die vorher im "Gewusel" untergegangen sind.

Code:

#include <Servo.h>

#define echoPin    7  
#define trigPin    6  
#define motorAPin 12
#define motorBPin 13
#define vAPin      3
#define vBPin     11
#define bremseAPin 9
#define bremseBPin 8
#define led1Pin    2
#define led2Pin    4
#define servoPin   5

Servo myservo;

void kopf_rechts(){
  myservo.write(35);
}

void kopf_links(){
  myservo.write(125);
}

void kopf_mitte(){
  //Servo 0-Position (etwas schief, daher 80°)
  myservo.write(80);
}

void geradeaus_fahren() {
  digitalWrite(motorAPin, HIGH);
  digitalWrite(motorBPin, HIGH);
  digitalWrite(bremseAPin, LOW);
  digitalWrite(bremseBPin, LOW);
  digitalWrite(led1Pin, HIGH);
  digitalWrite(led2Pin, LOW);
}

unsigned long ping_cm() {
  // Ultraschall-Messung
  // gibt Distanz in cm zurück
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  // maximale Pulslänge bei 3m Distanz,  v_Schall = 340m/s
  const unsigned long maxPulse_us = (3 * 2 * 1000000) / 340;  
  unsigned long duration = pulseIn(echoPin, HIGH, maxPulse_us);
  if (duration == 0) return 300;  //kein Echo -> Rückgabe max.Messweite
  return duration / 58.2;
}

void setup(){
  myservo.attach(servoPin);
  kopf_mitte();
 
  // Pins für Motorenrichtungen als Ausgang
  pinMode(motorAPin, OUTPUT);
  pinMode(motorBPin, OUTPUT);
  
  // Pins für Bremsen als Ausgang
  pinMode(bremseAPin, OUTPUT);
  pinMode(bremseBPin, OUTPUT);

  // Geschwindigkeiten voll setzen
  analogWrite(vAPin, 255);
  analogWrite(vBPin, 255);
  
  // Bremse an (Motoren aus)
  digitalWrite(bremseAPin, HIGH);
  digitalWrite(bremseBPin, HIGH);

  // 2 Sekunden warten nach einschalten
  delay(2000);

  //US-Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  //LEDs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
}

void loop(){
  //Distanz mit Ultraschall-Sensor ermitteln
  long distance = ping_cm();
  
  //Distanz größer 10cm, fährt vorwärts
  if (distance > 10) {
    geradeaus_fahren();
  }
  else { //Distanz kleiner gleich 10cm
    // Bremse ein, LED wechsel und Kopf drehen.
    digitalWrite(bremseAPin, HIGH);
    digitalWrite(bremseBPin, HIGH);
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, HIGH);

    kopf_links();   
    delay(500);
    long links_cm = ping_cm();

    kopf_rechts(); 
    delay(500);
    long rechts_cm = ping_cm();
      
    if (links_cm > 10 && rechts_cm <= 10){
      digitalWrite(motorAPin, HIGH);
      digitalWrite(motorAPin, LOW);
      digitalWrite(bremseAPin, LOW);
      digitalWrite(bremseBPin, LOW);
      delay(500);
      geradeaus_fahren();
    }
    else if (links_cm <= 10 && rechts_cm > 10){
      digitalWrite(motorAPin, LOW);
      digitalWrite(motorAPin, HIGH);
      digitalWrite(bremseAPin, LOW);
      digitalWrite(bremseBPin, LOW);
      delay(500);
      geradeaus_fahren();
    }

    kopf_mitte();
    delay(500);
  }
}

**i_make_it** · 18.11.2016, 08:50

Was mir persöhnlich an dem Code nicht gefällt, ist die verwendung der vielen Delays
delay()
delayMicroseconds()
PulseIn()
sind ja alles Befehle die das Programm solange anhalten bis sie fertig sind.
Das wird also ein extrem langsam reagierneder Roboter.
Da der Prozessor die meiste Zeit damit verbringt zu warten.
Duch geschickte Programmierung und Verwendung von Interrupts und Timern sowie Vergleichen, kann mann das Programm weiterlaufen lassen und somit auch andere Aufgaben ausführen.
Soll der Roboter also später mehr Funktionern bekommen wäre es sinnvoll sich frühzeitig mit den anderen Möglichkeiten zu befassen, bevor man sehr viel komplett neu entwickeln muß.

Ist halt meine persöhnliche Meinung.

Momentan lasse ich mit einem Arduino Nano, zwei hc-sr04 auf zwei RC-Microservos, drei IR Reflexlichtschranken (Für Linienfolger), 4 Schalter (Bumper für Kolisionserkennung) und zwei gehackte RC-Servos (als Getriebemotoren für die Räder), laufen.
Das sind 4 Ausgänge für die RC-Servos, 2 Ausgänge für die Trigger der HC-SR04, die beiden Interrupteingänge für die HC-SR04, 3 Digitaleingänge für die IR-Lichtschranken, und ein Analogeingang für die Bumper (ein Widerstandsnetzwerk generiert verschiedene Spannungen für die Schalterkombinationen).
Als nächstes kommt Odometrie hinzu womit nochmal 4 Digitaleingänge belegt werden.
Da der dann nicht ständig Denkpausen einlegen soll, gibt es bisher im Code kein einziges Delay.
Und es soll auch keins hinzukommen.