mini segway ESP32 feasibility study

// =======================================================

//  ESP32-S3 Segway-Balancer  v4  — Telemetrie-kalibriert

// =======================================================

//  Korrekturen gegenüber v3 (basierend auf Feldtest-Telemetrie):

//

//  FIX 1: setpoint = -2.5°

//    Telemetrie: pitch konstant -0.7° bis -3.4°, Mittel ~-2.5°.

//    Mechanischer Balancepunkt ≠ 0°. Ohne Korrektur läuft I-Term

//    immer voll auf → Wegrollen garantiert.

//

//  FIX 2: Kp 18→10, Kd 0.70→2.5

//    Kp=18 → bei 2.5° Fehler cmd=45 → starkes Übersteuern mit Getriebespiel.

//    Kd=0.70 dämpfte rate-Sprünge von ±22°/s nicht ausreichend.

//

//  FIX 3: Ki 15→3, iLimit 120→35

//    Ki=15: I-Term lief in 4s von -11 auf +25 unkontrolliert.

//    Ki=3 + iLimit=35 gibt Zeit zum Einpendeln ohne zu dominieren.

//

//  FIX 4: CMD_DEADBAND 8→5

//    Zu groß: kleine Korrekturen wurden verschluckt → ruckartige Reaktion.

//

//  FIX 5: PULSE_MIN_US 250→400

//    Kleinere Pulse überbrücken Getriebespiel nicht zuverlässig.

//

//  WICHTIG beim ersten Start:

//   - Roboter beim Booten RUHIG & AUFRECHT halten (Kalibrierung)

//   - Fährt er in die falsche Richtung → INVERT_MOTORS auf 1

//   - Reagiert er "verkehrt herum" → INVERT_ANGLE auf 1

//   - setpoint live mit "s-2.0", "s-3.0" etc. feinabstimmen

// =======================================================

#include <Arduino.h>

#include <Wire.h>

#include <Adafruit_NeoPixel.h>

// -------------------------------------------------------

// Pin-Belegung

// -------------------------------------------------------

#define SDA_PIN         8

#define SCL_PIN         9

#define NEOPIXEL_PIN    21

#define NEOPIXEL_COUNT  1

#define MPU_ADDR        0x68

#define L_IN1           4

#define L_IN2           5

#define R_IN1           6

#define R_IN2           7

// -------------------------------------------------------

// Vorzeichen-Korrekturen (bei Bedarf auf 1 setzen)

// -------------------------------------------------------

#define INVERT_ANGLE    0   // 1 = Neigungs-Vorzeichen drehen

#define INVERT_MOTORS   1   // Feldtest: Motoren fuhren in Sturzrichtung → gedreht

#define INVERT_LEFT     0   // 1 = nur linken Motor drehen

#define INVERT_RIGHT    0   // 1 = nur rechten Motor drehen

// -------------------------------------------------------

// Regel-Timing

// -------------------------------------------------------

#define CONTROL_HZ      600

const unsigned long CONTROL_PERIOD_US = 1000000UL / CONTROL_HZ;

// -------------------------------------------------------

// Puls-Parameter (Hardware-Timer-ISR)

//   ISR-Takt = 100 µs (10 kHz)

//   Eine Pulsperiode = PULSE_PERIOD_US (z.B. 3000 µs ≈ 333 Hz)

//   Innerhalb der Periode wird für "pulseWidth" angetrieben,

//   danach Coast (Freilauf) bis zur nächsten Periode.

// -------------------------------------------------------

#define ISR_TICK_US        100

#define PULSE_PERIOD_US    3000          // 333 Hz Pulsrate

#define PULSE_MIN_US       400           // Mindest-Kick (Getriebespiel überwinden, Feldtest: 250 zu klein)

#define PULSE_MAX_US       PULSE_PERIOD_US  // bei cmd=255 → Dauerstrich

const int PERIOD_TICKS  = PULSE_PERIOD_US / ISR_TICK_US;   // 30

const int MIN_TICKS     = PULSE_MIN_US    / ISR_TICK_US;   // 2-3

const int MAX_TICKS     = PULSE_MAX_US    / ISR_TICK_US;   // 30

// Hysterese-Totzone: verhindert schnelles Richtungswechseln bei Getriebespiel

