Motorsteuerung über Sensorik

[zerebrator]

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

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 :


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

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]

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]

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]

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/threads/68437-Vorstellung-eines-aktuellen-kleinen-Weihnachtsurlaub-Projekts-%28ab-22-12-%29?p=622061&viewfull=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).



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]

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]

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]

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]

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


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


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:


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.



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]

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:


/////////////////////////////////////////////////////////////
// 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 \\:D/

[i_make_it]

Ein Kompaßsensor kann keine Position mitteilen sondern nur einen Winkel zwichen seiner Ausrichtung und der Nordweisung.
Von dem was ich sehe vermute ich mal Das du keinen Kompass sondern ein 3DOF Mems oder Magnetometer Gyroskop hast.
Da mußt Du Dich dann auch noch um Drift etc. kümmern, sonst wird es über die Laufzeit immer lustiger.

Erst mal mußt Du unterscheiden können ob Hinderniss oder Gegenstand (wenn du nur beim Finden eines Gegenstandes so reagieren willst und nicht bei jedem Hinderniss).

vom Rechnen her ist das einfach. Wenn du schon Winkelgrad hast, dann mußt Du ja nur 90° addieren oder subtrahieren (je nach dem ob du links oder rechts Drehen willst).
Ist das Ergebniss gößer 360° ziehst du 360° ab.
Ist das Ergebniss kleiner 0° addierst du 360°.
Dann ist das Ergebniss immer im gültigen Wertebereich.

Du nimmst also vor der Drehung dein Sensorwert, führts die Berechnung aus und übergibst das Ergebniss einer Variablen.
Dann kannst Du den aktuellen Sensorwert mit der Variablen vergleichen und beim Drehen z.B. bei einer Differenz kleiner 10° und kleiner 3° jeweils die Geschwindigkeit senken, damit daraus kein endloses Hinundherdrehen wird, weil er nie das Ergebniss trifft.
Geht dann auch wieder mit IF oder CASE.

[zerebrator]

Sorry habe mich natürlich falsch ausgedrückt . Du hast vollkommen Recht es geht hier um den Winkel der ausgegeben wird. Und der Sensor ist ein CPMS10 mit Magnetometer Tilt Compensated und Gyro modul usw.
Es ist auch absolut richtig was du weiterhin beschreibst das hin und herdrehen nervt und hier liegt mein Problem ...
Wie kann ich das Codetechnisch lösen ?

Habe absolut keine Idee wie ich das in einer if Anweisung lösen soll
Sprich uS1 sieht etwas bei 15 cm dann Lese bearing ein, drehe solange bis du + 90° ( + Toleranz) erreicht hast, dann weiter gerade aus ?

