ESP32, GYUS42-Sensor, PCA9685

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

#define SERVO_PIN 0 // Der Kanal des PCA9685, an den der Servo angeschlossen ist

#define SensorAddresse 0x70

#define Reichweitenbefehl 0x51

#define PCA9685_SDA_PIN 21

#define PCA9685_SCL_PIN 22

#define SENSOR_SDA_PIN 15

#define SENSOR_SCL_PIN 4

TwoWire pca9685Wire = TwoWire(0);

TwoWire sensorWire = TwoWire(1); 

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

unsigned long servoDelay = 15;

int minWinkel = 60;

int maxWinkel = 200;

int minServoimpulsBreite = 150;  

int maxServoimpulsBreite = 600; 

void setup() {

  Serial.begin(115200); 

  while (!Serial); 

  pca9685Wire.begin(PCA9685_SDA_PIN, PCA9685_SCL_PIN);

  pwm.begin();

  pwm.setPWMFreq(60); 

  sensorWire.begin(SENSOR_SDA_PIN, SENSOR_SCL_PIN); 

}

void loop() {

  for (int winkel = minWinkel; winkel <= maxWinkel; winkel++) {

    int pulsBreite = map(winkel, 0, 180, minServoimpulsBreite, maxServoimpulsBreite); 

    pwm.setPWM(SERVO_PIN, 0, pulsBreite); 

    delay(servoDelay);

    Wire.beginTransmission(SensorAddresse);

    Wire.write(Reichweitenbefehl);

    Wire.endTransmission();

    delay(65);

    Wire.requestFrom(SensorAddresse, 2);

    if (Wire.available() >= 2) {

      byte highByte = Wire.read();

      byte lowByte = Wire.read();

      int distanz = highByte * 256 + lowByte;

      // Verarbeite die Distanz

      Serial.print("Winkel: ");

      Serial.print(winkel);

      Serial.print(" Grad, Entfernung: ");

      Serial.print(distanz);

      Serial.println(" cm");

    }

  }

}