Hey zusammen,
Ich bin gerade an einem Projekt beschäftigt in dem ich einen selbst-balancierenden Roboter baue. Ich verwende dabei einen Arduino Nano, 2x Nema 17 Stepper Motor, 2x a4988 Treiber, Mpu6050. Hier der Code welcher ich verwende:
#include <MultiStepper.h>
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
const int enaPin1 = 4;
const int stepPin1 = 5; // Definiert den Pin 5 am Arduino welcher mit dem Steppin des 1 Motortreiber verbunden ist.
const int stepPin2 = 6; // Definiert den Pin 6 am Arduino welcher mit dem Steppin des 2 Motortreiber verbunden ist.
const int dirPin1 = 7; // Definiert den Pin 7 am Arduino welcher mit dem Dirpin des 1 Motortreiber verbunden ist.
const int dirPin2 = 8; // Definiert den Pin 8 am Arduino welcher mit dem Dirpin des 2 Motortreiber verbunden ist.
AccelStepper stepper1(AccelStepper::DRIVER,stepPin1, dirPin1); // Erstellt ein AccelStepper Objekt und konfiguriert es als Driver(Treiber) unter Verwendung der definierten Pins.
AccelStepper stepper2(AccelStepper::DRIVER,stepPin2, dirPin2); // Erstellt ein AccelStepper Objekt und konfiguriert es als Driver(Treiber) unter Verwendung der definierten Pins.
float setpoint = 34.4; // Definiert die Variabel setpoint(Sollwert) als 0 (Abweichung von der Gleichgewichtslage soll 0 Grad sein)
float Kp = 80.0; // Definiert die PID Reglerkonstante Kp als 40
float Ki = 0.0; // Definiert die PID Reglerkonstante Ki als 0.0
float Kd = 10.0; // Definiert die PID Reglerkonstante Kd als 3.0
float input, output, error; // Deklatiert Variabel für den PID Regler (Aktueller Winkel, PID-Ausgabewert, Aktueller Fehlerwert).
float lastError = 0; // Deklariert Variabel für Fehlerwert der vorherigen Iteration.
float integral = 0; // Deklariert Variabel für den Integralanteil des Fehlers.
unsigned long lastTime; // Deklariert eine Variabel für die Zeitmessung in millis(Millisekunden).
void setup() {
Serial.begin(115200); // Setzt die Baudrate (bits per second) für die serielle Datenübertragung fest.
Serial.println("Start initialize Mpu...");
Serial.println("Initialize Wire...");
Wire.begin(); // Initialisiert die Wire Library und die I2C-Kommunikation
Serial.println("Wire.h success");
mpu.initialize(); // Initialisiert den MPU6050
Serial.println("Mpu initialize success");
Serial.println("Starting connection to MPU6050...");
if (!mpu.testConnection()) { // Überprüft die Verbindung (bei "false" wird der Loop ausgeführt, bei "true" nicht) ("!" wird als Umkehrwert verwendet)
Serial.println("MPU6050 connection failed"); // Falls Verbindung fehlgeschlagen wird "MPU6050 connection failed" ausgegeben
while (1); // Falls Verbindung fehlgeschlagen bleibt das Programm in einer Endlosschlaufe stecken
}
Serial.println("MPU6050 connected successfully.");
pinMode(enaPin1, OUTPUT);
digitalWrite(enaPin1, LOW);
stepper1.setMaxSpeed(2000); // Setzt die maximale Geschwindigkeit auf 2000 (Schritte pro Sekunde)
stepper1.setAcceleration(1000); // Setzt die maximale Beschleunigung auf 1000 (Schritte pro Sekunde^2)
stepper2.setMaxSpeed(2000); // Setzt die maximale Geschwindigkeit auf 2000 (Schritte pro Sekunde)
stepper2.setAcceleration(1000); // Setzt die maximale Beschleunigung auf 1000 (Schritte pro Sekunde^2)
calibrateSetpoint();
lastTime = millis(); // Speichert die aktuelle Zeit in Millisekunden
}
void calibrateSetpoint() {
float sumAngles = 0;
const int numSamples = 1000;
for (int i = 0; i < numSamples; i++) {
unsigned long currentTime = millis(); // Definiert eine Variabel in Millisekunden
float deltaTime = (currentTime - lastTime) /1000.0; // Berechnet die Zeitdifferenz zwischen zwei Loopdurchläufen -> deltaTime
lastTime = currentTime;
int16_t ax, ay, az;
int16_t gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float angleY = atan2(ax, az) * 180 / PI;
float gyroRateY = gy / 131.0;
static float angle = 0;
angle = (0.98 * (angle + gyroRateY * deltaTime) + (1 - 0.98) * angleY);
sumAngles += angle;
delay(1); // Optional: etwas Verzögerung zwischen den Messungen
}
setpoint = 1 + (sumAngles / numSamples);
Serial.print("Calibrated Setpoint: ");
Serial.println(setpoint);
}
void loop() {
unsigned long currentTime = millis(); // Definiert eine Variabel in Millisekunden
float deltaTime = (currentTime - lastTime) /1000.0; // Berechnet die Zeitdifferenz zwischen zwei Loopdurchläufen -> deltaTime
lastTime = currentTime; // Setzt die Variabel lastTime auf den selben Wert wie die Variabel currentTime
int16_t ax, ay, az;
int16_t gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float angleY = atan2(ax, az) * 180 / PI;
float gyroRateY = gy / 131.0;
const float alpha = 0.98;
static float angle = 0;
angle = (alpha * (angle + gyroRateY * deltaTime) + (1 - alpha) * angleY);
input = angle;
error = setpoint - input; // Unterschied zwischen Sollwert und aktuellem Wert
integral += error * deltaTime; // Die Summe aller vergangenen Fehler, gewichtet mit der Zeit, um den kumulierten Fehler zu berücksichtigen.
float derivative = (error - lastError) / deltaTime; // Die Rate der Änderung des Fehlers, um die Reaktion auf schnelle Änderungen zu berücksichtigen.
output = (Kp * error + Ki * integral + Kd * derivative); // Das kombinierte Ergebnis des PID-Reglers, das als Steuersignal verwendet wird.
lastError = error; // Setzt den Lasterror auf den selben Wert wie Error für den nächsten Umlauf
float motorSpeed = constrain(output, -1800, 1800); // Begrenzt Output auf den Bereich -1000 bis 1000
if (abs(error) > 1.0) {
stepper1.setSpeed(motorSpeed);
stepper2.setSpeed(-motorSpeed);
stepper1.runSpeed();
stepper2.runSpeed();
} else {
stepper1.setSpeed(0);
stepper2.setSpeed(0);
}
Serial.print("Angle: "); // Für Debugging
Serial.print(input); // Für Debugging
Serial.print(" Output: "); // Für Debugging
Serial.println(output); // Für Debugging
Serial.print(" motorSpeed: ");
Serial.print(motorSpeed);
Serial.print(" error: ");
Serial.print(error);
}
Leider balanciert der Roboter nicht und ich weiss nicht so genau woran das liegt. Ebenso ist der Winkel oft 2-3 Grad neben dem Sollwert (Setpoint) welcher ja vorher basierend auf den aktuellen Winkeldaten definiert wird. Mit dem PID-Faktoren habe ich bereits viel ausgetestet, jedoch ohne wirklich Erfolg zu haben.
Vielen Dank für eure Hilfe und Tipps
LG