ich krieg das nicht hin :-(
Wie würdest du das Code technisch lösen ?

[i_make_it]

Sprich uS1 sieht etwas bei 15 cm dann Lese bearing ein, drehe solange bis du + 90° ( + Toleranz) erreicht hast, dann weiter gerade aus ?


Das wird gerinfügig komlizierter.

In dem von mir Skizzierten Konstrukt dreht er ja bis die Bedingung des us-Sensors nicht mehr erfüllt ist.
Jetzt hast Du ja us-Sensor Bedingung erfüllt, Entscheidung der Drehrichtung, Bedingung 90° Drehen setzen
Drehen bis 90° Bedingung erfüllt ist unabhängig davon ob us-Sensor Bedingung weiterhin erfüllt ist.

Der Case Abschnitt, darf also nicht mehr selbst das Dehen ausführen sondern muß variablen setzen, die den Systemzustand speichern und über die dann im nächsten Schritt verhindert wird das der us-Case Teil erneut angesprungen wird dafür aber ein 90° Case Teil.
dort wird dann gedreht bis der Winkel entsprechend klein wird und dann mit geringerer Geschwindigkeit weitergedreht bis der Winkel 0 ist.
Wenn die Bedingugn erfüllt dit, werden die Merker wieder gelöscht und der Teil verlassen.
Dann kann wider normal weitergearbeitet werden.
Das Ziel ist dabei halt so Geschickt die Logik aufzubauen, das man sich nicht in endlosen hin und her Springen verzettelt.

Deshalb macht es Sinn sich vor dem Programmierne zu überlegen wass will man denn alles machen und welche Bedingunge nmüssen erfüllt werden.
Asl zweites mauß man sich darüber im klaren sein, daß, damit Sensoren immer wieder eingelesen werdne können das Programm ständig weiterlaufen muß.
Also Endlosschleife.
Damit wird auch der Code wärend einer Aktion tausende male durchlaufen.
Also wo halte ich wie, welchen Systemzustand fest, und verhindere damit das Entscheidungsbäume während einer Aktion immer wieder durchlaufen werden und sich so Werte verschieben oder Aktionen vorzeitig Terminiert werden, weil Eingangsbedingungen mittleriweile nicht mehr erfüllt sind.

delay sollte im Code nirgendswo vorkommen, sondern durch Timer ersetzt werden. so kann der Code durchlaufen werden und während einer Aktion auch andere Eingaben und Ausgaben verarbeitet werden.
Wenn etwas wirklich genau sein soll, muß man über Interrupts nachdenken.

Unter Umständen kann es notwendig sein den gesammten bisher erstellten Code auf den Prüfstand zu stellen und mit dme auge des neu erworbenen wissens zu überlegen ob ein kompletter Neubau nicht sinnvoller ist um schwächen und ausuferndes Volumen auszubügeln.

Wenn das ganze erst mal zu komplex ist, kannst Du Dir auch Anleien bei UML holen und z.B. auf einem Sück Hartschaum mit kleinen Papierkarten die einzelnen Funkionsblöcke mit Stecknadeln feststecken sowie mit bunten Fäden die Abhängigkeiten (alternativ auch ein UML Programm am Computer) aufzeigen.
Nach und nach eränzt man auf den Karten dan ndie Variabeln und erkennt so ob ein vorrausgehendes Modul die auch liefert oder ob eine Variabel nach dem Setzen auch irgendwo wieder gelöscht wird. Oder als Zombie das System blockiert.

Das dann in Code zu bringen ist dann meist der einfachere Teil, da man da schon den meisten Denksport hinter sich hat und (fast) nur noch auf Syntax achten muß.

[zerebrator]

Hi

Also ich habe das versucht so zu lösen :

else if (taskTrigger==3) { //nach rechts
switch (taskStep) {
case 0:
digitalWrite(leftMtrDirPin1, LOW);
digitalWrite(leftMtrDirPin2, LOW);
digitalWrite(rightMtrDirPin1, HIGH);
digitalWrite(rightMtrDirPin2, LOW);
analogWrite(leftMtrSpdPin, 255); //drive the motor
analogWrite(rightMtrSpdPin, 255);

currentPosition=bearing;
taskStep++;
break;
case 1:
if (currentPosition= newPosition+90) {
taskTrigger=0;
taskStep=0;

analogWrite(leftMtrSpdPin, 0); //stopp the motors
analogWrite(rightMtrSpdPin, 0);
}
}
}

vorher kommt die Anweisung :

if (!taskTrigger && (uS2 < 15 & uS3 > 30 )) //abstand kleiner als 15cm und abstand links grösser als 30
taskTrigger=1;

else if (!taskTrigger && ( uS2 < 15 & uS1 > 30 )) //wenn abstand kleiner als 15 und links groesser als 30 dann rechts
taskTrigger=2;

else if (!taskTrigger && (uS3 < 16)) //wenn abstand kleiner als 16 dann rechts drehen
taskTrigger=3;


leider "pulsiert" der Antrieb nur
Es schert ihm nicht was die Werte des kompasmoduls machen.

Ich krieg das nicht hin ....
Warum wartet er nicht bis er um 90° gedreht hat ???

Hat jemand einen Vorschlag ?

Gruß
Chr.

[i_make_it]

if (currentPosition= newPosition+90) {

Syntaxfehler?
http://www.arduino-tutorial.de/category/programmieren/

Arithmetische Operatoren
= Zuweisung a=2*b Weist der linken Seite den Wert auf der Rechten Seite zu.

Vergleichsoperatoren
== Gleichheit a==b Prüft auf Gleichheit.


currentPosition=bearing;
wird bei jedem Durchlauf aktualisiert

und mit dem Inhalt von "newPosition+90" wieder überschrieben.
if (currentPosition= newPosition+90) {

Wo "newPosition" herkommt, wann es mit was gesetzt wird und was sicherstellt das es nur einmal am Anfang einer Drehung gesetzt wird kann ich nicht erkennen.

[zerebrator]

hmm sobald ich es vergleiche läuft er endlos weiter egal wie oft ich ihm um seine eigene Achse drehe ...

Vielleicht zu Erklärung bzw. So wie ich es gemacht habe.
currentPosition bekommt die Werte direkt vom baering aus diesem 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);
}

currentPosition und newPosition habe ich deklariert mit

int currentPosition;
int newPosition;

in dem Fall ist newPosition = 0 ? oder
wie übergebe ich nur für diese Anweisung die Werte von currentPosition an newPosition ?
denn wenn ich das nach der Initialisierung mache mit newposition=currentposition dann ist doch new Position gleich wie current ,wenn ich dann vergleiche dann wird dieser Zustand, weil er ja dauernd übergeben wird nie erfüllt sein oder ?????

???

[zerebrator]

if (currentPosition= newPosition+90) {

Syntaxfehler?
http://www.arduino-tutorial.de/category/programmieren/

Arithmetische Operatoren
= Zuweisung a=2*b Weist der linken Seite den Wert auf der Rechten Seite zu.

Vergleichsoperatoren
== Gleichheit a==b Prüft auf Gleichheit.


currentPosition=bearing;
wird bei jedem Durchlauf aktualisiert

und mit dem Inhalt von "newPosition+90" wieder überschrieben.
if (currentPosition= newPosition+90) {

Wo "newPosition" herkommt, wann es mit was gesetzt wird und was sicherstellt das es nur einmal am Anfang einer Drehung gesetzt wird kann ich nicht erkennen.

Ja das ist auch mein Schmerz ... keine Ahnung wie ich newPosition initialieseren , übergeben , deklarieren soll....

[i_make_it]

Dein Triggerereigniss ist doch das einer oder mehrere us-Sensoren ein Hinderniss erkennen.

Wenn du fest um 90° drehen willst,
fragst Du ab ob die Sensoren was erkennen und der Veriegelungsmerker nicht gesetzt ist.
Wenn das zutrifft, wird dieser Programmteil ausgeführt.
Hier setzt Du dann die Variable für die Reaktion (links, Rechts, Rückwärts, wie auch immer).
Setzt die Variable für den Zielwinkel auf den aktuellen Winkel plus/minus 90°
Und setzt den Verriegleungsmerker.

Jetzt hast Du die informatuion in welche Richtung Du drehen musst.
Den Zielwinkel.
Und eine Verriegelung die verhindert, das der Teil der das alles verändert nicht wieder durchlaufen wird.

Im nächsten Programmteil wird abgefragt ob der Verrigelungsmerker gesetzt ist und nur dann der Teil ausgeführt.
Dort wird dann entsprechend der Variable für die Drehrichtung die Motoren für Linksdrehung oder Rechtsdrehung aktiviert und der Zielwinkel mit dem aktuellen Winkel verglichen.

Wenn Aktueller Winkel und Zielwinkel gleich sind werden die Motoren gestoppt (oder auf Gradeausfahrt), die Variable für die Reaktion/Richtung wieder auf 0 gesetzt und der Verriegelungsmerker ebenfalls auf 0 gesetzt.
Damit hat das System wieder den Ausgangszustand.

Du kannst das Ganze auch entwickeln ohne erst mal eine Zeile code in der Zielspache zu können.

Male erst mal einen Kreis, und von oben eine Linie die auf den Kreis trifft.

Der Kreis ist void main(); (Die hauptschleife)

Die Linie ist void setup(); und was davor kommt (also alles was einmalig beim Power Up des µC durchlaufen wird)

Jetzt kann man alles was im Programm passiert als Blöcke nehmen und auf kleinen Zetteln auf den Kreis kleben/anheften.

Wenn Du alles was Du haben willst untergebracht hast, fahre mit dem Finger vom Beginn der Linie in den Kreis und dann im Uhrzeigersinn immer wieder den Kreis entlang.
Bei jedem Punkt(Block) den Du hast, prüfe nach was passiert dort mit den Werten die alle Variablen zu dem Zeitpunkt haben.
Mit IF wird dann entweder ein Block ausgeführt oder übersprungen.

Dabei gehe aber niemals davon aus, das eine aktion wie z.B. um 90° drehen bei einem Kreisdurchlauf erkledigt wird.
Nimm einfach an bei jedem durchlauf wird 1° geschafft.

Dann mekrst Du nämlich sofort wenn du noch 89° zu drehen hast und wieder in den Block mit dem Setzen des Zielwinkels springst, das dieser wieder auf 90° unterscheid zum aktuellen winkel gesetzt wird.
Der Systemzustand wird also nicht verändert sondern das System verbleibt in dem Zustand beginne mit dem Drehen.
Die Folge ist ein endloses Drehen. Denn bei Jedem Programmdurchlauf wird einfach der bereits geschaffte Winkel wieder auf den noch zu drehenden winkel wieder draufgerechnet.

So kannst Du mit so einem Ablaufdiagramm alle Systemzustände erst mal definieren und prüfen ob auch Wirklich das passiert was Du haben willst.

Wenn das funktioniert, kannst Du hingehen und überlegen, was davon muß in der Hauptschleife bleiben und was kann in externe Funktionen ausgelagert werden, um die Zykluszeit der Hauptschleife klein zu halten.

Am Ende wird der Kreis direkt links von dem Punkt wo die Linie auf ihn trifft aufgetrennt.
So das du eine lange senkechte Linie hast.
ausgelagerte Funktionen werden dann einfach unten dran gehängt.
So bekommst Du
Alles
Für allgemeines Setup (includes etc.)
void setup();
{
}
void main();
{
}
void funktion1();
{
}
.
.
.

Dann kannst Du anfangen zu koden ohne ständig wieder irgendwas anpassen zu müssen damit neues integriert werden kann.
Oder ein komplett unübersichtlichen Code zu erhalten.


Mit Deinen Codeschnipseln bin ich nicht klar gekommen,


/////////////////////////////////////////////////////////////
// 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);
}
.
.
.
.
. ???
.
.
.
nicht Tasktrigger und us2 kleiner 15
if (!taskTrigger && (uS2 < 15 & uS3 > 30 )) //abstand kleiner als 15cm und abstand links grösser als 30
{
taskTrigger=1;
}
else if (!taskTrigger && ( uS2 < 15 & uS1 > 30 )) //wenn abstand kleiner als 15 und links groesser als 30 dann rechts
{
taskTrigger=2;
}
else if (!taskTrigger && (uS3 < 16)) //wenn abstand kleiner als 16 dann rechts drehen
{
taskTrigger=3;
}
.
.
.
. ????
.
.
.
else if (taskTrigger==3)
{ //nach rechts
switch (taskStep)
{
case 0:
digitalWrite(leftMtrDirPin1, LOW);
digitalWrite(leftMtrDirPin2, LOW);
digitalWrite(rightMtrDirPin1, HIGH);
digitalWrite(rightMtrDirPin2, LOW);
analogWrite(leftMtrSpdPin, 255); //drive the motor
analogWrite(rightMtrSpdPin, 255);
currentPosition=bearing;
taskStep++;
break;
case 1:
if (currentPosition= newPosition+90)
{
taskTrigger=0;
taskStep=0;
analogWrite(leftMtrSpdPin, 0); //stopp the motors
analogWrite(rightMtrSpdPin, 0);
}
}
}
.
.
.
.
.
.


Ich kann nicht sagen ob und wo Du eventuell Syntaxfehler drin hast oder ungünstig was weggelassen hast.

Schon bei:
while (Wire.available() < 4);
müsste von der Syntax eigentlich danach
{
der Code der solange ausgeführt wird
}
kommen.
Und wenn es nur delay ist.
Wobei es strittig ist ob man ein wire.available überhaupt braucht, da die Daten ja im Empfangspuffer stehen sollten.

[HaWe]

Schon bei:
while (Wire.available() < 4);
müsste von der Syntax eigentlich danach
{
der Code der solange ausgeführt wird
}
kommen.
Und wenn es nur delay ist.
Wobei es strittig ist ob man ein wire.available überhaupt braucht, da die Daten ja im Empfangspuffer stehen sollten.
wieso?

Wire.requestFrom(CMPS10Addr , 4);
fordert 4 Bytes von der dev addr an, und
while (Wire.available() < 4);
wartet einfach solange, bis 4 Bytes tatsächlich im i2c-Puffer vorliegen.
Und danach geht's weiter.

so funktioniert einfach nun mal das API der Wire() class.

[i_make_it]

Also
Wire.endTransmission();
Beendet die Übertragung und könnte mit den Returncodes 0 bis 4 zum Auswerten des Fehlerstatus benutzt werden.

Wire.requestFrom(CMPS10Addr , 4);
Fragt die (in diesem Fall 4) Bytes vom Slave ab. Danach stehen sie im Eingangspuffer.

Wire.available()
Gibt an wie viele Bytes im Puffer stehen.

while (Wire.available() < 4);
Wartet solange Wire.available() kleiner 4 ist.

Aber ohne Abbruchbedingung falls der Vergleichswert nie erreicht wird und ohne einen Befehl der die Schleife verzögert.

Sinnvoll wäre z.B. eine zweite, veroderte Abbruchbedingung und im Körper der Whileschleife ein Hochzählen.
Wenn dann z.B. nach 1000 Schleifendurchläufen keine 4 Bytes vorhanden sind wird die Schleife durch die zweite Abbruchbedingung trotzdem terminiert und man kännte den Zählerstand auf Erreichen des Maximalwertes auswerten um danach eine Fehlerbehandlung zu starten.

Will mann es unsauber, Quick & dirty machen, kann man es natürlich nackt hinschreiben, dann kann man es aber auch ganz weglassen und einfach ungeprüft den Puffer auslesen.

[HaWe]

Also
Wire.endTransmission();
Beendet die Übertragung und könnte mit den Returncodes 0 bis 4 zum Auswerten des Fehlerstatus benutzt werden.

Wire.requestFrom(CMPS10Addr , 4);
Fragt die (in diesem Fall 4) Bytes vom Slave ab. Danach stehen sie im Eingangspuffer.

Wire.available()
Gibt an wie viele Bytes im Puffer stehen.

while (Wire.available() < 4);
Wartet solange Wire.available() kleiner 4 ist.

Aber ohne Abbruchbedingung falls der Vergleichswert nie erreicht wird und ohne einen Befehl der die Schleife verzögert.

Sinnvoll wäre z.B. eine zweite, veroderte Abbruchbedingung und im Körper der Whileschleife ein Hochzählen.
Wenn dann z.B. nach 1000 Schleifendurchläufen keine 4 Bytes vorhanden sind wird die Schleife durch die zweite Abbruchbedingung trotzdem terminiert und man kännte den Zählerstand auf Erreichen des Maximalwertes auswerten um danach eine Fehlerbehandlung zu starten.

Will mann es unsauber, Quick & dirty machen, kann man es natürlich nackt hinschreiben, dann kann man es aber auch ganz weglassen und einfach ungeprüft den Puffer auslesen.

Das erste Wire.endTransmission(); trennt den vorangehenden i2c-write Befehl (um den Slave und seine Register zu adressieren und das Lesen vorzubereiten) von dem darauffolgenden i2c-read.
Hier auf Fehler zu testen, ist tatsächlich sinnvoll, wenn Verbindungsprobleme bestehen sollten. Dann kann man die Schleife vorzeitig verlassen (Fehlermeldung an Serial.print()!).

per Wire() zu versuchen, eine bestimmte Anzahl Bytes zu lesen ohne dass sie im Puffer zur Verfügung stehen gibt tatsächlich aber meistens Mist - das wegzulassen ist die schlechteste aller Lösungen: es sollte so stehen bleiben.

Ein Timeout in die while Schleife einzubauen, ist nicht verkehrt, ist aber bei Arduino Wire() üblicherweise nicht nötig, alle Arduino Beispiel Codes verzichten darauf: wenn 4 Bytes angefordert werden, dann ist damit zu rechnen, dass sie auch umgehend kommen (wenn das vorrangehende endTransmission fehlerfrei war) , es führt nur leider oft zu Fehlern wenn man zu früh anfängt mit dem Wire.read (also wenn, wie gesagt, der Puffer ausgelesen wird, er aber noch nicht mit den erforderlichen Bytes gefüllt ist).

Bei zickigen Master-Slave-Kombinationen (z.B. anderen MCU devices, die bitbang-i2c verwenden oder vlt auch übermäßiges clock-stretching) kann das zugegebenermaßen vielleicht sinnvoll sein. Das betrifft dann aber meist andere Master, nicht Arduino-Master (z.B. Raspis oder Lego EV3 als Master ), der Arduino ist da geduldig und der cmps10/11 als Slave gehört auch nicht zu den Zicken (hab ich selber, läuft bei 100-400kHz und 1m Kabel dazwischen ohne Probleme, und zwar sowohl am Arduino als auch am Raspi.)
http://www.mindstormsforum.de/viewtopic.php?f=78&t=8491&start=60#p69275
Wer mag, kann ja die beiden Codes mal gegenseitig vergleichen.

Ein delay ist in der while Schleife ebenfalls nicht nötig, Arduino-Standard-Beispiele fügen hier nie delays ein (ich wäre nicht überrascht, wenn die in der Funktion Wire.available() sowie so schon enthalten wären), daher kann man nach while() direkt die Nullanweisung ";" hinschreiben ohne irgendeinen {code} / Body/Körper.

Sprich, so wie der Code oben steht, ist es das Arduino-Standardverfahren, hier wäre der letzte Ort, wo ich dran herum spielen würde zur Fehlersuche.

ps,
Sinnvoll ist es aber immer, in den Code Serial.println() einzufügen, wobei dann verschiedenste Variablenwerte schrittweise zur Kontrolle ausgegeben werden. In diesem Fall sollte man mal das Heading zur Kontrolle anzeigen lassen, und natürlich auch den ganzen Rest zu Debug-Zwecken.

edit,
ggf bei Fehlern im i2c-auslesen: sind die verwendeten Pullups richtig?
welcher Arduino wird verwendet (manche haben ausreichende Pullups bereits intern verbaut, manche nicht) und ggf welche externen Pullups wurden gesteckt?

[Mxt]

Ein delay ist in der while Schleife ebenfalls nicht nötig, Arduino-Standard-Beispiele fügen hier nie delays ein (ich wäre nicht überrascht, wenn die in der Funktion Wire.available() sowie so schon enthalten wären),

Das ist in orignal Arduino viel simpler implementiert, ich habe gerade mal in den Sourcen nachgesehen.

Hier Auszüge aus Wire.cpp


int TwoWire::available(void)
{
return rxBufferLength - rxBufferIndex;
}

available gibt nur zurück, wieviele von den vom Bus geholten Bytes noch nicht vom Sketch verarbeitet wurden.

Alle Funktionen, die vom Bus lesen, warten bis sie fertig sind. Hier requestFrom (siehe Pfeil für die Wartestelle)


uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
{
if (isize > 0) {
// send internal address; this mode allows sending a repeated start to access
// some devices' internal registers. This function is executed by the hardware
// TWI module on other processors (for example Due's TWI_IADR and TWI_MMR registers)

beginTransmission(address);

// the maximum size of internal address is 3 bytes
if (isize > 3){
isize = 3;
}

// write internal register address - most significant byte first
while (isize-- > 0)
write((uint8_t)(iaddress >> (isize*8)));
endTransmission(false);
}

// clamp to buffer length
if(quantity > BUFFER_LENGTH){
quantity = BUFFER_LENGTH;
}
// perform blocking read into buffer <------------------------------ Da wartet er
uint8_t read = twi_readFrom(address, rxBuffer, quantity, sendStop);
// set rx buffer iterator vars
rxBufferIndex = 0;
rxBufferLength = read;

return read;
}


Eine Wire Implementierung, die im Hintergrund arbeiten kann, hat z.B. der Teensy. Aber da heißt das nicht wartende requestFrom sicherheitshalber sendRequest, um es von der blockierenden Arduino kompatiblen Variante zu unterscheiden.

[HaWe]

Das ist in orignal Arduino viel simpler implementiert, ich habe gerade mal in den Sourcen nachgesehen.

Hier Auszüge aus Wiring.cpp


int TwoWire::available(void)
{
return rxBufferLength - rxBufferIndex;
}

available gibt nur zurück, wieviele von den vom Bus geholten Bytes noch nicht vom Sketch verarbeitet wurden.

Alle Funktionen, die vom Bus lesen, warten bis sie fertig sind. Hier requestFrom (siehe Pfeil für die Wartestelle)


uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
{
if (isize > 0) {
// send internal address; this mode allows sending a repeated start to access
// some devices' internal registers. This function is executed by the hardware
// TWI module on other processors (for example Due's TWI_IADR and TWI_MMR registers)

beginTransmission(address);

// the maximum size of internal address is 3 bytes
if (isize > 3){
isize = 3;
}

// write internal register address - most significant byte first
while (isize-- > 0)
write((uint8_t)(iaddress >> (isize*8)));
endTransmission(false);
}

// clamp to buffer length
if(quantity > BUFFER_LENGTH){
quantity = BUFFER_LENGTH;
}
// perform blocking read into buffer <------------------------------ Da wartet er
uint8_t read = twi_readFrom(address, rxBuffer, quantity, sendStop);
// set rx buffer iterator vars
rxBufferIndex = 0;
rxBufferLength = read;

return read;
}


Eine Wire Implementierung, die im Hintergrund arbeiten kann, hat z.B. der Teensy. Aber da heißt das nicht wartende requestFrom sicherheitshalber sendRequest, um es von der blockierenden Arduino kompatiblen Variante zu unterscheiden.

was also unter dem Strich ebenfalls bestätigt, dass man hinter dem while(Wire.available()....) keinen {code} mit delays - oder was auch immer - einfügen muss, was i_make_it verlangt oder ansonsten als schlechten Programmierstil ("quick and dirty") verunglimpft - und dann sogar zum Weglassen rät, was dann angeblich auch keinen Unterschied machen würde.
Und was ebenfalls bestätigt, dass die Konstruktion
Wire.requestFrom(CMPS10Addr , 4); // Abfrage von 4 Bytes vom CMPS10
while (Wire.available() < 4); // Warten, bis 4 Bytes verfügbar
völlig korrekt ist und verwendet wird, um zu warten, bis die i2c-Daten im Puffer soweit zur Verfügung stehen, dass sie anschließend von Wire.read() gelesen und weiter verarbeitet werden können.

Wenn der OP also Probleme mit dem cmps10 hat und auch nicht von sich aus einen Fehler findet, würde ich, wie teilw. schon oben angeführt, zusammengefasst dazu raten:
- den cmps10 Teil prüfen, ob er wirklich dem verwendeten Original-Example Code (meist wohl ursprünglich von James Henderson) entspricht, aber dann lassen wie er ist (s.z.B.: http://www.robot-electronics.co.uk/files/arduino_cmps11_i2c.ino)
- erstmal dann diesen isolierten cmps10-Code laufen lassen, wie er im Original cmps10/11 Beispiel-Code steht, ob er dann auch wirklich wenigstens isoliert funktioniert
- in den Code Serial.println() einzufügen, wobei dann verschiedenste Variablenwerte schrittweise zur Kontrolle ausgegeben werden. In diesem Fall sollte man mal das Heading zur Kontrolle anzeigen lassen, und natürlich auch den ganzen Rest zu Debug-Zwecken.
- ggf bei Fehlern im i2c-auslesen die verwendeten Pullups überprüfen: welcher Arduino wird verwendet (manche haben ausreichende Pullups bereits intern verbaut, manche nicht) und ggf welche externen Pullups wurden gesteckt?

Anm. (Thema "Nikolausi" und "Osterhasi" ;) ):
im Original-Code von James Henderson steht fälschlicherweise immer "bearing", das ist der falsche Name für das, was hier ausgelesen wird:
Es handelt sich um "heading", auf deutsch "Kurs";
bearing hingegen ist die Peilung eines (externen) Objektes relativ zum momentanen Kurs.
Das nur zur Erklärung, warum ich immer "heading" eingesetzt habe, wo der O-Autor "bearing" verwendet hat. Das tut dem Algorithmus aber ntl keinen Abbruch.