Problem bei gleichzeitiger Verwendung von Wire.h, QMC5883L und Servo.h auf Mega

Hallo,

ich habe folgendes Problem bei dem ich derzeit nicht weiter komme:
Ich habe ein Schaufelraddampfermodell gebaut, dass ich über einen Arduino Leonardo mit einem alten Flightstick pro fernsteuere. Die Funkverbindung läuft über zwei HC12 Funkmodule.
Im Schiff befindet sich ein Arduino Mega 2560.
Es gibt einen 24V Motor (betrieben mit 16V) der das Schaufelrad antreibt und an einem L293DNE Motortreiber hängt. Der Motortreiber ist mit den PINs 5 und 6 verbunden.
Das Funkmodul hängt an Seriell1.
Es gibt zwei 12V Pumpen für Eigenbaustrahlruder die über MOSFETs ein und aus geschaltet werden. Je ein MOFET hängt an PIN 8 und PIN 4.
Beleuchtung wird ebenfalls über 2 MOSFETs geschaltet die an den PINs 2 und 3 hängen.
Dann gibt es noch einen Servo für das Ruder, der an PIN 9 hängt.

Das ganze funktioniert soweit gut (Schiff schwimmt und lässt sich wie gewünscht steuern, Funkreichweite getestet auf 30m).

Ich wollte das Schiff jetzt um einen QMC5883L und ein NEO6M GPS erweitern. Ziel ist, dass es bei einem Abriss der Funkverbindung selbstständig zu einem festgelegten Punkt zurückfährt.
Der Kompass ist über I2C verbunden. Für den Kompass verwende ich folgende Bibliothek:
https://github.com/dthain/QMC5883L
Für die Verbindung wird die Wire.h benötigt und diese oder die QMC5883L.h scheinen sich nicht mit der Servo.h zu vertragen.
In dem nachfolgenden Code habe ich die Wire.h und den Kompass eingebunden. Die Werte des Kompass lasse ich mir testweise einfach an den PC ausgeben (Ganz am Ende vom Sketch). Das Problem ist, dass der Servo permanent im Rhythmus der Datenabfrage an den Kompass minimal zuckt. Das Verhalten gleicht dem, wenn man am Leonardo Softserial.h und Servo.h zusammen benutzt und beide auf dem gleichen Timer liegen.
Ich finde die Ursache für das Problem nicht.
Ich habe den Servo testweise von PIN9 auf PIN7 verlegt. Das hat aber keine Veränderung gebracht.

Ich habe für den Kompass eine andere Bibliothek genommen (https://github.com/mprograms/QMC5883LCompass). Keine Veränderung.

Ich habe anstelle des Kompass mal ein Display über I2C angeschlossen. Dies führte ebenfalls dazu, dass der Servo gezuckt hat. Daher vermute ich, dass die Wire.h die Ursache des Problems ist.

Ich habe testweise mal die Servotimer2.h getestet. Ohne die Wire.h und die QMC5883L.h funktioniert der Servo einwandfrei. Mit den beiden Bibliotheken funktioniert weder der Servo noch der Kompass.

Ich sehe jetzt noch die Möglichkeit, dass ich irgend einen Fehler in der Platine habe, an die die ganzen Motoren etc. angeschlossen sind. Oder ich habe irgendwo Mist programmiert, den ich nicht finde. Bevor ich also die Elektronik wieder auseinander nehme wollte ich erst einmal diese Spur verfolgen.

Oder ist es so, dass die genannten Bibliotheken einfach nicht zusammen funktionieren?

Ich bitte den etwas unübersichtlichen Code zu entschuldigen. Das ist alles gerade noch sehr in Arbeit :confused: .

#include <Servo.h>
#include <dcmotor.h>
#include <QMC5883L.h>
#include <Wire.h>


dcmotor motor1 (5,6);
//SoftwareSerial blue(11, 10); // RX, TX
//Ergebnisse: 0=68
//Voller Ausschlag nach rechts = 17
//Voller Ausschlag nach links= 112
//Motor langsam = 110
//Motor schnell = 160


//Kompass einbinden
QMC5883L compass;

float round_to_dp( float in_value, int decimal_place )
{
  float multiplier = powf( 10.0f, decimal_place );
  in_value = roundf( in_value * multiplier ) / multiplier;
  return in_value;
}

int float2int(float in_value) {
  return (int) (in_value / 1);
}

int heading = 0;
int heading_last=0;
//Kompass Ende


const int serial_buffer_size = 30;
const int MAX_VALUES = 9;

char serialBuffer[serial_buffer_size];
int values[MAX_VALUES];

const int SMOOTH_FACTOR = 4;
//Servopositionen
int spos = 68;
int sposlast;
int dcspeed = 0;
int movemode = 0;

int bowthruster_to_l = 8;
int bowthruster_to_r = 4;

int light1_on = 3;
int light2_on = 2;

bool bowthruster_to_l_val;
bool bowthruster_to_r_val;

int light_on_val;

bool getvoltage = 0;

int getident = 0;
int ident = 9271;

bool lifebit = 0;
bool nolifebit = 0;
unsigned long currentlifebittimer = 0;
unsigned long startlifebittimer = 0;
unsigned long lifebittimer = 10000;

#define BLINKON 2000
#define BLINKOFF 1000

unsigned long currentBLINK = millis();

int analogInput1 = A0;
int values1 = 0;
int analogInput2 = A1;
int values2 = 0;

String tosend = "";
int targetident = 9272;

Servo Servo1;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Servo1.attach(7);
  Serial1.begin(9600);
 //Kompass starten
  Wire.begin();
  compass.init();
  compass.setSamplingRate(10);

  pinMode(bowthruster_to_l, OUTPUT);
  pinMode(bowthruster_to_r, OUTPUT); 

  pinMode(light1_on, OUTPUT);
  pinMode(light2_on, OUTPUT);

  pinMode(analogInput1, INPUT); 
  pinMode(analogInput2, INPUT);  
}

  //Funktionen
