Problem mit ToF-Sensor, Servo, DC-Motoren, ADAFRUIT Motorshield

Hallo Leute,
ich habe ein Problem bei meinem aktuellen Projekt und hoffe, jemand kann mir helfen!
Ich baue eine Wagen, der Drei Räder hat (Zwei motorisierte Heckräder, ein bewegliches “Bug-Rad”) und der sich autonom fortbewegen soll. Anstatt mehrere Sensoren einzusetzen, wollte ich das ganze mit einem einzigen ToF-Sensor umsetzen. Dafür habe ih diesen auf einen Servo gesetzt, der sich am Bug des Gefährts befindet und je 95 Grad nach links und 95 Grad nach recht dreht, während der Sensor die Abstände misst. Ist ein gemessener Abstand nun kleiner als ein vorgegebener kritischer Wert (200mm), während der Servo gerade nach rechts ausgerichtet ist, weicht der Wagen nach links aus und so weiter (gleiches System für alle Richtungen, auch vorne und hinten).

Nun zu meinem Problem: An sich funktioniert alles, nur an einer Stelle gibt es einen essentiellen Fehler:
Wenn man die Motoren aus dem loop nimmt, misst der ToF fleißig die Abstände. Befinden sich allerdings die Anweisungen für den motor auch im Loop, mist der ToF nur noch den ersten Abstand und gibt anschließend nur noch den ersten Abstand wieder.

Dadurch wird der gesamte Wagen unbrauchbar. Da ich trotz intensiver Recherche keine Hinweise finden konnte, frage ich nun hier, und hoffe, ihr könnt mir helfen!
Mit freundlichen Grüßen
Max

Code:

#include "Adafruit_VL53L0X.h"
#include <AFMotor.h>
#include <Servo.h>
#include <math.h>

int barrierDist = 200;

AF_DCMotor motor1(1);
AF_DCMotor motor4(4);
Servo name_servo;

int servo_position = 0;
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

void setup()
{

   motor1.setSpeed(0);
   motor4.setSpeed(0);

   Serial.begin(115200);
   name_servo.attach(10);

   while (!Serial) {
       delay(1);
   }

   Serial.println("Adafruit VL53L0X test");
   if (!lox.begin()) {
       Serial.println(F("Failed to boot VL53L0X"));
       while (1)
           ;
   }
   // power
   Serial.println(F("VL53L0X API Simple Ranging example\n\n"));
}

int getMsrmnt()
{
   VL53L0X_RangingMeasurementData_t measure;
   lox.rangingTest(&measure, false);
   return measure.RangeMilliMeter;
}

// driveDir between [-1; 1] // -1 = go left; 1 = go right; 0 = go straight forward
void driveCar(int velocity, float driveDir)
{
   if (driveDir == 0) {
       motor1.setSpeed((int)abs(velocity));
       motor4.setSpeed((int)abs(velocity));
   }
   else {

       motor1.setSpeed((int)abs(velocity * max(0, -driveDir)));
       motor4.setSpeed((int)abs(velocity * max(0, driveDir)));
   }
   if (velocity > 0) {
       motor1.run(BACKWARD);
       motor4.run(BACKWARD);
   }
   else if (velocity < 0) {
       motor1.run(FORWARD);
       motor4.run(FORWARD);
   }
   else {
       motor1.run(RELEASE);
       motor4.run(RELEASE);
   }
}

void outputMsrmnt()
{
   int distance = getMsrmnt();

   Serial.print("Reading a measurement... ");

   Serial.print("Distance (mm): ");
   Serial.println(distance);
}

void loop()
{

   int rightMsrmnt = getMsrmnt();

   Serial.print("Right Msrmnt (mm): ");
   Serial.print(rightMsrmnt);
   Serial.println("");

   for (servo_position = 0; servo_position <= 180; servo_position += 8) {

       name_servo.write(servo_position);

       //  outputMsrmnt();

       delay(10);
   }

   int leftMsrmnt = getMsrmnt();

   Serial.print("Left Msrmnt (mm): ");
   Serial.print(leftMsrmnt);
   Serial.println("");

   for (servo_position = 180; servo_position >= 0; servo_position -= 8) {

       name_servo.write(servo_position);

       //   outputMsrmnt();

       delay(10);
   }

   if (leftMsrmnt < barrierDist && rightMsrmnt >= barrierDist) {
       driveCar(250, 1);
   }
   else if (leftMsrmnt >= barrierDist && rightMsrmnt < barrierDist) {
       driveCar(250, -1);
   }
   else if (leftMsrmnt >= barrierDist && rightMsrmnt >= barrierDist) {
       driveCar(250, 0);
   }
   else {
       driveCar(-250, 1);
   } 
   delay(100);
}

projekt_1.ino (2.56 KB)

Setze Deinen Code bitte in Codetags (</>-Button oben links im Forumseditor oder [code] davor und [/code] dahinter ohne *).
Das kannst Du auch noch nachträglich ändern.

Gruß Tommy

Ist erledigt, danke!
Mit freundlichen Grüßen
Max

Ich glaube nicht, dass das Crossposting einen wesentlichen Erkenntnisgewinn bringt.

Gruß Tommy