// Motor bleibt aktiv bis cmd unter DEADBAND_OFF fällt (nicht erst bei 0)

// Motor startet erst wenn cmd über DEADBAND_ON steigt

#define CMD_DEADBAND_ON   6.0f    // Einschaltschwelle

#define CMD_DEADBAND_OFF  3.0f    // Ausschaltschwelle (Hysterese)

// -------------------------------------------------------

// Filter / Regler-Parameter  (STARTWERTE — live tunen!)

// -------------------------------------------------------

float alpha = 0.98f;   // Komplementärfilter: Gyro-Gewichtung (0.97–0.99)

// PID — Fehler & Winkel in GRAD, Stellgröße in 0..255

// Werte aus Feldtest-Telemetrie abgeleitet:

float Kp = 38.0f;     // erhöht: mehr Masse braucht frühere/stärkere Reaktion

float Kd =  1.2f;     // leicht erhöht: mehr Dämpfung gegen Trägheitspendeln

float Ki =  2.0f;     // unverändert

// Telemetrie: pitch pendelte präzise um -1.8°

float setpoint   = -1.8f;    // Grad — live mit "s" in 0.2°-Schritten feinabstimmen

const float iLimit     = 35.0f;    // war 120 → viel zu groß, dominierte alles

const float tipLimit   = 30.0f;    // Grad: Notabschaltung

const float reArmLimit = 3.0f;     // Grad: Re-Arm-Fenster

// -------------------------------------------------------

// Zustand

// -------------------------------------------------------

float pitch        = 0.0f;   // gefilterter Winkel (Grad)

float gyroRateDeg  = 0.0f;   // Winkelgeschwindigkeit (Grad/s)

float gyroBiasDeg  = 0.0f;   // Gyro-Bias (Grad/s)

float accPitchOff  = 0.0f;   // Accel-Nullpunkt (Grad)

float iTerm        = 0.0f;   // Integralspeicher

unsigned long lastControlUs = 0;

unsigned long lastTeleMs     = 0;

bool  telemetry = true;

// Arm-Logik

bool  armed = false;

unsigned long stableSinceMs = 0;

// ---- ISR-geteilte Variablen (volatile!) ----

volatile int  pulseTicksL = 0;   // 0..PERIOD_TICKS  (0 = Coast)

volatile int  pulseTicksR = 0;

volatile int8_t dirL = 0;        // -1, 0, +1

volatile int8_t dirR = 0;

volatile bool isrArmed = false;

Adafruit_NeoPixel pixel(NEOPIXEL_COUNT, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);

hw_timer_t *pulseTimer = NULL;

// =======================================================

// Pulse-Timer-ISR (10 kHz) — nur Pins schalten, keine Mathematik!

// =======================================================

void ARDUINO_ISR_ATTR onPulseTimer() {

  static int counter = 0;

  counter++;

  if (counter >= PERIOD_TICKS) counter = 0;

  // --- Linker Motor ---

  if (!isrArmed || dirL == 0 || pulseTicksL <= 0) {

    digitalWrite(L_IN1, LOW); digitalWrite(L_IN2, LOW);          // Coast

  } else if (counter < pulseTicksL) {

    if (dirL > 0) { digitalWrite(L_IN1, HIGH); digitalWrite(L_IN2, LOW); }

    else          { digitalWrite(L_IN1, LOW);  digitalWrite(L_IN2, HIGH); }

  } else {

    digitalWrite(L_IN1, LOW); digitalWrite(L_IN2, LOW);          // Coast (Pause)

  }

  // --- Rechter Motor ---

  if (!isrArmed || dirR == 0 || pulseTicksR <= 0) {

    digitalWrite(R_IN1, LOW); digitalWrite(R_IN2, LOW);

  } else if (counter < pulseTicksR) {

    if (dirR > 0) { digitalWrite(R_IN1, HIGH); digitalWrite(R_IN2, LOW); }

    else          { digitalWrite(R_IN1, LOW);  digitalWrite(R_IN2, HIGH); }

  } else {

    digitalWrite(R_IN1, LOW); digitalWrite(R_IN2, LOW);

  }

}

// =======================================================

// Stellgröße → Puls-Parameter (im Regelloop berechnet)

//   cmd: -255 .. +255

// =======================================================

