Arduino Roboter - Servo reagiert nicht korrekt

[Sisor]

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.

#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]

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.

[Sisor]

Die Servo-Bibliothek verwendet Timer und Interrupts.

[i_make_it]

Ich meinte ja auch explizit:
delay()
delayMicroseconds()
PulseIn()
und nicht die Servo Lib.

PulseIn wartet im Fehlerfall z.B. per Default eine Sekunde bis es weitergeht.
Mit den beiden "delay(500)" wären das im Extremfall bei einem Messzyklus (links-rechts) 3 Sekunden.
Und beim Gradeausfahren eine Sekunde von einer Fehlmessung bis zur nächsten Messung die dann eventuell ein gültiges Ergebniss liefert.
Der hc-sr04 z.B. ist bei einer Fehlmessung nach maximal 0,2 Sekunden wieder bereit zur nächten Messung

[wendtdir]

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:

[Sisor]

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?

Hallo wenddir,

lass dich nicht verrückt machen. Z.Z. hat dein Programm vier Hauptaufgaben:
- Fahren
- Kopfposition ändern
- Ultraschallmessung
- Entscheidungen aufgrund der Messung treffen

Das Fahren passiert in dem Programm automatisch. Dein Roboter kann Messungen machen oder die Kopfposition ändern, während er fährt.
Da dein Roboter nur einen Sensor besitzt, ist für die Entscheidung, was zu tun ist, auch nur das eine Messergebnis entscheidend. Der Roboter muss nicht gleichzeitig mehrere Sensoren auswerten. Daher muss dein Programm auch keine weiteren Dinge parallel abarbeiten. Interupt- oder schedulerbasierte Programmierung ist also z.Z. nicht nötig.

Der Post von I_make_it bezieht sich darauf, dass das Programm umgeschrieben werden müsste, wenn du das System um weitere Sensoren erweiterst wie z.B. Kontaktsensoren oder ähnliches.

Das von dir oben genannte Verhalten wäre dann so realisierbar.

    
void kopf_mitte() {
  //Servo 0-Position (etwas schief, daher 80°)
  myservo.write(80);
}
void wenden() {  
   digitalWrite(motorAPin, HIGH);
   digitalWrite(motorAPin, LOW);
   digitalWrite(bremseAPin, LOW);
   digitalWrite(bremseBPin, LOW);
   delay(500); // Hier Wendezeit eintragen
}
void loop() {
  // ...
  kopf_mitte(); // Servo beginnt Bewegung
  wenden(); // Servo bewegt sich während des Wendens rasch zur Mittelposition
  // ...

[i_make_it]

Der Post von I_make_it bezieht sich darauf, dass das Programm umgeschrieben werden müsste, wenn du das System um weitere Sensoren erweiterst wie z.B. Kontaktsensoren oder ähnliches.

Exakt das.
Wenn Der Roboter also für einige Zeit so bleiben soll, kann man das lassen.
Wenn man aber plant noch Bumper (Kollisionserkennung) und ggf. weitere Fernsensoren (US oder IR) oder Odometriesensoren hinzuzufügen, dann sollte man sich frühzeitig überlegen ob es nicht sinnvoll ist die Programmierung zu ändern bevor man irgendwann ein unterirdisches Timingverhalten hat und dann erst mal Wochen oder Monate damit verbringt alles noch mal neu zu machen.

Mein erstes Arduino Programm war eine Fußgängerampel mit Delays. mein zweites Programm zwei hc-sr04 und die drei Reflexlichtschranken fürs Linienfolgen.
Das wurde dann um die beiden gehackten Servos als Antriebe erweitert und um die Bumper.
Dann kam der Wunsch die hc-sr04 jeweils um 180° in 10° schritten zu drehen und so eine 360° Abdeckung zu bekommen.
Schon bei den beiden parallel betriebenen hc-sr04 und den drei Reflexlichtschranken, war mit Delay und Co kein Blumentopf mehr zu gewinnen.
Weil da keiner Linie mehr gefolgt werden konnte oder man dazu so langsam hätte fahren müssen, daß das gar keinen Sinn mehr macht.

Es ist also eine Anregung etwas vorzuplanen und sich Gedanken zu machen, wo man denn hin will.
Klar, erst mal will man überhaupt ein Ergebniss bekommen das irgendwie funktioniert, Aber wenn man es noch besser hinbekommen kann ist ja auch nicht grade schlecht.

Bei mir selbst geht es momentan halt darum wie viel kann man mit einem Nano denn erreichen.
Der nächste Umbau ist Hardware, um alle Eingänge per IRQ überwachen zu können.
Sprich alles mit UND versehen (Eigenlich ja umgekehrt da 2 Eingänge Gesetzt werden müssen ohne das es zu einem Übersprechen auf die Anderen Eingänge über den gemeinsam genutzten IRQ Eingang kommt) so das immer auch der IRQ ausgelöst wird und dann in der ISR den Status der Eingänge auswerten und entsprechende Merker Variablen für den normal Programmablauf Setzen.
Der zweite IRQ Eingang des Nano ist dann Frei für wirklich wichtige Sachen.

[oberallgeier]

.. 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? ..

Die triviale Antwort ist : ja.

Das hatte ich zuerst mit meinem Dottie (siehe "Innenleben" - klick) gemacht. Servo agiert - und gleichzeitig schwenkt der Fahrebot, siehe Video durch Klick auf Bild. Nur - ich habs in C programmiert, Arduino-language kann ich nit. Aber es sollte sicher gehen - und Sisor schrieb ja auch schon wie´s geht.