Motorsteuerung über Sensorik

**zerebrator** · 10.06.2017, 14:53

Hallo zusammen,

Bin am kämpfen mit einem wahrscheinlich für Programmierexperten kleinem Problem.
Ich würde gerne meinen kleinen Robi über die Sensoren positionieren lassen.
Dazu habe ich 3 Ultraschalsensoren Links ; Mitte ; Rechts
Jetzt zu der Aufgabe : Sobald der Linke ein Hindernis erkennt nehmen wir mal an bei ca. 16 cm soll er sich nach rechts drehen und zwar so lange bis er ca. 30 cm vom Hindernis weg ist.
Wie kann ich das Programmtechnisch lösen ?

Hier meine Anweisung welch leider nicht funktioniert ...

Code:

if ((uS1 < 16 )) {//abstand kleiner als 16cm und abstand links grösser als 30**
********for (; uS1 > 30 ; uS1++)
********{
********//Motor A forward @ full speed
**** digitalWrite(12, LOW); 
**** digitalWrite(9, LOW); 
**** analogWrite(3, 255); 
** 
**** //Motor B forward @ full speed
**** digitalWrite(13, HIGH); 
**** digitalWrite(8, LOW); 
**** analogWrite(11, 255); 
********
********}
}
else {
****** //Motor A forward @ full speed
**** digitalWrite(12, HIGH); 
**** digitalWrite(9, LOW);
**** analogWrite(3, 255); 
** 
**** //Motor B forward @ full speed
**** digitalWrite(13, HIGH); 
**** digitalWrite(8, LOW); 
**** analogWrite(11, 255); 
}

**
}**//end loop

uS1 ist mein Ultraschalsensor für die Erkennung von Hindernissen auf der linken Pos .

deklariert wurde er mit :

Code:

*unsigned int uS1 = sonar1.ping(); //Sensor rechts
**Serial.print("Ping: ");
**Serial.print(uS1 / US_ROUNDTRIP_CM); 
**Serial.println("cm");
**uS1=uS1 / US_ROUNDTRIP_CM;
**if (uS1 ==0 ){
uS1=50;
}

Leider wie schon gesagt habe ich keinen Plan wie ich es realisieren kann.
Ich will wirklich nur mit den Werten arbeiten ... ( klar ich könnte den Robi ca 500 ms lang nach links drehen lassen o.ä. doch das ist keine Option )

Würde mich über jegliche Code Vorschläge freuen

Beste Grüße

Chris

**i_make_it** · 10.06.2017, 18:15

Hm, der US sensor müssten dann 14cm vom Drehpunkt des Roboters entfernt sein, damit bei einem Abstand von 16cm zum Hinderniss, durch Drehen ein Abstand von 30cm erreichbar ist.
wenn der Sensor genau auf dem Drehpunkt ist, kannst Du soviel drehen wie du willst der Abstand wird immer gleich bleiben, nur der Winkel ändert sich.
Du könntes den Linken und den rechten US-Sensor jeweils auf ein RC-Servo montieren und beim Drehen des Roboters, immer in z.B. 5° Schritten schwenken. dann kannst Du prüfen, ob das Hindernis z.B. mehr als 45° von der Fahrtrichtung entfernt ist oder bei einer Wand dann 90° um parallel zur Wand zu fahren.

Geometrische, physikalische gegebenheiten kannst Du halt nicht mit Programmcode auser Kraft setzen.

**zerebrator** · 10.06.2017, 18:50

Hmm schau mal das funktioniert bereits.
Ich habe die Sensoren auf meinem Robi drauf und drehe immer ca 500 ms nach links vom Hindernis. Das Programm hierzu ist a bissl komplizierter.

Was ich mit der Frage erreichen will ( scheint schwierig zu erklären ), ist nur der Zustand, dass ich den Sensor währen der Roboter sich von Hindernis wegbewegt kontrolliert wird.
Egal ob das durch drehen oder rückwärtsfahren erreicht wird.