void setMotorCommand(float cmdLeft, float cmdRight) {

#if INVERT_MOTORS

  cmdLeft = -cmdLeft;  cmdRight = -cmdRight;

#endif

#if INVERT_LEFT

  cmdLeft  = -cmdLeft;

#endif

#if INVERT_RIGHT

  cmdRight = -cmdRight;

#endif

  auto compute = [](float cmd, volatile int8_t &dir, volatile int &ticks) {

    static float lastCmd = 0;

    float a = fabs(cmd);

    float threshold = (fabs(lastCmd) > CMD_DEADBAND_OFF) ? CMD_DEADBAND_OFF : CMD_DEADBAND_ON;

    if (a < threshold) {

      dir = 0; ticks = 0; lastCmd = 0; return;

    }

    lastCmd = cmd;

    dir = (cmd > 0) ? 1 : -1;

    float ratio = (a - CMD_DEADBAND_ON) / (255.0f - CMD_DEADBAND_ON);

    ratio = constrain(ratio, 0.0f, 1.0f);

    ticks = MIN_TICKS + (int)(ratio * (MAX_TICKS - MIN_TICKS));

  };

  compute(cmdLeft,  dirL, pulseTicksL);

  compute(cmdRight, dirR, pulseTicksR);

}

// =======================================================

// I2C / MPU6050

// =======================================================

void i2cWrite(uint8_t reg, uint8_t val) {

  Wire.beginTransmission(MPU_ADDR);

  Wire.write(reg); Wire.write(val);

  Wire.endTransmission();

}

void readMPU(float &ax, float &ay, float &az, float &gyDeg) {

  Wire.beginTransmission(MPU_ADDR);

  Wire.write(0x3B);

  Wire.endTransmission(false);

  Wire.requestFrom(MPU_ADDR, 14, true);

  int16_t axr = (Wire.read() << 8) | Wire.read();

  int16_t ayr = (Wire.read() << 8) | Wire.read();

  int16_t azr = (Wire.read() << 8) | Wire.read();

  Wire.read(); Wire.read();                 // Temp

  Wire.read(); Wire.read();                 // gx (ungenutzt)

  int16_t gyr = (Wire.read() << 8) | Wire.read();

  Wire.read(); Wire.read();                 // gz (ungenutzt)

  ax = axr / 16384.0f;

  ay = ayr / 16384.0f;

  az = azr / 16384.0f;

  gyDeg = gyr / 16.4f;                       // ±2000°/s → Grad/s

}

// =======================================================

// Setup

// =======================================================

void setup() {

  Serial.begin(115200);

  delay(300);

  Serial.println("\n=== Balancer v4 (Telemetrie-kalibriert) ===");

  pinMode(L_IN1, OUTPUT); pinMode(L_IN2, OUTPUT);

  pinMode(R_IN1, OUTPUT); pinMode(R_IN2, OUTPUT);

  digitalWrite(L_IN1, LOW); digitalWrite(L_IN2, LOW);

  digitalWrite(R_IN1, LOW); digitalWrite(R_IN2, LOW);

  Wire.begin(SDA_PIN, SCL_PIN, 400000);

  i2cWrite(0x6B, 0x00);   // Sleep aus

  i2cWrite(0x1B, 0x18);   // Gyro ±2000°/s

  i2cWrite(0x1C, 0x00);   // Acc ±2g

  i2cWrite(0x1A, 0x02);   // DLPF 94 Hz

  pixel.begin();

  pixel.setPixelColor(0, pixel.Color(80,80,0)); pixel.show();   // Gelb

  // ---- Gyro-Bias (Roboter ruhig halten) ----

  Serial.println("Kalibriere Gyro-Bias...");

  const int N = 2000;

  double s = 0;

  for (int i=0;i<N;i++){ float ax,ay,az,gy; readMPU(ax,ay,az,gy); s+=gy; delayMicroseconds(700);}

  gyroBiasDeg = (float)(s/N);

  Serial.printf("Gyro-Bias: %.4f Grad/s\n", gyroBiasDeg);

  // ---- Accel-Nullpunkt (Roboter AUFRECHT halten) ----

  Serial.println("Kalibriere Nullpunkt (aufrecht halten!)...");

  s = 0;

  for (int i=0;i<400;i++){

    float ax,ay,az,gy; readMPU(ax,ay,az,gy);

    s += atan2f(-ax, sqrtf(ay*ay+az*az)) * 180.0f/PI;

    delay(4);

  }

  accPitchOff = (float)(s/400.0);

  Serial.printf("Nullpunkt: %.3f Grad\n", accPitchOff);

  // Filter initialisieren (kein Startsprung)

  {

    float ax,ay,az,gy; readMPU(ax,ay,az,gy);

    pitch = atan2f(-ax, sqrtf(ay*ay+az*az)) * 180.0f/PI - accPitchOff;

#if INVERT_ANGLE

    pitch = -pitch;

#endif

  }

  // ---- Hardware-Timer für Pulsansteuerung ----

  pulseTimer = timerBegin(1000000);              // 1 MHz → 1 Tick = 1 µs

  timerAttachInterrupt(pulseTimer, &onPulseTimer);

  timerAlarm(pulseTimer, ISR_TICK_US, true, 0);  // alle 100 µs

  pixel.setPixelColor(0, pixel.Color(0,80,0)); pixel.show();   // Grün

  Serial.println("Bereit. Befehle: p/i/d/a/s <wert>, m=arm-toggle, t=telemetrie, r=reset-I, ?=status");

  lastControlUs = micros();

}

