🤝 Aufgabe: Servos mit MPU6050 koppeln

Zum Schluss reagieren die Servos auf die Neigung deines Shuttles. Du nutzt den Winkel Pitch (Y), um die Elevons gegensinnig zu bewegen.

---

🧠 Ziel

• MPU6050-Winkel auslesen (getAngleY())
• Winkel sinnvoll in Servo-Winkel (45°–135°) abbilden

---

⚙️ Konstanten

#include <Servo.h>
#include <MPU6050_tockn.h>

const uint8_t SERVO_LEFT_PIN  = 9;
const uint8_t SERVO_RIGHT_PIN = 10;

---

🧪 Code

#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <MPU6050_tockn.h>

const uint8_t SERVO_LEFT_PIN  = 9;
const uint8_t SERVO_RIGHT_PIN = 10;

Servo servoLeft, servoRight;
MPU6050 mpu(Wire);

int mapFloatToInt(float x, float in_min, float in_max, int out_min, int out_max) {
  if (x < in_min) x = in_min;
  if (x > in_max) x = in_max;
  return (int)((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.begin();
  mpu.calcGyroOffsets(true);

  servoLeft.attach(SERVO_LEFT_PIN);
  servoRight.attach(SERVO_RIGHT_PIN);

  servoLeft.write(90);
  servoRight.write(90);
}

void loop() {
  mpu.update();

  float angleY = mpu.getAngleY();                // -∞ … +∞ (typisch ~ -90 … +90)
  int pos = mapFloatToInt(angleY, -45, 45, 45, 135); // mappe Pitch auf 45°…135°

  servoLeft.write(pos);
  servoRight.write(180 - pos);                   // gegensinnig

  Serial.print("Pitch(Y): "); Serial.print(angleY);
  Serial.print("  | ServoPos: "); Serial.println(pos);

  delay(20); // sanftes Update
}

---

✅ Checkliste

• Servos reagieren sichtbar auf Neigung (Y-Achse)
• Grenzen werden eingehalten (45°–135°)
• Serial Monitor zeigt Winkel + Servoposition