Der Zustand des Sensors soll überwacht werden und (nur als bsp) bei 30 cm wieder die else Bedingung erfüllt sein also weiter gerade aus Fahren.

Im Prinzip geht es mir darum mit meinem vorh. Kompass dem Robi zu steuern und zwar immer um ca. 90° ++ oder, je nach zustand um 90° -- vom Hindernis wegbewegen.

Doch wenn ich schon mit dem Sonar solche "Probleme " habe ...

Ich hoffe ich konnte dir ansatzweise vermitteln was ich damit bewerkstelligen will.
Mein Problem ist wirklich dass ich kein Programmierer bin Mechanik und Elektrik kein Problem ich tue mir schwer mit C++

Darum wirklich nur die Frage wie kann ich in der if Anweisung die unter 16 cm mit der "sich vom Hindernis weg Bewegung erledigen damit ich die 20-30 cm erreiche und weiter vorwärts fahren kann.

Beste Grüße
Chr.

**zerebrator** · 11.06.2017, 12:57

Hat niemand eine Idee.
Sollte die Erklärung nicht ganz verständlich sein kann ich gerne nochmal versuchen das ganze genauer zu beschreiben.

VG
Chr.

**i_make_it** · 11.06.2017, 14:17

Welche US-Sensoren hast du denn?
Bei den hc-sr04 zum Beispiel muß man das Aussenden eines Bursts auslösen und dabei eine Starzeit festhalten dann wartet man bis ein Echo kommt oder die maximale Zeit abgelaufen ist (Was dann heist, das in der Reichweite kein Hinderniss ist oder es nicht detektiert werden kann.)
Einen einfachen Code (ohne Fehlerauswertung) habe ich hier im dritten Listing:
https://www.roboternetz.de/community...l=1#post622061
Das ist für einen Arduino Nano, mit zwei hc-sr04 und einem TCRT5000 fürs Linienfolgen.
Als Echoeingänge habe ich die Interrupteingänge genutzt, Zusammen mit dem trei laufenden Timer muß ich mich da um nichts weiter kümmern.
in der normalen Programschleife wird nur der zweite Timerwert vom ersten abgezogen und dann die Umrechnung von Zeit auf Entferung anhand der Werte für die Schallgeschwindigkeit vorgenommen (ohne Temperatur- und Luftfeuchtekompensation, einfach auf 20°C und trockne Luft bezogen).

Zitat von zerebrator

Würde mich über jegliche Code Vorschläge freuen