bool readSerial(Stream& stream)
{
  static byte index;

  while (stream.available() > 0)
  {
    char c = stream.read();

    if (c >= 32 && index < serial_buffer_size - 1)
    {
      serialBuffer[index++] = c;
    }
    else if (c == '\n' && index > 0)
    {
      serialBuffer[index] = '\0';
      index = 0;
      return true;
    }
  }
  return false;
}

void parseSerial()
{
  char* ptr = strtok(serialBuffer, ",;");
  int index = 0;

  while (ptr != NULL && index < MAX_VALUES)
  {
    values[index++] = atoi(ptr);
    ptr = strtok(NULL, ",;");
  }
}

void loop() {
//Funkverbindung abfragen
 if (readSerial(Serial1) == true)
  {
    parseSerial();
 
    for (int i = 0; i < MAX_VALUES; i++){
      Serial.println(values[i]);
    Serial.println("---");}
    movemode = values[0];
    dcspeed = values[1];
    spos = values[2];
    bowthruster_to_l_val = values[3];
    bowthruster_to_r_val = values[4];
    light_on_val = values[5];
    getvoltage = values[6];
    lifebit = values[7];
    getident = values[8];

    Serial.print("movemode=");
    Serial.println(movemode);
    Serial.print("spos=");
    Serial.println(spos);
    Serial.print("dcspeed=");
    Serial.println(dcspeed);
    Serial.print("bowthruster_to_l_val=");
    Serial.println(bowthruster_to_l_val);
    Serial.print("bowthruster_to_r_val=");
    Serial.println(bowthruster_to_r_val);
    Serial.print("getvoltage=");
    Serial.println(getvoltage);
    Serial.print("lifebit=");
    Serial.println(lifebit);
    Serial.print("getident=");
    Serial.println(getident);
    Serial.println("---");
  }

  //Prüfen ob Verbindung abgerissen
  currentlifebittimer = millis(); 
  if (lifebit==1){
    startlifebittimer = currentlifebittimer;
    lifebit = 0;
  }

  if (currentlifebittimer - startlifebittimer >= lifebittimer){
    movemode = 0;
    spos = 68;
    bowthruster_to_l_val = 0;
    bowthruster_to_r_val = 0;
    nolifebit = 1;
    light_on_val = 3;
  }

  //Ansteuern der Motoren und Beleuchtung
  if (getident == ident){
    if(spos != sposlast){
      Servo1.write(spos);
      sposlast = spos;
      Serial.println("Servopos");
    }
  
    switch (movemode) {
      case 1: //w
        motor1.forwards(dcspeed);
      break;
      case 2: //s
        motor1.backwards(dcspeed);
      break;
      case 0: //e
        motor1.off();
       break;
    }
  
    if (bowthruster_to_l_val) {
      digitalWrite(bowthruster_to_l, HIGH);
    }else{
      digitalWrite(bowthruster_to_l, LOW);
    }
  
    if (bowthruster_to_r_val) {
      digitalWrite(bowthruster_to_r, HIGH);
    }else{
      digitalWrite(bowthruster_to_r, LOW);
    }
  
    switch (light_on_val) {
      case 0:
        digitalWrite(light1_on, LOW);
        digitalWrite(light2_on, LOW);
      break;
      case 1:
        digitalWrite(light1_on, HIGH);
      break;
      case 2:
        digitalWrite(light1_on, HIGH);
        digitalWrite(light2_on, HIGH);
      break;
      case 3:
        if (millis() - currentBLINK >= BLINKON){
         digitalWrite(light1_on, HIGH); 
         currentBLINK = millis();
        }
        if (millis() - currentBLINK >= BLINKOFF){
          digitalWrite(light1_on, LOW);
        }
      break;
    }
    //Abfrage Batteriespannung
    if (getvoltage == 1){
      values1 = analogRead(analogInput1);
      values2 = analogRead(analogInput2); 
      tosend = String(values1) + String(",") + String(values2) + String(",") + String(targetident);
      Serial.println(tosend);
      Serial1.print(tosend);
      Serial1.println();
      getvoltage = 0;
    }
    
  }

//Kompass abfragen
if(compass.ready()) {
   heading = float2int(round_to_dp(compass.readHeading(),0));
  }

 
if (heading != heading_last){
    Serial.println(heading);
    heading_last = heading;
}
}

Danke für eure Hilfe und einen schönen Abend!

Hallo,

I2C ist auf dem Mega eine Hardware Implementation, da sollten auch mit der Wire.h keine Probleme auftreten.

Ich vermute eher ein Problem mit der Spannungsversorgung.
Wie hast du denn die Spannung der Servos angeschlossen ?

Oder du fragst deinen I2C-Bus zu oft ab, so dass deine Servos ständig blockiert werden.

Da dein Sketch keine Kommentare enthält, habe ich mir den nicht weiter angesehen.

Hallo,

danke für den Tipp. Es handelt sich um einen 5V Servo der an einem L7805 hängt. Der L7805 ist an einen 8,5V Akku angeschlossen. Dieser versorgt den L7805 und den Mega.
Ich werde die Häufigkeit der Abfrage anpassen und sehen ob das hilft.

Oder du fragst deinen I2C-Bus zu oft ab, so dass deine Servos ständig blockiert werden.

Das war das Problem.