// =======================================================

// Serielle Befehle (Live-Tuning)

// =======================================================

void handleSerial() {

  static char buf[24];

  static uint8_t idx = 0;

  while (Serial.available()) {

    char c = Serial.read();

    if (c=='\n' || c=='\r') {

      if (idx==0) return;

      buf[idx] = 0; idx = 0;

      char cmd = buf[0];

      float val = atof(buf+1);

      switch (cmd) {

        case 'p': Kp = val; Serial.printf(">> Kp=%.2f\n", Kp); break;

        case 'i': Ki = val; iTerm=0; Serial.printf(">> Ki=%.2f (I reset)\n", Ki); break;

        case 'd': Kd = val; Serial.printf(">> Kd=%.3f\n", Kd); break;

        case 'a': alpha = constrain(val,0.90f,0.999f); Serial.printf(">> alpha=%.3f\n", alpha); break;

        case 's': setpoint = val; Serial.printf(">> setpoint=%.2f Grad\n", setpoint); break;

        case 'r': iTerm = 0; Serial.println(">> I-Term reset"); break;

        case 't': telemetry = !telemetry; Serial.printf(">> telemetry=%d\n", telemetry); break;

        case 'm': armed = !armed; if(!armed){iTerm=0;} Serial.printf(">> armed=%d\n", armed); break;

        case '?':

          Serial.printf(">> Kp=%.2f Ki=%.2f Kd=%.3f alpha=%.3f setpoint=%.2f armed=%d\n",

                        Kp,Ki,Kd,alpha,setpoint,armed);

          break;

        default: Serial.println(">> ? unbekannt"); break;

      }

    } else if (idx < sizeof(buf)-1) {

      buf[idx++] = c;

    }

  }

}

// =======================================================

// Hauptschleife

// =======================================================