Konkrete Codevorschläde beiziehen sich üblicherweise auch auf eine konkrete Zielplattform.
C++ kann je nach Zielsystem auch unterschiedlich (vom Umfang und von der Funktion) implementiert sein.
Grade was die Hardwarespezifischen Funktionen angeht (Was ja bei µC's und Robotenr der wichtige Teil ist).

Also wäre noch interessant welche Zielplatform, welche IDE und welchen Dialekt (Sprachvariante) Du nutzt.

**zerebrator** · 11.06.2017, 15:58

Hallo i-make_it

Also mein Mover hat 3 Sensoren er läuft absolut autonom und Problemlos.
Es geht nicht darum die Sensorik zum laufen zu bringen sondern wirklich um die Tatsache dass ich eine definierte ( hier nur als Beispiel die 30 cm) Entfernung zum Objekt einhalten will.
Es ist quasi ein Puzzle der später im Gesamtprogramm verwendet wird.

Mir geht es nur um die Formulierung der Anweisung
Eigentlich frage ich mich warum die Anweisung mit der schleife nicht das tut was ich eigentlich will.
Hier liegt mein Problem ich stehe auf dem Schlauch ....

Ich benutze Arduino IDE 1.06 und 1.69
Zum ansteuern der Sonar Sensoren eine Library NewPing.h
Beim Arduino sehr verbreitet.
Der echte Code würde hier den Rahmen sprengen deshalb habe ich es sehr vereinfacht und wirklich nur auf das wesentliche ( genau darauf wo meine Aufgabe steht) reduziert.

Arduino hat nur eine Spezifische Programmierumgebung.

Ich hoffe damit konnte ich die Programmier experten unter Euch ansprechen.

SG
Chr.

**i_make_it** · 12.06.2017, 06:15

Den Code den Du angegeben hast, kann ich so nicht prüfen,
Bei Verwendung von newping.h gehört der Aufruf von sonar.ping(); innerhalb der void loop(); und es muß zwichen zwei Aufrufen mindesten 29ms vergehen, was in den Beispielcodes per Delay gelöst wird.
Ob Du die Randbedingungen einhält oder nicht ist aus Deinem Code nicht erkennbar.

Bei dem IF Konstrukt wird geprüft ob us1 kleiner 16 ist. Die Zählschleife, erhöht bei jedem Durchlauf us1 um 1, bis us1 > 30 ist. Ein vergleich des tatsächlichen Messwerts mit einem Sollwert findet nicht statt.
Also: Messe us1, setze us1 gleich us1 geteilt durch roundtrip, wenn us1 kleiner 16 ist zähle us1 hoch bis us größer 30 ist.

Was Du für Ausgänge setzt, beachte ich absichtlich nicht, denn die Motoren sind physikaliach vorhanden, haben also Masse und Trägheit.
Du kannst ja mal eine For Schleife nehmen und bei jedem Durchlauf den Wert der Zählervariable mit serial.print ausgeben.
Von 16 bis 30 zu zählen dauert nur ein paar Nanosekunden da hat sich kein Motor auch überhaupt nur angefangen zu drehen bis die Abbruchbedingung erfüllt ist.

Für einen Soll-Ist Vergleich ist ein Zählschleife so nicht zu gebrauchen.

Also ergeben sich folgende Fragen:
- Wie implementierst Du die minimale Wartezeit von 29ms die sonar.ping(); benötigt?
- Was hattest du vor mit der Zählschleife an dieser Stelle zu erreichen?
- Warum enthällt der Else Zweig eine entgegengesetzte Drehung wie die Zählschleife die im IF Zweig steckt? (Das würde den Roboter doch grade wieder in das Hinderniss hineindrehen.)

Eine gleichzeitige Überprüfung (z.B. mit switch case) wäre sinnvoller, um ein Festfahren in einer engen Bucht zu verhindern, da kann in einem Entscheidungskonstrukt auf alle Sensoren Rücksicht genommen werden, und je nach Fall entschieden werden, ob Linksdrehen, Rechtsdrehen oder Rückwärtsfahren sinnvoller ist.
Bei sequentieller Auswertung und Reaktion, sind diese Teile unabhängig voneinander.
Wird also erst links ein Hinderniss erkannt und nach rechts gedreht und dann rechts ein Hinderniss erkannt und nach links gedreht, hängt der Roboter in einer Loop aus der er selbst nicht mehr rauskommt.
Ein Gedächtniss (Merker) die es ihm erlauben zu erkennen, das er die selbe Handlung grade schon mal durchgeführt hat ohne das es Erfolg brachte ist also sinnvoll um ein Verzweigen in andere Verhaltensmuster zu ermöglichen.

**zerebrator** · 12.06.2017, 08:01

Ja endlich jetzt kommen wir der Sache näher.
Die schleife ... ist ein plumper verusch meinerseits die Aufagbe zu Lösen das es nicht funktioniert ist mir leider auch schon aufgefallen

Wie gesagt mein Problem ist das ich mich mit Programmieren rel. schlecht auskenne.

Nehmen wir mal an der Code ( der eigentliche ) Code Passt wir brauchen uns also keine Gedanken machen wieso der Mover nach der else Anweisung wieder auf das Hindernis zusteuert. Die Sensoren werden mit einer Verzögerung abgefragt und verreichten Ihren dienst perfekt. Alles läuft prima.... So jetzt aber benötige ich nur die Routine um meinen Mover rel. exakt 20- 30 oder 40 cm von einem Hindernis ( Wand , Eimer , Bein ..... usw. ) wegzubewegen. Wie Löse ich das softwaretechnisch.
Dein Ansatz mit Switch Case hört sich gut an doch.
Ist die Switch Case Anweisung nicht für beispielweise Tastatur eingaben ?
char choice = Serial1.read();
Serial.print("Val=");
Serial.println(choice);

switch(choice){
case 'U':
driveMotors(0, 100); //forward
break;
case 'D':
driveMotors(0, -100); //backward
break;

???

Wie bewerkstellige ich hier dass er die Prüfung durchführt. ??

**i_make_it** · 12.06.2017, 11:17

Zitat von zerebrator

Dazu habe ich 3 Ultraschalsensoren Links ; Mitte ; Rechts

Du hast 3 Sensoren: nennen wir sie mal us1, us2, us3.

us1 - ist der Linke
us2 - vorne mittig
us3 - ist der rechte

"ein Hinderniss anzeigt" steht für "der Schwellwert wurde überschritten" (dies kann später noch mit mehreren Schwellwerten erweitert werden)

wenn us2 ein Hinderniss anzeigt, soll was passieren?
wenn us1 ein Hinderniss anzeigt soll solange nach rechts gedreht werden bis kein Hinderniss mehr angezeigt wird.
Wenn us1 und us2 ein Hinderniss anzeigen soll solange nach rechts gedreht werden, bis Beide kein Hinderniss mehr anzeigen.
wenn us3 ein Hinderniss anzeigt soll solange nach links gedreht werden bis kein Hinderniss mehr angezeigt wird.
Wenn us3 und us2 ein Hinderniss anzeigen soll solange nach links gedreht werden, bis Beide kein Hinderniss mehr anzeigen.
wenn us1, us2 und us3 ein Hinderniss anzeigen soll solange rückwärts gefahren werden, bis entweder us1 oder us2 kein Hinderniss mehr anzeigen und dann um mindestens 60° gedreht werden.
Wenn us1 und us3 ein Hinderniss anzeogen aber nicht us2, soll was passieren?

wir haben jetzt also 7 Fälle die eine Aktion benötigen.
und 3 Eingänge (us1, us2, us3) die zu verarbeiten sind.

Werten wir eine Detektion an us1 mit 1,
an us2 mit 2 und an us3 mit 4,

bekommen wir folgende Wahrheitstabelle

Code:

us3|us2|us1
---+---+---
 4 | 2 | 1
---+---+---
 0 | 0 | 0 = 0 kein Handlungsbedarf
---+---+---
 0 | 0 | 1 = 1 nach rechts drehen
---+---+---
 0 | 1 | 0 = 2 Was soll passieren?
---+---+---
 0 | 1 | 1 = 3 stark nach rechts drehen
---+---+---
 1 | 0 | 0 = 4 nach links drehen
---+---+---
 1 | 0 | 1 = 5 was soll passieren?
---+---+---
 1 | 1 | 0 = 6 stark nach links drehen
---+---+---
 1 | 1 | 1 = 7 Rückwärts fahren und dann um mindestens 60° drehen
---+---+---

Das könnte dann so in etwa implementiert werden

Code:

if us1 < 16 
then var1 = 1
else var1 = 0

if us2 < 16 
then var2 = 2
else var2 = 0

if us3 < 16 
then var2 = 4
else var2 = 0

var = var1 + var2 + var3

switch (var)
case 1
   rechts drehen
break
case 3
   rechts drehen
break
case 4
   links drehen
break
case 6
   links drehen
brake
case 7
   rückwärts
   merker7 = 1
default
   if merker7 == 1
   then rechts drehen 60°
   merker7 = 0
   else
   vorwärts

Das ist ein Denkanstoß und kein Code zum stur Abtippen, da ich mich an keine Sprachsyntax halte sondern einen Pseudocode nutze der nur das Konstrukt verdeutlichen soll.

Die hier nicht berücksichtigten case(es) kann man natürlich (besser, sollte man) auch noch füllen, da alle nicht behandelten Fälle im Default landen.

bei einem System mit 2 Schwellwerten könnte man das erweitern:

Code:

if us1 < 16 
then var1 = 8
else if us1 < 32 
then var1 = 1
else var1 = 0

if us2 < 16 
then var2 = 16
else if us2 < 32 
then var2 = 2
else var2 = 0

if us3 < 16 
then var3 = 32
else if us3 < 32 
then var3 = 4
else var3 = 0

Die entstehenden Fälle kannst Du problemlos in eine Tabelle eintragen und so wie ich es oben gezeigt habe den jeweiligen Zahlenwert errechnen.

So kann man z.B. bei einem etfernteren Hinderniss schon eine kleine Kurskorrektur beginnen ohne die Geschwindikeit zu verlangsamen.
Reicht das nicht, kommt man dem Hinderniss ja immer näher und irgendwann gilt die Bedingung für das nahe Hinderniss, wo man dann eventuell sogar stoppt und im Stillstand dreht.

Zitat von zerebrator

Ist die Switch Case Anweisung nicht für beispielweise Tastatur eingaben ?

Kann man natürlich auch für Tastatur nehmen, aber Abfragen sei es nun IF-THEN oder SWITCH-CASE,
ist das eigentlich wurst welchen Variablentyp man abfragt.

Es ist eine Kontrollstruktur. Und die ist unabhängig von dem was man damit füttert.
Wenn ich ein Blech mit 4 Verschieden großen Löchern drin habe, ist dem das auch egal ob ich durch die Löcher Äpfel, Birnen oder Mandarinen fallen lasse.
Wenn ich vom kleinsten Loch zum größten gehe wird einfach alles nach Größe sortiert.

So ist es auch mit den Abfragen. Bedingung erfüllt? Ja/Nein, fertig.

Schau einfach mal hier:
http://www.arduino-tutorial.de/category/programmieren/

**zerebrator** · 12.06.2017, 11:39

Super danke damit kann ich was anfangen. vor allem die Anweisungen.

Falls das nicht allzu viel ist hätte ich noch eine zusätzliche Frage

Kann ich das ganze auf einen Kompasssensor übertragen.?
Und zwar dieser gibt mir ebenso ständig die Position aus:

Code:

  /////////////////////////////////////////////////////////////
  //     Kompass Modul Abfrage und senden der Daten         ///
  ////////////////////////////////////////////////////////////
  
  byte byteHigh, byteLow; // byteHigh / byteLow für Bearing
  char pitch, roll;       // Pitch und Roll
  int bearing;            // Bearing

  Wire.beginTransmission(CMPS10Addr); // Kommunikation mit CMPS10
  Wire.write(2);                      // Start Register (2)
  Wire.endTransmission();
  Wire.requestFrom(CMPS10Addr , 4); // Abfrage von 4 Bytes vom CMPS10
  while (Wire.available() < 4); // Warten, bis 4 Bytes verfügbar
  byteHigh = Wire.read();       // High-Byte für Bearing speichern
  byteLow = Wire.read();        // Low-Byte für Bearing speichern
  pitch = Wire.read();          // Byte für Pitch speichern
  roll = Wire.read();           // Byte für Roll speichern
  bearing = ((byteHigh << 8) + byteLow) / 10; // Bearing berechnen
  
  sendData(bearing, pitch, roll);             // Daten versenden

void sendData(int bearing, int pitch, int roll) {
  String data = String(bearing) + ";" + String(pitch) + ";" + String(roll);
  
  lcd.setCursor(0,0);
    lcd.print("bear-pitch-roll");
    lcd.setCursor(0,1);
    lcd.print(data);
   
  
 // Serial.println(data);
delay (100);
}

Bearing ist für mich interessant.

Hier möchte ich unabhängig von Wert ( es kann ja im raum der Wert zwischen 0 und 360° vorliegen) bei finden eines Gegenstandes mit den vorher beschriebenen Sonar Sensor um 90° gedreht werden .

Wie wäre hier dein Ansatz ?

1000 dank nochmal