Motorsteuerung über Sensorik

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