void loop() {

  handleSerial();

  unsigned long now = micros();

  if (now - lastControlUs < CONTROL_PERIOD_US) return;   // nicht-blockierendes Pacing

  // Echtes dt messen (robust gegen Jitter), begrenzen

  float dt = (now - lastControlUs) / 1e6f;

  if (dt > 0.01f) dt = 0.01f;            // nach Stall nicht explodieren

  lastControlUs = now;

  // ---- Sensorik ----

  float ax, ay, az, gyDeg;

  readMPU(ax, ay, az, gyDeg);

  gyroRateDeg = gyDeg - gyroBiasDeg;

  float pitchAcc = atan2f(-ax, sqrtf(ay*ay+az*az)) * 180.0f/PI - accPitchOff;

#if INVERT_ANGLE

  pitchAcc    = -pitchAcc;

  gyroRateDeg = -gyroRateDeg;

#endif

  // ---- Komplementärfilter (KORREKT: Ausgang rückgekoppelt → driftfrei) ----

  pitch = alpha * (pitch + gyroRateDeg * dt) + (1.0f - alpha) * pitchAcc;

  // ---- Arm-Logik ----

  if (armed && fabs(pitch) > tipLimit) {      // umgekippt → abschalten

    armed = false; iTerm = 0;

  }

  if (!armed) {

    isrArmed = false;

    setMotorCommand(0, 0);

    // Auto-Re-Arm wenn aufrecht & ruhig gehalten

    if (fabs(pitch) < reArmLimit && fabs(gyroRateDeg) < 30.0f) {

      if (stableSinceMs == 0) stableSinceMs = millis();

      if (millis() - stableSinceMs > 400) { armed = true; iTerm = 0; }

    } else {

      stableSinceMs = 0;

    }

    pixel.setPixelColor(0, pixel.Color(80,0,0)); pixel.show();  // Rot = disarmed

    if (telemetry && millis()-lastTeleMs > 50) {

      lastTeleMs = millis();

      Serial.printf("DISARM pitch:%.2f rate:%.1f (aufrecht halten zum Re-Arm)\n", pitch, gyroRateDeg);

    }

    return;

  }

  isrArmed = true;

  // ---- PID-Regler (Grad) ----

  float e = setpoint - pitch;

  float P = Kp * e;

  iTerm += Ki * e * dt;

  iTerm = constrain(iTerm, -iLimit, iLimit);

  float D = Kd * gyroRateDeg;          // Derivative on Measurement (Gyrorate)

  float u = P + iTerm - D;

  float cmd = constrain(u, -255.0f, 255.0f);

  setMotorCommand(cmd, cmd);

  // ---- Status-LED ----

  if (fabs(pitch) > tipLimit*0.6f)

    pixel.setPixelColor(0, pixel.Color(80,40,0));   // Orange

  else

    pixel.setPixelColor(0, pixel.Color(0,80,0));    // Grün

  pixel.show();

  // ---- Telemetrie (~20 Hz, copy-paste-freundlich & parsebar) ----

  if (telemetry && millis() - lastTeleMs > 50) {

    lastTeleMs = millis();

    Serial.printf("pitch:%.2f rate:%.1f e:%.2f P:%.1f I:%.1f D:%.1f u:%.0f cmd:%.0f kp:%.1f ki:%.1f kd:%.2f a:%.3f dt:%lu\n",

                  pitch, gyroRateDeg, e, P, iTerm, D, u, cmd,

                  Kp, Ki, Kd, alpha, (unsigned long)(dt*1e6));

  }

}

// =======================================================

//  TUNING-WORKFLOW (Stand v4 — Startwerte aus Telemetrie)

// =======================================================

//

//  setpoint = -2.5° ist der aus Telemetrie gemessene Balancepunkt.

//  Falls er nach dem Flash noch systematisch rollt:

//    vorwärts rollt → "s-3.0" oder "s-3.5" probieren

//    rückwärts rollt → "s-2.0" oder "s-1.5" probieren

//  In 0.5°-Schritten tasten bis er nahezu steht.

//

//  PID-Feintuning danach:

//   Schwingt er (regelmäßiges Vor-Rück)?  → Kp runter ("p8") oder Kd hoch ("d3.0")

//   Zittert er hochfrequent?              → Kd runter ("d2.0")

//   Pendelt er langsam in eine Richtung?  → Ki leicht hoch ("i4") oder setpoint trimmen

//   Reagiert er zu träge / fällt um?      → Kp hoch ("p12")

//

//  Serielle Befehle (kein Neuflashen nötig):

//   p10    → Kp setzen       d2.5   → Kd setzen

//   i3     → Ki setzen       a0.98  → alpha setzen

//   s-2.5  → setpoint        r      → I-Term reset

//   m      → arm/disarm      t      → telemetrie an/aus

//   ?      → aktuelle Werte

//

//  Telemetrie-Spalten:

//   pitch  = Ist-Winkel (Grad)    rate = Gyrorate (Grad/s)

//   e      = Fehler (setpoint-pitch)

//   P/I/D  = Regleranteile        u = Summe     cmd = an Motor (−255..255)

//   dt     = Loopzeit µs (sollte ~1666 sein)

//

//  Puls-Parameter (nur bei Bedarf, dann neu flashen):

//   PULSE_MIN_US  hoch wenn Motor bei kleinen Korrekturen nicht anspricht

//   PULSE_PERIOD_US runter für feinere Regelung, hoch bei H-Brücken-Wärme

// =======================================================

#define Code

#define Code