000
22.09.2026, 23:55 Uhr
TobyB
|
Ich möchte euch mal was zeigen...
ich hab mal ein wenig in Hard und Software gebastelt..
siehe hier:
https://qlb-harz.de/tbqlb/Video/segway.MOV
Den Code habe ich für Interessenten hier mal rein kopiert...
Für mich war das nen cooles Projekt, hat Spaß gemacht darum teile ich es hier gern.
LG Tobias
https://qlb-harz.de/Z80/
// =======================================================
// 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
// ======================================================= |
